diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 7dcf2be..94c0545 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 9b620f0..69a153c 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 25b2e71..a77f5e5 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 0abfad2..1d69bfb 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;