Save/Restore stream rates. Auto flight mode support

src/QmlControls/PIDTuning.qml

@@ -146,7 +146,7 @@ RowLayout {
         saveTuningParamValues()
     }
 
-    Component.onDestruction: _activeVehicle.setPIDTuningTelemetryMode(true)
+    Component.onDestruction: _activeVehicle.setPIDTuningTelemetryMode(false)
 
     on_CurrentTuneTypeChanged: {
         saveTuningParamValues()
@@ -197,14 +197,6 @@ RowLayout {
         running:    false
         repeat:     true
 
-        function startOrStop() {
-            if (dataTimer.running) {
-                dataTimer.stop()
-            } else {
-                dataTimer.start()
-            }
-        }
-
         onTriggered: {
             _valueXAxis.max = _msecs
             _valueRateXAxis.max = _msecs
@@ -238,25 +230,25 @@ RowLayout {
         spacing:            _margins
         Layout.alignment:   Qt.AlignTop
 
-        QGCLabel { text: qsTr("Tuning Axis:") }
+        Column {
+            QGCLabel { text: qsTr("Tuning Axis:") }
 
-        RowLayout {
+            RowLayout {
-            spacing: _margins
+                spacing: _margins
 
-            Repeater {
+                Repeater {
-                model: tuneList
+                    model: tuneList
-                QGCRadioButton {
+                    QGCRadioButton {
-                    text:           modelData
+                        text:           modelData
-                    checked:        _currentTuneType === modelData
+                        checked:        _currentTuneType === modelData
-                    exclusiveGroup: tuneTypeRadios
+                        exclusiveGroup: tuneTypeRadios
 
-                    onClicked: _currentTuneType = modelData
+                        onClicked: _currentTuneType = modelData
+                    }
                 }
             }
         }
 
-        Item { width: 1; height: 1 }
-
         QGCLabel { text: qsTr("Tuning Values:") }
 
         GridLayout {
@@ -328,26 +320,27 @@ RowLayout {
                 }
             }
         }
-        Item { width: 1; height: 1 }
 
-        QGCLabel { text: qsTr("Clipboard Values:") }
+        Column {
+            QGCLabel { text: qsTr("Clipboard Values:") }
 
-        GridLayout {
+            GridLayout {
-            rows:           savedRepeater.model.length
+                rows:           savedRepeater.model.length
-            flow:           GridLayout.TopToBottom
+                flow:           GridLayout.TopToBottom
-            rowSpacing:     _margins
+                rowSpacing:     0
-            columnSpacing:  _margins
+                columnSpacing:  _margins
 
-            Repeater {
+                Repeater {
-                model: params[tuneList.indexOf(_currentTuneType)]
+                    model: params[tuneList.indexOf(_currentTuneType)]
 
-                QGCLabel { text: modelData.name }
+                    QGCLabel { text: modelData.name }
-            }
+                }
 
-            Repeater {
+                Repeater {
-                id: savedRepeater
+                    id: savedRepeater
 
-                QGCLabel { text: modelData }
+                    QGCLabel { text: modelData }
+                }
             }
         }
 
@@ -379,7 +372,30 @@ RowLayout {
 
             QGCButton {
                 text:       dataTimer.running ? qsTr("Stop") : qsTr("Start")
-                onClicked:  dataTimer.startOrStop()
+                onClicked: {
+                    dataTimer.running = !dataTimer.running
+                    if (autoModeChange.checked) {
+                        _activeVehicle.flightMode = dataTimer.running ? "Stabilized" : _activeVehicle.pauseFlightMode
+                    }
+                }
+            }
+        }
+
+        QGCCheckBox {
+            id:     autoModeChange
+            text:   qsTr("Automatic Flight Mode Switching")
+        }
+
+        Column {
+            visible: autoModeChange.checked
+            QGCLabel {
+                text:            qsTr("Switches to 'Stabilized' when you click Start.")
+                font.pointSize:     ScreenTools.smallFontPointSize
+            }
+
+            QGCLabel {
+                text:            qsTr("Switches to '%1' when you click Stop.").arg(_activeVehicle.pauseFlightMode)
+                font.pointSize:     ScreenTools.smallFontPointSize
             }
         }
     }

src/Vehicle/Vehicle.cc

@@ -185,6 +185,8 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _lastAnnouncedLowBatteryPercent(100)
     , _priorityLinkCommanded(false)
     , _orbitActive(false)
+    , _pidTuningTelemetryMode(false)
+    , _pidTuningWaitingForRates(false)
     , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
     , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
     , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
@@ -386,6 +388,8 @@ Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
     , _uid(0)
     , _lastAnnouncedLowBatteryPercent(100)
     , _orbitActive(false)
+    , _pidTuningTelemetryMode(false)
+    , _pidTuningWaitingForRates(false)
     , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
     , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
     , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
@@ -519,6 +523,8 @@ void Vehicle::_commonInit(void)
         }
     }
 #endif
+
+    _pidTuningMessages << MAVLINK_MSG_ID_ATTITUDE << MAVLINK_MSG_ID_ATTITUDE_TARGET;
 }
 
 Vehicle::~Vehicle()
@@ -794,7 +800,9 @@ 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;
@@ -3859,29 +3867,71 @@ int  Vehicle::versionCompare(int major, int minor, int patch)
     return _firmwarePlugin->versionCompare(this, major, minor, patch);
 }
 
-void Vehicle::setPIDTuningTelemetryMode(bool pidTuning)
+void Vehicle::_handleMessageInterval(const mavlink_message_t& message)
 {
-    qDebug() << "setPIDTuningTelemetryMode" << pidTuning;
+    if (_pidTuningWaitingForRates) {
-    QList<int> rgTelemetry;
+        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;
+            _pidTuningAdjustRates(true);
+        }
+    }
+}
+
+void Vehicle::setPIDTuningTelemetryMode(bool pidTuning)
+{
     if (pidTuning) {
-        rgTelemetry << MAVLINK_MSG_ID_ATTITUDE << MAVLINK_MSG_ID_ATTITUDE_TARGET;
+        if (!_pidTuningTelemetryMode) {
-        for (int telemetry: rgTelemetry) {
+            // First step is to get the current message rates before we adjust them
+            _pidTuningTelemetryMode = true;
+            _pidTuningWaitingForRates = true;
+            _pidTuningMessageRatesUsecs.clear();
+            for (int telemetry: _pidTuningMessages) {
+                sendMavCommand(defaultComponentId(),
+                               MAV_CMD_GET_MESSAGE_INTERVAL,
+                               true,                        // show error
+                               telemetry);
+            }
+        }
+    } else {
+        if (_pidTuningTelemetryMode) {
+            _pidTuningTelemetryMode = false;
+            if (_pidTuningWaitingForRates) {
+                // We never finished waiting for previous rates
+                _pidTuningWaitingForRates = false;
+            } else {
+                _pidTuningAdjustRates(false);
+            }
+        }
+    }
+}
+
+void Vehicle::_pidTuningAdjustRates(bool setRatesForTuning)
+{
+    int requestedRate = (int)(1000000.0 / 30.0); // 30 Hz in usecs
+
+    for (int telemetry: _pidTuningMessages) {
+
+        if (requestedRate < _pidTuningMessageRatesUsecs[telemetry]) {
             sendMavCommand(defaultComponentId(),
                            MAV_CMD_SET_MESSAGE_INTERVAL,
                            true,                        // show error
                            telemetry,
-                           pidTuning ? (1000000.0 / 30.0) /* Hz */ : -1 /* default Hz */);
+                           setRatesForTuning ? requestedRate : _pidTuningMessageRatesUsecs[telemetry]);
         }
     }
 
-    setLiveUpdates(pidTuning);
+    setLiveUpdates(setRatesForTuning);
-    _setpointFactGroup.setLiveUpdates(pidTuning);
+    _setpointFactGroup.setLiveUpdates(setRatesForTuning);
-
-    sendMavCommand(defaultComponentId(),
-                   MAV_CMD_GET_MESSAGE_INTERVAL,
-                   true,                        // show error
-                   MAVLINK_MSG_ID_ATTITUDE);
 }
 
 

src/Vehicle/Vehicle.h

@@ -1267,6 +1267,7 @@ private:
     void _handleEstimatorStatus(mavlink_message_t& message);
     void _handleStatusText(mavlink_message_t& message, bool longVersion);
     void _handleOrbitExecutionStatus(const mavlink_message_t& message);
+    void _handleMessageInterval(const mavlink_message_t& message);
     // ArduPilot dialect messages
 #if !defined(NO_ARDUPILOT_DIALECT)
     void _handleCameraFeedback(const mavlink_message_t& message);
@@ -1293,6 +1294,7 @@ private:
     void _setCapabilities(uint64_t capabilityBits);
     void _updateArmed(bool armed);
     bool _apmArmingNotRequired(void);
+    void _pidTuningAdjustRates(bool setRatesForTuning);
 
     int     _id;                    ///< Mavlink system id
     int     _defaultComponentId;
@@ -1475,6 +1477,12 @@ private:
     QTimer          _orbitTelemetryTimer;
     static const int _orbitTelemetryTimeoutMsecs = 3000; // No telemetry for this amount and orbit will go inactive
 
+    // PID Tuning telemetry mode
+    bool            _pidTuningTelemetryMode;
+    bool            _pidTuningWaitingForRates;
+    QList<int>      _pidTuningMessages;
+    QMap<int, int>  _pidTuningMessageRatesUsecs;
+
     // FactGroup facts
 
     Fact _rollFact;