Merge pull request #6401 from bluerobotics/sub-telem-rate

APM Sub: default attitude streamrate to 20Hz

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -595,6 +595,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);
@@ -632,13 +644,7 @@ void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
         }
     } else {
         // Streams are not started automatically on APM stack
-        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);
+        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

@@ -164,6 +164,17 @@ 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::supportsThrottleModeCenterZero(void)
 {
     return false;

src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h

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