From f380ac520f33b042bfc1ed04d8b462a0da199e3c Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Wed, 7 Oct 2015 21:30:51 -0700
Subject: [PATCH] Add sendMultipleMessage, requestDataStream

---
 src/Vehicle/Vehicle.cc | 52 ++++++++++++++++++++++++++++++++++++++++++
 src/Vehicle/Vehicle.h  | 27 +++++++++++++++++++++-
 2 files changed, 78 insertions(+), 1 deletion(-)

diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index e31895b98..17c308c4f 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/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);
+}
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 97e4649f8..84145e250 100644
--- a/src/Vehicle/Vehicle.h
+++ b/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;
-- 
2.22.0