From d4b9ff9fd178d966ed29a3d9e0975e4b50f2b28b Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 29 Nov 2018 21:28:28 -0800 Subject: [PATCH] Revert "ArduPilot: Don't starts streams from QGC side. Assume firmware side does the right thing" --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 15 +++++++++++++++ src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 1 + src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc | 10 ++++++++++ src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h | 2 ++ 4 files changed, 28 insertions(+) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 7dcf2be84a..94c0545cff 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 9b620f078f..69a153ccd2 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 25b2e712b7..a77f5e50b5 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 0abfad2f99..1d69bfbfb8 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; -- GitLab