src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -446,29 +446,40 @@ void APMFirmwarePlugin::_handleIncomingHeartbeat(Vehicle* vehicle, mavlink_messa
 {
     bool flying = false;
 
-    // We pull Vehicle::flying state from HEARTBEAT on ArduPilot. This is a firmware specific test.
-
-    if (vehicle->armed()) {
     mavlink_heartbeat_t heartbeat;
     mavlink_msg_heartbeat_decode(message, &heartbeat);
 
+    if (message->compid == MAV_COMP_ID_AUTOPILOT1) {
+        // We pull Vehicle::flying state from HEARTBEAT on ArduPilot. This is a firmware specific test.
+        if (vehicle->armed()) {
+
             flying = heartbeat.system_status == MAV_STATE_ACTIVE;
             if (!flying && vehicle->flying()) {
                 // If we were previously flying, and we go into critical or emergency assume we are still flying
                 flying = heartbeat.system_status == MAV_STATE_CRITICAL || heartbeat.system_status == MAV_STATE_EMERGENCY;
             }
         }
-
         vehicle->_setFlying(flying);
+    }
+
+    // We need to know whether this component is part of the ArduPilot stack code or not so we can adjust mavlink quirks appropriately.
+    // If the component sends a heartbeat we can know that. If it's doesn't there is pretty much no way to know.
+    _ardupilotComponentMap[message->sysid][message->compid] = heartbeat.autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA;
+
+    // Force the ESP8266 to be non-ArduPilot code (it doesn't send heartbeats)
+    _ardupilotComponentMap[message->sysid][MAV_COMP_ID_UDP_BRIDGE] = false;
 }
 
 bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
 {
-    // Only translate messages which come from the autopilot. All other components are expected to follow current mavlink spec.
-    if (message->compid != MAV_COMP_ID_AUTOPILOT1) {
+    if (message->msgid == MAVLINK_MSG_ID_HEARTBEAT) {
+        // We need to look at all heartbeats that go by from any component
+        _handleIncomingHeartbeat(vehicle, message);
         return true;
     }
 
+    // Only translate messages which come from ArduPilot code. All other components are expected to follow current mavlink spec.
+    if (_ardupilotComponentMap[vehicle->id()][message->compid]) {
         switch (message->msgid) {
         case MAVLINK_MSG_ID_PARAM_VALUE:
             _handleIncomingParamValue(vehicle, message);
@@ -477,9 +488,6 @@ bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_m
             return _handleIncomingStatusText(vehicle, message, false /* longVersion */);
         case MAVLINK_MSG_ID_STATUSTEXT_LONG:
             return _handleIncomingStatusText(vehicle, message, true /* longVersion */);
-    case MAVLINK_MSG_ID_HEARTBEAT:
-        _handleIncomingHeartbeat(vehicle, message);
-        break;
         case MAVLINK_MSG_ID_RC_CHANNELS:
             _handleRCChannels(vehicle, message);
             break;
@@ -487,22 +495,21 @@ bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_m
             _handleRCChannelsRaw(vehicle, message);
             break;
         }
+    }
 
     return true;
 }
 
 void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message)
 {
-    //-- Don't process messages to/from UDP Bridge. It doesn't suffer from these issues
-    if (message->compid == MAV_COMP_ID_UDP_BRIDGE) {
-        return;
-    }
-
+    // Only translate messages which come from ArduPilot code. All other components are expected to follow current mavlink spec.
+    if (_ardupilotComponentMap[vehicle->id()][message->compid]) {
         switch (message->msgid) {
         case MAVLINK_MSG_ID_PARAM_SET:
             _handleOutgoingParamSet(vehicle, outgoingLink, message);
             break;
         }
+    }
 }
 
 QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message, bool longVersion) const

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -137,6 +137,7 @@ private:
     // Vehicle specific data should go into APMFirmwarePluginInstanceData
 
     QList<APMCustomMode>    _supportedModes;
+    QMap<int /* vehicle id */, QMap<int /* componentId */, bool /* true: component is part of ArduPilot stack */>> _ardupilotComponentMap;
 
     static const char*      _artooIP;
     static const int        _artooVideoHandshakePort;