Restart ArduPilot streams as needed

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -309,12 +309,17 @@ void APMFirmwarePlugin::_handleIncomingHeartbeat(Vehicle* vehicle, mavlink_messa
 
 bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
 {
+    // We use loss of BATTERY_STATUS/HOME_POSITION as a trigger to reinitialize stream rates
+    auto instanceData = qobject_cast<APMFirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData());
+
     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;
+    } else if (message->msgid == MAVLINK_MSG_ID_BATTERY_STATUS && instanceData)  {
-    }
+        instanceData->lastBatteryStatusTime = QTime::currentTime();
-
+    } else if (message->msgid == MAVLINK_MSG_ID_HOME_POSITION && instanceData)  {
+        instanceData->lastHomePositionTime = QTime::currentTime();
+    } else {
         // 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) {
@@ -331,6 +336,13 @@ bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_m
                 break;
             }
         }
+    }
+
+    // If we lose BATTERY_STATUS/HOME_POSITION for reinitStreamsTimeoutSecs seconds we re-initialize stream rates
+    const int reinitStreamsTimeoutSecs = 10;
+    if (instanceData && (instanceData->lastBatteryStatusTime.secsTo(QTime::currentTime()) > reinitStreamsTimeoutSecs || instanceData->lastHomePositionTime.secsTo(QTime::currentTime()) > reinitStreamsTimeoutSecs)) {
+        initializeStreamRates(vehicle);
+    }
 
     return true;
 }
@@ -400,6 +412,16 @@ void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* mes
 
 void APMFirmwarePlugin::initializeStreamRates(Vehicle* vehicle)
 {
+    // We use loss of BATTERY_STATUS/HOME_POSITION as a trigger to reinitialize stream rates
+    auto instanceData = qobject_cast<APMFirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData());
+    if (!instanceData) {
+        instanceData = new APMFirmwarePluginInstanceData(vehicle);
+        instanceData->lastBatteryStatusTime = instanceData->lastHomePositionTime = QTime::currentTime();
+        vehicle->setFirmwarePluginInstanceData(instanceData);
+    }
+
+    if (qgcApp()->toolbox()->settingsManager()->appSettings()->apmStartMavlinkStreams()->rawValue().toBool()) {
+
         APMMavlinkStreamRateSettings* streamRates = qgcApp()->toolbox()->settingsManager()->apmMavlinkStreamRateSettings();
 
         struct StreamInfo_s {
@@ -424,6 +446,7 @@ void APMFirmwarePlugin::initializeStreamRates(Vehicle* vehicle)
                 vehicle->requestDataStream(streamInfo.mavStream, static_cast<uint16_t>(streamInfo.streamRate));
             }
         }
+    }
 
     // ArduPilot only sends home position on first boot and then when it arms. It does not stream the position.
     // This means that depending on when QGC connects to the vehicle it may not have home position.
@@ -431,6 +454,8 @@ void APMFirmwarePlugin::initializeStreamRates(Vehicle* vehicle)
     // The MAV_CMD_SET_MESSAGE_INTERVAL command is only supported on newer firmwares. So we set showError=false.
     // Which also means than on older firmwares you may be left with some missing features.
     vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_SET_MESSAGE_INTERVAL, false /* showError */, MAVLINK_MSG_ID_HOME_POSITION, 1000000 /* 1 second interval in usec */);
+
+    instanceData->lastBatteryStatusTime = instanceData->lastHomePositionTime = QTime::currentTime();
 }
 
 
@@ -468,11 +493,8 @@ void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
             break;
         }
     } else {
-        if (qgcApp()->toolbox()->settingsManager()->appSettings()->apmStartMavlinkStreams()->rawValue().toBool()) {
-            // Streams are not started automatically on APM stack (sort of)
         initializeStreamRates(vehicle);
     }
-    }
 
     if (qgcApp()->toolbox()->settingsManager()->videoSettings()->videoSource()->rawValue() == VideoSettings::videoSource3DRSolo) {
         _soloVideoHandshake();

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -7,12 +7,7 @@
  *
  ****************************************************************************/
 
-
+#pragma once
-/// @file
-///     @author Don Gagne <don@thegagnes.com>
-
-#ifndef APMFirmwarePlugin_H
-#define APMFirmwarePlugin_H
 
 #include "FirmwarePlugin.h"
 #include "QGCLoggingCategory.h"
@@ -125,4 +120,17 @@ private:
     static QMutex&          _reencodeMavlinkChannelMutex();
 };
 
-#endif
+class APMFirmwarePluginInstanceData : public QObject
+{
+    Q_OBJECT
+
+public:
+    APMFirmwarePluginInstanceData(QObject* parent = nullptr)
+        : QObject(parent)
+    {
+
+    }
+
+    QTime lastBatteryStatusTime;
+    QTime lastHomePositionTime;
+};