AirMapTelemetry.cc 5.28 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9 10
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "AirMapTelemetry.h"
Gus Grubba's avatar
Gus Grubba committed
11 12 13
#include "AirMapManager.h"

#include "QGCMAVLink.h"
14

Gus Grubba's avatar
Gus Grubba committed
15 16 17 18 19
#include "airmap/telemetry.h"
#include "airmap/flights.h"

using namespace airmap;

20
//-----------------------------------------------------------------------------
21
AirMapTelemetry::AirMapTelemetry(AirMapSharedState& shared)
22
    : _shared(shared)
23 24 25
{
}

26 27 28
//-----------------------------------------------------------------------------
void
AirMapTelemetry::vehicleMessageReceived(const mavlink_message_t& message)
29 30 31 32 33 34 35 36 37 38 39
{
    switch (message.msgid) {
    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
        _handleGlobalPositionInt(message);
        break;
    case MAVLINK_MSG_ID_GPS_RAW_INT:
        _handleGPSRawInt(message);
        break;
    }
}

40 41 42
//-----------------------------------------------------------------------------
bool
AirMapTelemetry::isTelemetryStreaming()
43 44 45 46
{
    return _state == State::Streaming;
}

47 48 49
//-----------------------------------------------------------------------------
void
AirMapTelemetry::_handleGPSRawInt(const mavlink_message_t& message)
50 51 52 53 54 55 56 57 58 59 60 61 62
{
    if (!isTelemetryStreaming()) {
        return;
    }
    mavlink_gps_raw_int_t gps_raw;
    mavlink_msg_gps_raw_int_decode(&message, &gps_raw);
    if (gps_raw.eph == UINT16_MAX) {
        _lastHdop = 1.f;
    } else {
        _lastHdop = gps_raw.eph / 100.f;
    }
}

63 64 65
//-----------------------------------------------------------------------------
void
AirMapTelemetry::_handleGlobalPositionInt(const mavlink_message_t& message)
66 67 68 69
{
    if (!isTelemetryStreaming()) {
        return;
    }
70 71 72 73 74 75
    // rate-limit updates to 5 Hz
    if (!_timerLastSent.hasExpired(200)) {
        return;
    }
    _timerLastSent.restart();

76 77 78 79
    mavlink_global_position_int_t globalPosition;
    mavlink_msg_global_position_int_decode(&message, &globalPosition);
    Telemetry::Position position{
        milliseconds_since_epoch(Clock::universal_time()),
Gus Grubba's avatar
Gus Grubba committed
80 81 82 83 84
        static_cast<double>(globalPosition.lat / 1e7),
        static_cast<double>(globalPosition.lon / 1e7),
        static_cast<double>(globalPosition.alt) / 1000.0,
        static_cast<double>(globalPosition.relative_alt) / 1000.0,
        static_cast<double>(_lastHdop)
85 86 87 88 89 90 91 92
    };
    Telemetry::Speed speed{
        milliseconds_since_epoch(Clock::universal_time()),
        globalPosition.vx / 100.f,
        globalPosition.vy / 100.f,
        globalPosition.vz / 100.f
    };

93
    //qCDebug(AirMapManagerLog) << "Telemetry:" << globalPosition.lat / 1e7 << globalPosition.lon / 1e7;
94 95 96 97 98 99
    Flight flight;
    flight.id = _flightID.toStdString();
    _shared.client()->telemetry().submit_updates(flight, _key,
        {Telemetry::Update{position}, Telemetry::Update{speed}});
}

100 101 102
//-----------------------------------------------------------------------------
void
AirMapTelemetry::startTelemetryStream(const QString& flightID)
103 104
{
    if (_state != State::Idle) {
Gus Grubba's avatar
Gus Grubba committed
105
        qCWarning(AirMapManagerLog) << "Not starting telemetry: not in idle state:" << static_cast<int>(_state);
106 107
        return;
    }
108 109 110 111
    if(flightID.isEmpty()) {
        qCWarning(AirMapManagerLog) << "Not starting telemetry: No flight ID.";
        return;
    }
112
    qCInfo(AirMapManagerLog) << "Starting Telemetry stream with flightID" << flightID;
113 114
    _state      = State::StartCommunication;
    _flightID   = flightID;
115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131
    Flights::StartFlightCommunications::Parameters params;
    params.authorization = _shared.loginToken().toStdString();
    params.id = _flightID.toStdString();
    std::weak_ptr<LifetimeChecker> isAlive(_instance);
    _shared.client()->flights().start_flight_communications(params, [this, isAlive](const Flights::StartFlightCommunications::Result& result) {
        if (!isAlive.lock()) return;
        if (_state != State::StartCommunication) return;
        if (result) {
            _key = result.value().key;
            _state = State::Streaming;
        } else {
            _state = State::Idle;
            QString description = QString::fromStdString(result.error().description() ? result.error().description().get() : "");
            emit error("Failed to start telemetry streaming",
                    QString::fromStdString(result.error().message()), description);
        }
    });
132
    _timerLastSent.start();
133 134
}

135 136 137
//-----------------------------------------------------------------------------
void
AirMapTelemetry::stopTelemetryStream()
138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156
{
    if (_state == State::Idle) {
        return;
    }
    qCInfo(AirMapManagerLog) << "Stopping Telemetry stream with flightID" << _flightID;
    _state = State::EndCommunication;
    Flights::EndFlightCommunications::Parameters params;
    params.authorization = _shared.loginToken().toStdString();
    params.id = _flightID.toStdString();
    std::weak_ptr<LifetimeChecker> isAlive(_instance);
    _shared.client()->flights().end_flight_communications(params, [this, isAlive](const Flights::EndFlightCommunications::Result& result) {
        Q_UNUSED(result);
        if (!isAlive.lock()) return;
        if (_state != State::EndCommunication) return;
        _key = "";
        _state = State::Idle;
    });
}