diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index ef56be6e0d3bae8d8ec3f231601a43d230fa5ae8..c45ae846bb7799b0c5e7a0c9c0598da6eac4141f 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -798,6 +798,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; @@ -1293,6 +1296,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<double>(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<double>::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 7ebaafdde81902dd735e219efa2f8f8c5296a559..3e1b8cab3c5d848e9d43afce34920b2cc8c46082 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); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index b32119d96cd4287c558c953b062a8cfef1dcd3f1..e897543904df74a8a3bc7af0b6bce7f5048dc200 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -325,6 +325,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) emit vehicleHeartbeatInfo(link, _message.sysid, _message.compid, heartbeat.autopilot, heartbeat.type); } + if (_message.msgid == MAVLINK_MSG_ID_HIGH_LATENCY) { + _startLogging(); + mavlink_high_latency_t highLatency; + mavlink_msg_high_latency_decode(&_message, &highLatency); + // HIGH_LATENCY does not provide autopilot or type information, generic is our safest bet + emit vehicleHeartbeatInfo(link, _message.sysid, _message.compid, MAV_AUTOPILOT_GENERIC, MAV_TYPE_GENERIC); + } + if (_message.msgid == MAVLINK_MSG_ID_HIGH_LATENCY2) { _startLogging(); mavlink_high_latency2_t highLatency2;