Merge pull request #1927 from DonLakeFlyer/APMDataStream

Apm Stack data stream request

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

src/Vehicle/Vehicle.cc

@@ -89,6 +89,7 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType)
     , _armed(false)
     , _base_mode(0)
     , _custom_mode(0)
+    , _nextSendMessageMultipleIndex(0)
 {
     _addLink(link);
     
@@ -161,6 +162,11 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType)
     if (qgcApp()->useNewMissionEditor()) {
         _missionManager = new MissionManager(this);
     }
+    
+    _firmwarePlugin->initializeVehicle(this);
+    
+    _sendMultipleTimer.start(_sendMessageMultipleIntraMessageDelay);
+    connect(&_sendMultipleTimer, &QTimer::timeout, this, &Vehicle::_sendMessageMultipleNext);
 }
 
 Vehicle::~Vehicle()
@@ -1069,3 +1075,49 @@ bool Vehicle::missingParameters(void)
 {
     return _autopilotPlugin->missingParameters();
 }
+
+void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate)
+{
+    mavlink_message_t               msg;
+    mavlink_request_data_stream_t   dataStream;
+    
+    dataStream.req_stream_id = stream;
+    dataStream.req_message_rate = rate;
+    dataStream.start_stop = 1;  // start
+    dataStream.target_system = id();
+    dataStream.target_component = 0;
+    
+    mavlink_msg_request_data_stream_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &dataStream);
+
+    // We use sendMessageMultiple since we really want these to make it to the vehicle
+    sendMessageMultiple(msg);
+}
+
+void Vehicle::_sendMessageMultipleNext(void)
+{
+    if (_nextSendMessageMultipleIndex < _sendMessageMultipleList.count()) {
+        qCDebug(VehicleLog) << "_sendMessageMultipleNext:" << _sendMessageMultipleList[_nextSendMessageMultipleIndex].message.msgid;
+        
+        sendMessage(_sendMessageMultipleList[_nextSendMessageMultipleIndex].message);
+        
+        if (--_sendMessageMultipleList[_nextSendMessageMultipleIndex].retryCount <= 0) {
+            _sendMessageMultipleList.removeAt(_nextSendMessageMultipleIndex);
+        } else {
+            _nextSendMessageMultipleIndex++;
+        }
+    }
+    
+    if (_nextSendMessageMultipleIndex >= _sendMessageMultipleList.count()) {
+        _nextSendMessageMultipleIndex = 0;
+    }
+}
+
+void Vehicle::sendMessageMultiple(mavlink_message_t message)
+{
+    SendMessageMultipleInfo_t   info;
+    
+    info.message =      message;
+    info.retryCount =   _sendMessageMultipleRetries;
+    
+    _sendMessageMultipleList.append(info);
+}

src/Vehicle/Vehicle.h

@@ -151,6 +151,10 @@ public:
     /// Sends this specified message to all links accociated with this vehicle
     void sendMessage(mavlink_message_t message);
     
+    /// Sends the specified messages multiple times to the vehicle in order to attempt to
+    /// guarantee that it makes it to the vehicle.
+    void sendMessageMultiple(mavlink_message_t message);
+    
     /// Provides access to uas from vehicle. Temporary workaround until UAS is fully phased out.
     UAS* uas(void) { return _uas; }
     
@@ -180,6 +184,11 @@ public:
     bool hilMode(void);
     void setHilMode(bool hilMode);
     
+    /// Requests the specified data stream from the vehicle
+    ///     @param stream Stream which is being requested
+    ///     @param rate Rate at which to send stream in Hz
+    void requestDataStream(MAV_DATA_STREAM stream, uint16_t rate);
+    
     bool missingParameters(void);
     
     typedef enum {
@@ -289,7 +298,8 @@ private slots:
     void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
     void _linkDisconnected(LinkInterface* link);
     void _sendMessage(mavlink_message_t message);
-    
+    void _sendMessageMultipleNext(void);
+
     void _handleTextMessage                 (int newCount);
     /** @brief Attitude from main autopilot / system state */
     void _updateAttitude                    (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
@@ -397,7 +407,22 @@ private:
     bool    _armed;         ///< true: vehicle is armed
     uint8_t _base_mode;     ///< base_mode from HEARTBEAT
     uint32_t _custom_mode;  ///< custom_mode from HEARTBEAT
+
+    /// Used to store a message being sent by sendMessageMultiple
+    typedef struct {
+        mavlink_message_t   message;    ///< Message to send multiple times
+        int                 retryCount; ///< Number of retries left
+    } SendMessageMultipleInfo_t;
+    
+    QList<SendMessageMultipleInfo_t> _sendMessageMultipleList;    ///< List of messages being sent multiple times
+    
+    static const int _sendMessageMultipleRetries = 5;
+    static const int _sendMessageMultipleIntraMessageDelay = 500;
+    
+    QTimer  _sendMultipleTimer;
+    int     _nextSendMessageMultipleIndex;
     
+    // Settings keys
     static const char* _settingsGroup;
     static const char* _joystickModeSettingsKey;
     static const char* _joystickEnabledSettingsKey;