Commit 47d0b9f7 authored by Patrick José Pereira's avatar Patrick José Pereira

MAVLinkProtocol: Add handle for HIGH_LATENCY

Signed-off-by: 's avatarPatrick José Pereira <patrickelectric@gmail.com>
parent c7e2fa5c
...@@ -325,6 +325,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) ...@@ -325,6 +325,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
emit vehicleHeartbeatInfo(link, _message.sysid, _message.compid, heartbeat.autopilot, heartbeat.type); 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) { if (_message.msgid == MAVLINK_MSG_ID_HIGH_LATENCY2) {
_startLogging(); _startLogging();
mavlink_high_latency2_t highLatency2; mavlink_high_latency2_t highLatency2;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment