refactor Vehicle: move MAVLink stream rate overrides into separate class

qgroundcontrol.pro

@@ -687,6 +687,7 @@ HEADERS += \
     src/Vehicle/GPSRTKFactGroup.h \
     src/Vehicle/InitialConnectStateMachine.h \
     src/Vehicle/MAVLinkLogManager.h \
+    src/Vehicle/MAVLinkStreamConfig.h \
     src/Vehicle/MultiVehicleManager.h \
     src/Vehicle/StateMachine.h \
     src/Vehicle/SysStatusSensorInfo.h \
@@ -918,6 +919,7 @@ SOURCES += \
     src/Vehicle/GPSRTKFactGroup.cc \
     src/Vehicle/InitialConnectStateMachine.cc \
     src/Vehicle/MAVLinkLogManager.cc \
+    src/Vehicle/MAVLinkStreamConfig.cc \
     src/Vehicle/MultiVehicleManager.cc \
     src/Vehicle/StateMachine.cc \
     src/Vehicle/SysStatusSensorInfo.cc \

src/Vehicle/MAVLinkStreamConfig.cc

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ * (c) 2021 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "MAVLinkStreamConfig.h"
+#include "QGCMAVLink.h"
+
+MAVLinkStreamConfig::MAVLinkStreamConfig(const SetMessageIntervalCb &messageIntervalCb)
+    : _messageIntervalCb(messageIntervalCb)
+{
+}
+
+void MAVLinkStreamConfig::setHighRateRateAndAttitude()
+{
+    int requestedRate = (int)(1000000.0 / 100.0); // 100 Hz in usecs (better set this a bit higher than actually needed,
+    // to give it more priority in case of exceeding link bandwidth)
+
+    _nextDesiredRates = QVector<DesiredStreamRate>{{
+        {MAVLINK_MSG_ID_ATTITUDE_QUATERNION, requestedRate},
+        {MAVLINK_MSG_ID_ATTITUDE_TARGET, requestedRate},
+    }};
+    // we could check if we're already configured as requested
+
+    setNextState(State::Configuring);
+}
+
+void MAVLinkStreamConfig::gotSetMessageIntervalAck()
+{
+    switch (_state) {
+        case State::Configuring:
+            nextDesiredRate();
+            break;
+        case State::RestoringDefaults:
+            restoreNextDefault();
+            break;
+        default:
+            break;
+    }
+}
+
+void MAVLinkStreamConfig::setNextState(State state)
+{
+    _nextState = state;
+    if (_state == State::Idle) {
+        // first restore defaults no matter what the next state is
+        _state = State::RestoringDefaults;
+        restoreNextDefault();
+    }
+}
+
+void MAVLinkStreamConfig::nextDesiredRate()
+{
+    if (_nextState != State::Idle) { // allow state to be interrupted by a new request
+        _desiredRates.clear();
+        _state = State::RestoringDefaults;
+        restoreNextDefault();
+        return;
+    }
+
+    if (_desiredRates.empty()) {
+        _state = State::Idle;
+        return;
+    }
+    const DesiredStreamRate& rate = _desiredRates.last();
+    _changedIds.push_back(rate.messageId);
+    _messageIntervalCb(rate.messageId, rate.rate);
+    _desiredRates.pop_back();
+}
+
+void MAVLinkStreamConfig::restoreDefaults()
+{
+    setNextState(State::RestoringDefaults);
+}
+
+void MAVLinkStreamConfig::restoreNextDefault()
+{
+    if (_changedIds.empty()) {
+        // do we have a pending request?
+        switch (_nextState) {
+            case State::Configuring:
+                _state = _nextState;
+                _desiredRates = _nextDesiredRates;
+                _nextDesiredRates.clear();
+                _nextState = State::Idle;
+                nextDesiredRate();
+                break;
+            case State::RestoringDefaults:
+            case State::Idle:
+                _nextState = State::Idle; // nothing to do, we just finished restoring
+                _state = State::Idle;
+                break;
+        }
+        return;
+    }
+
+    int id = _changedIds.last();
+    _messageIntervalCb(id, 0); // restore the default rate
+    _changedIds.pop_back();
+}

src/Vehicle/MAVLinkStreamConfig.h

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ * (c) 2021 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <QVector>
+
+#include <functional>
+
+/**
+ * @class MAVLinkStreamConfig
+ * Allows to configure a set of mavlink streams to a specific rate,
+ * and restore back to default.
+ * Note that only one set is active at a time.
+ */
+class MAVLinkStreamConfig
+{
+public:
+    using SetMessageIntervalCb = std::function<void(int messageId, int rate)>;
+
+    MAVLinkStreamConfig(const SetMessageIntervalCb& messageIntervalCb);
+
+    void setHighRateRateAndAttitude();
+
+    void restoreDefaults();
+
+    void gotSetMessageIntervalAck();
+private:
+    enum class State {
+        Idle,
+        RestoringDefaults,
+        Configuring
+    };
+
+    struct DesiredStreamRate {
+        int messageId;
+        int rate;
+    };
+
+    void restoreNextDefault();
+    void nextDesiredRate();
+    void setNextState(State state);
+
+    State _state{State::Idle};
+    QVector<DesiredStreamRate> _desiredRates;
+    QVector<int> _changedIds;
+
+    State _nextState{State::Idle};
+    QVector<DesiredStreamRate> _nextDesiredRates;
+
+    const SetMessageIntervalCb _messageIntervalCb;
+};

src/Vehicle/Vehicle.cc

@@ -101,8 +101,6 @@ const char* Vehicle::_escStatusFactGroupName =          "escStatus";
 const char* Vehicle::_estimatorStatusFactGroupName =    "estimatorStatus";
 const char* Vehicle::_terrainFactGroupName =            "terrain";
 
-const QList<int> Vehicle::_pidTuningMessages = {MAVLINK_MSG_ID_ATTITUDE_QUATERNION, MAVLINK_MSG_ID_ATTITUDE_TARGET};
-
 // Standard connected vehicle
 Vehicle::Vehicle(LinkInterface*             link,
                  int                        vehicleId,
@@ -123,6 +121,7 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _firmwarePluginManager        (firmwarePluginManager)
     , _joystickManager              (joystickManager)
     , _trajectoryPoints             (new TrajectoryPoints(this, this))
+    , _mavlinkStreamConfig          (std::bind(&Vehicle::_setMessageInterval, this, std::placeholders::_1, std::placeholders::_2))
     , _rollFact                     (0, _rollFactName,              FactMetaData::valueTypeDouble)
     , _pitchFact                    (0, _pitchFactName,             FactMetaData::valueTypeDouble)
     , _headingFact                  (0, _headingFactName,           FactMetaData::valueTypeDouble)
@@ -270,6 +269,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
     , _capabilityBits                   (MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY)
     , _firmwarePluginManager            (firmwarePluginManager)
     , _trajectoryPoints                 (new TrajectoryPoints(this, this))
+    , _mavlinkStreamConfig              (std::bind(&Vehicle::_setMessageInterval, this, std::placeholders::_1, std::placeholders::_2))
     , _rollFact                         (0, _rollFactName,              FactMetaData::valueTypeDouble)
     , _pitchFact                        (0, _pitchFactName,             FactMetaData::valueTypeDouble)
     , _headingFact                      (0, _headingFactName,           FactMetaData::valueTypeDouble)
@@ -703,9 +703,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS:
         _handleOrbitExecutionStatus(message);
         break;
-    case MAVLINK_MSG_ID_MESSAGE_INTERVAL:
-        _handleMessageInterval(message);
-        break;
     case MAVLINK_MSG_ID_PING:
         _handlePing(link, message);
         break;
@@ -2892,16 +2889,8 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
     }
 
     // advance PID tuning setup/teardown
-    if (ack.command == MAV_CMD_SET_MESSAGE_INTERVAL && _pidTuningNextAdjustIndex >= 0) {
-        _pidTuningAdjustRates();
-    }
-    if (ack.command == MAV_CMD_GET_MESSAGE_INTERVAL && _pidTuningWaitingForRates) {
-        if (_pidTuningMessageRatesUsecs.count() < _pidTuningMessages.count()) {
-            sendMavCommand(defaultComponentId(),
-                    MAV_CMD_GET_MESSAGE_INTERVAL,
-                    true,                        // show error
-                    _pidTuningMessages[_pidTuningMessageRatesUsecs.count()]);
-        }
+    if (ack.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
+        _mavlinkStreamConfig.gotSetMessageIntervalAck();
     }
 }
 
@@ -3558,78 +3547,24 @@ int  Vehicle::versionCompare(int major, int minor, int patch)
     return _firmwarePlugin->versionCompare(this, major, minor, patch);
 }
 
-void Vehicle::_handleMessageInterval(const mavlink_message_t& message)
-{
-    if (_pidTuningWaitingForRates) {
-        mavlink_message_interval_t messageInterval;
-
-        mavlink_msg_message_interval_decode(&message, &messageInterval);
-
-        int msgId = messageInterval.message_id;
-        if (_pidTuningMessages.contains(msgId)) {
-            _pidTuningMessageRatesUsecs[msgId] = messageInterval.interval_us;
-        }
-
-        if (_pidTuningMessageRatesUsecs.count() == _pidTuningMessages.count()) {
-            // We have back all the rates we requested
-            _pidTuningWaitingForRates = false;
-            _pidTuningNextAdjustIndex = 0;
-            _pidTuningAdjustRates();
-        }
-    }
-}
-
 void Vehicle::setPIDTuningTelemetryMode(bool pidTuning)
 {
+    setLiveUpdates(pidTuning);
+    _setpointFactGroup.setLiveUpdates(pidTuning);
     if (pidTuning) {
-        if (!_pidTuningTelemetryMode) {
-            // First step is to get the current message rates before we adjust them
-            _pidTuningTelemetryMode = true;
-            _pidTuningWaitingForRates = true;
-            _pidTuningMessageRatesUsecs.clear();
-            sendMavCommand(defaultComponentId(),
-                    MAV_CMD_GET_MESSAGE_INTERVAL,
-                    true,                        // show error
-                    _pidTuningMessages[0]);
-        }
+        _mavlinkStreamConfig.setHighRateRateAndAttitude();
     } else {
-        if (_pidTuningTelemetryMode) {
-            _pidTuningTelemetryMode = false;
-            if (_pidTuningWaitingForRates) {
-                // We never finished waiting for previous rates
-                _pidTuningWaitingForRates = false;
-            } else {
-                _pidTuningNextAdjustIndex = 0;
-                _pidTuningAdjustRates();
-            }
-        }
+        _mavlinkStreamConfig.restoreDefaults();
     }
 }
 
-void Vehicle::_pidTuningAdjustRates()
+void Vehicle::_setMessageInterval(int messageId, int rate)
 {
-    int requestedRate = (int)(1000000.0 / 100.0); // 100 Hz in usecs (better set this a bit higher than actually needed,
-    // to give it more priority in case of exceeing link bandwidth)
-
-    if (_pidTuningNextAdjustIndex >= _pidTuningMessages.size()) {
-        _pidTuningNextAdjustIndex = -1;
-        return;
-    }
-    int telemetry = _pidTuningMessages[_pidTuningNextAdjustIndex];
-
-    if (requestedRate < _pidTuningMessageRatesUsecs[telemetry] || _pidTuningMessageRatesUsecs[telemetry] == 0) {
-        sendMavCommand(defaultComponentId(),
-                MAV_CMD_SET_MESSAGE_INTERVAL,
-                true,                        // show error
-                telemetry,
-                _pidTuningTelemetryMode ? requestedRate : _pidTuningMessageRatesUsecs[telemetry]);
-    }
-
-    if (_pidTuningNextAdjustIndex == 0) {
-        setLiveUpdates(_pidTuningTelemetryMode);
-        _setpointFactGroup.setLiveUpdates(_pidTuningTelemetryMode);
-    }
-    ++_pidTuningNextAdjustIndex;
+    sendMavCommand(defaultComponentId(),
+            MAV_CMD_SET_MESSAGE_INTERVAL,
+            true,                        // show error
+            messageId,
+            rate);
 }
 
 void Vehicle::_initializeCsv()

src/Vehicle/Vehicle.h

@@ -20,6 +20,7 @@
 #include "QGCMAVLink.h"
 #include "QmlObjectListModel.h"
 #include "MAVLinkProtocol.h"
+#include "MAVLinkStreamConfig.h"
 #include "UASMessageHandler.h"
 #include "SettingsFact.h"
 #include "QGCMapCircle.h"
@@ -924,7 +925,6 @@ private:
     void _handleAttitudeQuaternion      (mavlink_message_t& message);
     void _handleStatusText              (mavlink_message_t& message);
     void _handleOrbitExecutionStatus    (const mavlink_message_t& message);
-    void _handleMessageInterval         (const mavlink_message_t& message);
     void _handleGimbalOrientation       (const mavlink_message_t& message);
     void _handleObstacleDistance        (const mavlink_message_t& message);
     // ArduPilot dialect messages
@@ -946,13 +946,13 @@ private:
     void _setCapabilities               (uint64_t capabilityBits);
     void _updateArmed                   (bool armed);
     bool _apmArmingNotRequired          ();
-    void _pidTuningAdjustRates          ();
     void _initializeCsv                 ();
     void _writeCsvLine                  ();
     void _flightTimerStart              ();
     void _flightTimerStop               ();
     void _chunkedStatusTextTimeout      (void);
     void _chunkedStatusTextCompleted    (uint8_t compId);
+    void _setMessageInterval            (int messageId, int rate);
 
     static void _rebootCommandResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, MavCmdResultFailureCode_t failureCode);
 
@@ -1112,12 +1112,7 @@ private:
     QTimer          _orbitTelemetryTimer;
     static const int _orbitTelemetryTimeoutMsecs = 3000; // No telemetry for this amount and orbit will go inactive
 
-    // PID Tuning telemetry mode
-    bool            _pidTuningTelemetryMode = false;
-    bool            _pidTuningWaitingForRates = false;
-    int             _pidTuningNextAdjustIndex = -1;
-    QMap<int, int>  _pidTuningMessageRatesUsecs;
-    static const QList<int> _pidTuningMessages;
+    MAVLinkStreamConfig _mavlinkStreamConfig;
 
     // Chunked status text support
     typedef struct {