APM stack must request data streams on connect

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);
+}

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -37,7 +37,6 @@ class APMFirmwarePlugin : public FirmwarePlugin
     
 public:
     // Overrides from FirmwarePlugin
-    
     virtual bool isCapable(FirmwareCapabilities capabilities);
     virtual QList<VehicleComponent*> 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);

src/FirmwarePlugin/FirmwarePlugin.h

@@ -35,6 +35,8 @@
 #include <QList>
 #include <QString>
 
+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) { }
 };

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
+}

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

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
+}

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