diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 7dcf2be84a6d14593c66a5e461254e8035718894..94c0545cffedddf3b72a09de737b77f2b304806f 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/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); } } diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 9b620f078f228f9da4fe425d73a53023afcb7600..69a153ccd244ac33a72e05c37101f24fd40fd9c3 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/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; diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc index 25b2e712b788201465a5b80233af43781e01f25f..a77f5e50b541cb5682c3c070e8dc674bdc1b11df 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc +++ b/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); diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h index 0abfad2f9923d2eec24f0a640ade6069e28f44ab..1d69bfbfb854760a37e0b16f8c5d874a7f60819f 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h +++ b/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;