diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 2a7d1b493fbdd53b16463c6b2d54f507dd17acf6..32f6ebb49305cc2fcb6044ddb3e15128d40c2cae 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -801,6 +801,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_ADSB_VEHICLE: _handleADSBVehicle(message); break; + case MAVLINK_MSG_ID_HIGH_LATENCY: + _handleHighLatency(message); + break; case MAVLINK_MSG_ID_HIGH_LATENCY2: _handleHighLatency2(message); break; @@ -1296,6 +1299,63 @@ void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message) } } +void Vehicle::_handleHighLatency(mavlink_message_t& message) +{ + mavlink_high_latency_t highLatency; + mavlink_msg_high_latency_decode(&message, &highLatency); + + QString previousFlightMode; + if (_base_mode != 0 || _custom_mode != 0){ + // Vehicle is initialized with _base_mode=0 and _custom_mode=0. Don't pass this to flightMode() since it will complain about + // bad modes while unit testing. + previousFlightMode = flightMode(); + } + _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + _custom_mode = _firmwarePlugin->highLatencyCustomModeTo32Bits(highLatency.custom_mode); + if (previousFlightMode != flightMode()) { + emit flightModeChanged(flightMode()); + } + + // Assume armed since we don't know + if (_armed != true) { + _armed = true; + emit armedChanged(_armed); + } + + struct { + const double latitude; + const double longitude; + const double altitude; + } coordinate { + highLatency.latitude / (double)1E7, + highLatency.longitude / (double)1E7, + static_cast(highLatency.altitude_amsl) + }; + + _coordinate.setLatitude(coordinate.latitude); + _coordinate.setLongitude(coordinate.longitude); + _coordinate.setAltitude(coordinate.altitude); + emit coordinateChanged(_coordinate); + + _airSpeedFact.setRawValue((double)highLatency.airspeed / 5.0); + _groundSpeedFact.setRawValue((double)highLatency.groundspeed / 5.0); + _climbRateFact.setRawValue((double)highLatency.climb_rate / 10.0); + _headingFact.setRawValue((double)highLatency.heading * 2.0); + _altitudeRelativeFact.setRawValue(std::numeric_limits::quiet_NaN()); + _altitudeAMSLFact.setRawValue(coordinate.altitude); + + _windFactGroup.speed()->setRawValue((double)highLatency.airspeed / 5.0); + + _battery1FactGroup.percentRemaining()->setRawValue(highLatency.battery_remaining); + + _temperatureFactGroup.temperature1()->setRawValue(highLatency.temperature_air); + + _gpsFactGroup.lat()->setRawValue(coordinate.latitude); + _gpsFactGroup.lon()->setRawValue(coordinate.longitude); + _gpsFactGroup.mgrs()->setRawValue(convertGeoToMGRS(QGeoCoordinate(coordinate.latitude, coordinate.longitude))); + _gpsFactGroup.count()->setRawValue(0); +} + void Vehicle::_handleHighLatency2(mavlink_message_t& message) { mavlink_high_latency2_t highLatency2; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 1229c67daacf0d276b2a18e5e600e1775e67ca5d..2ad40a51fc0d76614e19e70c5e0a1885ea16f177 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -1331,6 +1331,7 @@ private: void _handleScaledPressure (mavlink_message_t& message); void _handleScaledPressure2 (mavlink_message_t& message); void _handleScaledPressure3 (mavlink_message_t& message); + void _handleHighLatency (mavlink_message_t& message); void _handleHighLatency2 (mavlink_message_t& message); void _handleAttitudeWorker (double rollRadians, double pitchRadians, double yawRadians); void _handleAttitude (mavlink_message_t& message);