From bd304b2c69d4f4a8e29b36a2e45fa21888bc89f2 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 7 Oct 2015 21:31:06 -0700 Subject: [PATCH] APM stack must request data streams on connect --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 12 ++++++++++++ src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 4 ++-- src/FirmwarePlugin/FirmwarePlugin.h | 5 +++++ src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc | 7 +++++++ src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h | 1 + src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 7 +++++++ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 1 + 7 files changed, 35 insertions(+), 2 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 3341c6b9c2..898e2c5448 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -169,3 +169,15 @@ void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) // FIXME: Need to implement mavlink message severity adjustment } + +void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle) +{ + // 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); +} diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 524a36f385..6e798089bb 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -37,7 +37,6 @@ class APMFirmwarePlugin : public FirmwarePlugin public: // Overrides from FirmwarePlugin - virtual bool isCapable(FirmwareCapabilities capabilities); virtual QList componentsForVehicle(AutoPilotPlugin* vehicle); virtual QStringList flightModes(void); @@ -45,7 +44,8 @@ public: virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); virtual int manualControlReservedButtonCount(void); virtual void adjustMavlinkMessage(mavlink_message_t* message); - + virtual void initializeVehicle(Vehicle* vehicle); + private: /// All access to singleton is through AutoPilotPluginManager::instance APMFirmwarePlugin(QObject* parent = NULL); diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index c84a8ece94..3783c5a8db 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -35,6 +35,8 @@ #include #include +class Vehicle; + /// This is the base class for Firmware specific plugins /// /// The FirmwarePlugin class is the abstract base class which represents the methods and objects @@ -92,6 +94,9 @@ public: /// @param message[in,out] Mavlink message to adjust if needed. virtual void adjustMavlinkMessage(mavlink_message_t* message) = 0; + /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware. + virtual void initializeVehicle(Vehicle* vehicle) = 0; + protected: FirmwarePlugin(QObject* parent = NULL) : QGCSingleton(parent) { } }; diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc index 5b11fdf1c3..910eba2f8e 100644 --- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc +++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc @@ -103,3 +103,10 @@ void GenericFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) // Generic plugin does no message adjustment } + +void GenericFirmwarePlugin::initializeVehicle(Vehicle* vehicle) +{ + Q_UNUSED(vehicle); + + // Generic Flight Stack is by definition "generic", so no extra work +} diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h index 505a3de3df..7b2634e4f6 100644 --- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h +++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h @@ -45,6 +45,7 @@ public: virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); virtual int manualControlReservedButtonCount(void); virtual void adjustMavlinkMessage(mavlink_message_t* message); + virtual void initializeVehicle(Vehicle* vehicle); private: /// All access to singleton is through AutoPilotPluginManager::instance diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 731f750567..2a2de268b7 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -193,3 +193,10 @@ bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities) { return (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability)) == capabilities; } + +void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle) +{ + Q_UNUSED(vehicle); + + // PX4 Flight Stack doesn't need to do any extra work +} diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 953842e595..f1400622b1 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -45,6 +45,7 @@ public: virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); virtual int manualControlReservedButtonCount(void); virtual void adjustMavlinkMessage(mavlink_message_t* message); + virtual void initializeVehicle(Vehicle* vehicle); private: /// All access to singleton is through AutoPilotPluginManager::instance -- GitLab