QGC.cc 2.19 KB
Newer Older
pixhawk's avatar
pixhawk committed
1
2
3
4
/*=====================================================================

QGroundControl Open Source Ground Control Station

lm's avatar
lm committed
5
(c) 2009 - 2011 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
pixhawk's avatar
pixhawk committed
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    QGROUNDCONTROL is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

pixhawk's avatar
pixhawk committed
24
#include "QGC.h"
lm's avatar
lm committed
25
#include <qmath.h>
26
#include <float.h>
lm's avatar
lm committed
27

28
29
namespace QGC
{
pixhawk's avatar
pixhawk committed
30
31
32

quint64 groundTimeUsecs()
{
33
    return groundTimeMilliseconds() * 1000;
pixhawk's avatar
pixhawk committed
34
35
}

lm's avatar
lm committed
36
37
quint64 groundTimeMilliseconds()
{
38
    return static_cast<quint64>(QDateTime::currentMSecsSinceEpoch());
lm's avatar
lm committed
39
40
}

41
42
qreal groundTimeSeconds()
{
43
    return static_cast<qreal>(groundTimeMilliseconds()) / 1000.0f;
44
45
}

46
47
float limitAngleToPMPIf(float angle)
{
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
    if (angle > -20*M_PI && angle < 20*M_PI)
    {
        while (angle > ((float)M_PI+FLT_EPSILON))
        {
            angle -= 2.0f * (float)M_PI;
        }

        while (angle <= -((float)M_PI+FLT_EPSILON))
        {
            angle += 2.0f * (float)M_PI;
        }
    }
    else
    {
        // Approximate
Lorenz Meier's avatar
Lorenz Meier committed
63
        angle = fmodf(angle, (float)M_PI);
64
    }
65
66
67
68
69

    return angle;
}

double limitAngleToPMPId(double angle)
lm's avatar
lm committed
70
{
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
    if (angle > -20*M_PI && angle < 20*M_PI)
    {
        if (angle < -M_PI)
        {
            while (angle < -M_PI)
            {
                angle += M_PI;
            }
        }
        else if (angle > M_PI)
        {
            while (angle > M_PI)
            {
                angle -= M_PI;
            }
        }
    }
    else
    {
        // Approximate
        angle = fmod(angle, M_PI);
    }

lm's avatar
lm committed
94
95
96
    return angle;
}

pixhawk's avatar
pixhawk committed
97
}