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

QGroundControl Open Source Ground Control Station

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 25
#include "QGC.h"

lm's avatar
lm committed
26
#include <qmath.h>
27
#include <float.h>
lm's avatar
lm committed
28

29 30
namespace QGC
{
pixhawk's avatar
pixhawk committed
31 32 33 34 35 36 37 38 39 40

quint64 groundTimeUsecs()
{
    QDateTime time = QDateTime::currentDateTime();
    time = time.toUTC();
    /* Return seconds and milliseconds, in milliseconds unit */
    quint64 microseconds = time.toTime_t() * static_cast<quint64>(1000000);
    return static_cast<quint64>(microseconds + (time.time().msec()*1000));
}

lm's avatar
lm committed
41 42 43 44 45 46 47 48 49
quint64 groundTimeMilliseconds()
{
    QDateTime time = QDateTime::currentDateTime();
    time = time.toUTC();
    /* Return seconds and milliseconds, in milliseconds unit */
    quint64 seconds = time.toTime_t() * static_cast<quint64>(1000);
    return static_cast<quint64>(seconds + (time.time().msec()));
}

50 51 52 53 54 55 56 57 58
qreal groundTimeSeconds()
{
    QDateTime time = QDateTime::currentDateTime();
    time = time.toUTC();
    /* Return time in seconds unit */
    quint64 seconds = time.toTime_t();
    return static_cast<qreal>(seconds + (time.time().msec() / 1000.0));
}

59 60
float limitAngleToPMPIf(float angle)
{
61 62 63 64 65 66 67 68 69 70 71 72 73 74 75
    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
76
        angle = fmodf(angle, (float) M_PI);
77
    }
78 79 80 81 82

    return angle;
}

double limitAngleToPMPId(double angle)
lm's avatar
lm committed
83
{
84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106
    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
107 108 109
    return angle;
}

110 111 112 113 114
int applicationVersion()
{
    return APPLICATIONVERSION;
}

pixhawk's avatar
pixhawk committed
115
}