ADSBVehicle.cc 2.8 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "ADSBVehicle.h"

#include <QDebug>
#include <QtMath>

ADSBVehicle::ADSBVehicle(mavlink_adsb_vehicle_t& adsbVehicle, QObject* parent)
    : QObject       (parent)
    , _icaoAddress  (adsbVehicle.ICAO_address)
18
    , _callsign     (adsbVehicle.callsign)
19 20
    , _altitude     (NAN)
    , _heading      (NAN)
Gus Grubba's avatar
Gus Grubba committed
21
    , _alert        (false)
22 23 24 25 26 27 28 29
{
    if (!(adsbVehicle.flags | ADSB_FLAGS_VALID_COORDS)) {
        qWarning() << "At least coords must be valid";
        return;
    }
    update(adsbVehicle);
}

Gus Grubba's avatar
Gus Grubba committed
30 31 32 33
ADSBVehicle::ADSBVehicle(const QGeoCoordinate& location, float heading, bool alert, QObject* parent)
    : QObject(parent)
    , _icaoAddress(0)
    , _alert(alert)
34
{
Gus Grubba's avatar
Gus Grubba committed
35
    update(alert, location, heading);
36 37
}

Gus Grubba's avatar
Gus Grubba committed
38
void ADSBVehicle::update(bool alert, const QGeoCoordinate& location, float heading)
39 40
{
    _coordinate = location;
Gus Grubba's avatar
Gus Grubba committed
41 42 43 44 45 46 47
    _altitude   = location.altitude();
    _heading    = heading;
    _alert      = alert;
    emit coordinateChanged();
    emit altitudeChanged();
    emit headingChanged();
    emit alertChanged();
48
    _lastUpdateTimer.restart();
49 50
}

51 52 53 54 55 56 57 58 59 60 61
void ADSBVehicle::update(mavlink_adsb_vehicle_t& adsbVehicle)
{
    if (_icaoAddress != adsbVehicle.ICAO_address) {
        qWarning() << "ICAO address mismatch expected:actual" << _icaoAddress << adsbVehicle.ICAO_address;
        return;
    }

    if (!(adsbVehicle.flags | ADSB_FLAGS_VALID_COORDS)) {
        return;
    }

62 63 64 65
    QString currCallsign(adsbVehicle.callsign);

    if (currCallsign != _callsign) {
        _callsign = currCallsign;
Gus Grubba's avatar
Gus Grubba committed
66
        emit callsignChanged();
67 68 69
    }

    QGeoCoordinate newCoordinate(adsbVehicle.lat / 1e7, adsbVehicle.lon / 1e7);
70 71
    if (newCoordinate != _coordinate) {
        _coordinate = newCoordinate;
Gus Grubba's avatar
Gus Grubba committed
72
        emit coordinateChanged();
73 74 75 76
    }

    double newAltitude = NAN;
    if (adsbVehicle.flags | ADSB_FLAGS_VALID_ALTITUDE) {
77
        newAltitude = (double)adsbVehicle.altitude / 1e3;
78 79 80
    }
    if (!(qIsNaN(newAltitude) && qIsNaN(_altitude)) && !qFuzzyCompare(newAltitude, _altitude)) {
        _altitude = newAltitude;
Gus Grubba's avatar
Gus Grubba committed
81
        emit altitudeChanged();
82 83 84 85 86 87 88 89
    }

    double newHeading = NAN;
    if (adsbVehicle.flags | ADSB_FLAGS_VALID_HEADING) {
        newHeading = (double)adsbVehicle.heading / 100.0;
    }
    if (!(qIsNaN(newHeading) && qIsNaN(_heading)) && !qFuzzyCompare(newHeading, _heading)) {
        _heading = newHeading;
Gus Grubba's avatar
Gus Grubba committed
90
        emit headingChanged();
91
    }
92 93 94 95 96 97
    _lastUpdateTimer.restart();
}

bool ADSBVehicle::expired()
{
    return _lastUpdateTimer.hasExpired(expirationTimeoutMs);
98
}