Revert "ArduPilot: Don't starts streams from QGC side. Assume firmware side does the right thing"

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -593,6 +593,18 @@ void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* mes
     mavlink_msg_statustext_encode_chan(message->sysid, message->compid, 0, message, &statusText);
 }
 
+void APMFirmwarePlugin::initializeStreamRates(Vehicle* vehicle)
+{
+    vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS,     2);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2);
+    vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS,     2);
+    vehicle->requestDataStream(MAV_DATA_STREAM_POSITION,        3);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1,          10);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2,          10);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3,          3);
+}
+
+
 void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
 {
     vehicle->setFirmwarePluginInstanceData(new APMFirmwarePluginInstanceData);
@@ -628,6 +640,9 @@ void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
             // No version set
             break;
         }
+    } else {
+        // Streams are not started automatically on APM stack
+        initializeStreamRates(vehicle);
     }
 }
 

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -92,6 +92,7 @@ public:
     int                 manualControlReservedButtonCount(void) override;
     bool                adjustIncomingMavlinkMessage    (Vehicle* vehicle, mavlink_message_t* message) override;
     void                adjustOutgoingMavlinkMessage    (Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) override;
+    virtual void        initializeStreamRates           (Vehicle* vehicle);
     void                initializeVehicle               (Vehicle* vehicle) override;
     bool                sendHomePositionToVehicle       (void) override;
     void                addMetaDataToFact               (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) override;

src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc

@@ -169,6 +169,16 @@ int ArduSubFirmwarePlugin::manualControlReservedButtonCount(void)
     return 0;
 }
 
+void ArduSubFirmwarePlugin::initializeStreamRates(Vehicle* vehicle) {
+    vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS,     2);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2);
+    vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS,     2);
+    vehicle->requestDataStream(MAV_DATA_STREAM_POSITION,        3);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1,          20);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2,          10);
+    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3,          3);
+}
+
 bool ArduSubFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
 {
     Q_UNUSED(vehicle);

src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h

@@ -115,6 +115,8 @@ public:
 
     int defaultJoystickTXMode(void) final { return 3; }
 
+    void initializeStreamRates(Vehicle* vehicle) override final;
+
     bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
 
     bool supportsThrottleModeCenterZero(void) final;