Merge pull request #7407 from DonLakeFlyer/PIDTuning

Pid Tuning: Stream rate adjustment, Automatic flight mode switching

src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml

@@ -61,26 +61,6 @@ SetupPage {
                         max:            15
                         step:           1
                     }
-                    /*
-  These seem to have disappeared from PX4 firmware!
-                    ListElement {
-                        title:          qsTr("Roll sensitivity")
-                        description:    qsTr("Slide to the left to make roll control faster and more accurate. Slide to the right if roll oscillates or is too twitchy.")
-                        param:          "MC_ROLL_TC"
-                        min:            0.15
-                        max:            0.25
-                        step:           0.01
-                    }
-
-                    ListElement {
-                        title:          qsTr("Pitch sensitivity")
-                        description:    qsTr("Slide to the left to make pitch control faster and more accurate. Slide to the right if pitch oscillates or is too twitchy.")
-                        param:          "MC_PITCH_TC"
-                        min:            0.15
-                        max:            0.25
-                        step:           0.01
-                    }
-*/
                 }
             }
 

src/QmlControls/FactSliderPanel.qml

@@ -34,6 +34,8 @@ Column {
     property real _margins:         ScreenTools.defaultFontPixelHeight
     property bool _loadComplete:    false
 
+    Component.onCompleted: _loadComplete = true
+
     FactPanelController {
         id:         controller
         factPanel:  qgcViewPanel
@@ -71,10 +73,22 @@ Column {
                         font.family:    ScreenTools.demiboldFontFamily
                     }
 
-                    FactValueSlider {
-                        digitCount:     fact.maxString.length
-                        incrementSlots: 3
-                        fact:           controller.getParameterFact(-1, param)
+                    Slider {
+                        anchors.left:       parent.left
+                        anchors.right:      parent.right
+                        minimumValue:       min
+                        maximumValue:       max
+                        stepSize:           step
+                        tickmarksEnabled:   true
+                        value:              _fact.value
+
+                        property Fact _fact: controller.getParameterFact(-1, param)
+
+                        onValueChanged: {
+                            if (_loadComplete) {
+                                _fact.value = value
+                            }
+                        }
                     }
 
                     QGCLabel {

src/QmlControls/PIDTuning.qml

@@ -142,16 +142,11 @@ RowLayout {
     }
 
     Component.onCompleted: {
-        // Stop deferring updates to vehicle values. We need them as fast as we can for charting.
-        _activeVehicle.setLiveUpdates(true)
-        _activeVehicle.setpoint.setLiveUpdates(true)
+        _activeVehicle.setPIDTuningTelemetryMode(true)
         saveTuningParamValues()
     }
 
-    Component.onDestruction: {
-        _activeVehicle.setLiveUpdates(false)
-        _activeVehicle.setpoint.setLiveUpdates(true)
-    }
+    Component.onDestruction: _activeVehicle.setPIDTuningTelemetryMode(false)
 
     on_CurrentTuneTypeChanged: {
         saveTuningParamValues()
@@ -202,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
@@ -243,6 +230,7 @@ RowLayout {
         spacing:            _margins
         Layout.alignment:   Qt.AlignTop
 
+        Column {
             QGCLabel { text: qsTr("Tuning Axis:") }
 
             RowLayout {
@@ -259,8 +247,7 @@ RowLayout {
                     }
                 }
             }
-
-        Item { width: 1; height: 1 }
+        }
 
         QGCLabel { text: qsTr("Tuning Values:") }
 
@@ -333,14 +320,14 @@ RowLayout {
                 }
             }
         }
-        Item { width: 1; height: 1 }
 
+        Column {
             QGCLabel { text: qsTr("Clipboard Values:") }
 
             GridLayout {
                 rows:           savedRepeater.model.length
                 flow:           GridLayout.TopToBottom
-            rowSpacing:     _margins
+                rowSpacing:     0
                 columnSpacing:  _margins
 
                 Repeater {
@@ -355,6 +342,7 @@ RowLayout {
                     QGCLabel { text: modelData }
                 }
             }
+        }
 
         RowLayout {
             spacing: _margins
@@ -384,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,6 +3867,74 @@ 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;
+            _pidTuningAdjustRates(true);
+        }
+    }
+}
+
+void Vehicle::setPIDTuningTelemetryMode(bool pidTuning)
+{
+    if (pidTuning) {
+        if (!_pidTuningTelemetryMode) {
+            // 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,
+                           setRatesForTuning ? requestedRate : _pidTuningMessageRatesUsecs[telemetry]);
+        }
+    }
+
+    setLiveUpdates(setRatesForTuning);
+    _setpointFactGroup.setLiveUpdates(setRatesForTuning);
+}
+
+
 #if !defined(NO_ARDUPILOT_DIALECT)
 void Vehicle::flashBootloader(void)
 {

src/Vehicle/Vehicle.h

@@ -758,6 +758,8 @@ public:
     ///     @param percent 0-no power, 100-full power
     Q_INVOKABLE void motorTest(int motor, int percent);
 
+    Q_INVOKABLE void setPIDTuningTelemetryMode(bool pidTuning);
+
 #if !defined(NO_ARDUPILOT_DIALECT)
     Q_INVOKABLE void flashBootloader(void);
 #endif
@@ -1265,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);
@@ -1291,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;
@@ -1473,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;