mc pid tuning: add tab for velocity controller

src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml

@@ -25,113 +25,35 @@ SetupPage {
     Component {
         id: pageComponent
 
-        Column {
+        Item {
             width: availableWidth
 
             FactPanelController {
                 id:         controller
             }
 
-            PIDTuning {
+            QGCTabBar {
-                width: availableWidth
+                id:             bar
-
+                width:          parent.width
-                property var roll: QtObject {
+                anchors.top:    parent.top
-                    property string name: qsTr("Roll")
+                QGCTabButton {
-                    property var plot: [
+                    text:       qsTr("Rate Controller")
-                        { name: "Response", value: globals.activeVehicle.rollRate.value },
-                        { name: "Setpoint", value: globals.activeVehicle.setpoint.rollRate.value }
-                    ]
-                    property var params: ListModel {
-                        ListElement {
-                            title:          qsTr("Overall Multiplier (MC_ROLLRATE_K)")
-                            description:    qsTr("Multiplier for P, I and D gains: increase for more responsiveness, reduce if the rates overshoot (and increasing D does not help).")
-                            param:          "MC_ROLLRATE_K"
-                            min:            0.3
-                            max:            3
-                            step:           0.05
-                        }
-                        ListElement {
-                            title:          qsTr("Differential Gain (MC_ROLLRATE_D)")
-                            description:    qsTr("Damping: increase to reduce overshoots and oscillations, but not higher than really needed.")
-                            param:          "MC_ROLLRATE_D"
-                            min:            0.0004
-                            max:            0.01
-                            step:           0.0002
-                        }
-                        ListElement {
-                            title:          qsTr("Integral Gain (MC_ROLLRATE_I)")
-                            description:    qsTr("Generally does not need much adjustment, reduce this when seeing slow oscillations.")
-                            param:          "MC_ROLLRATE_I"
-                            min:            0.1
-                            max:            0.5
-                            step:           0.025
-                        }
-                    }
-                }
-                property var pitch: QtObject {
-                    property string name: qsTr("Pitch")
-                    property var plot: [
-                        { name: "Response", value: globals.activeVehicle.pitchRate.value },
-                        { name: "Setpoint", value: globals.activeVehicle.setpoint.pitchRate.value }
-                    ]
-                    property var params: ListModel {
-                        ListElement {
-                            title:          qsTr("Overall Multiplier (MC_PITCHRATE_K)")
-                            description:    qsTr("Multiplier for P, I and D gains: increase for more responsiveness, reduce if the rates overshoot (and increasing D does not help).")
-                            param:          "MC_PITCHRATE_K"
-                            min:            0.3
-                            max:            3
-                            step:           0.05
-                        }
-                        ListElement {
-                            title:          qsTr("Differential Gain (MC_PITCHRATE_D)")
-                            description:    qsTr("Damping: increase to reduce overshoots and oscillations, but not higher than really needed.")
-                            param:          "MC_PITCHRATE_D"
-                            min:            0.0004
-                            max:            0.01
-                            step:           0.0002
-                        }
-                        ListElement {
-                            title:          qsTr("Integral Gain (MC_PITCHRATE_I)")
-                            description:    qsTr("Generally does not need much adjustment, reduce this when seeing slow oscillations.")
-                            param:          "MC_PITCHRATE_I"
-                            min:            0.1
-                            max:            0.5
-                            step:           0.025
-                        }
-                    }
                 }
-                property var yaw: QtObject {
+                QGCTabButton {
-                    property string name: qsTr("Yaw")
+                    text:       qsTr("Velocity Controller")
-                    property var plot: [
-                        { name: "Response", value: globals.activeVehicle.yawRate.value },
-                        { name: "Setpoint", value: globals.activeVehicle.setpoint.yawRate.value }
-                    ]
-                    property var params: ListModel {
-                        ListElement {
-                            title:          qsTr("Overall Multiplier (MC_YAWRATE_K)")
-                            description:    qsTr("Multiplier for P, I and D gains: increase for more responsiveness, reduce if the rates overshoot (and increasing D does not help).")
-                            param:          "MC_YAWRATE_K"
-                            min:            0.3
-                            max:            3
-                            step:           0.05
-                        }
-                        ListElement {
-                            title:          qsTr("Integral Gain (MC_YAWRATE_I)")
-                            description:    qsTr("Generally does not need much adjustment, reduce this when seeing slow oscillations.")
-                            param:          "MC_YAWRATE_I"
-                            min:            0.04
-                            max:            0.4
-                            step:           0.02
-                        }
-                    }
                 }
-                anchors.left:   parent.left
-                anchors.right:  parent.right
-                title: "Rate"
-                unit: "deg/s"
-                axis: [ roll, pitch, yaw ]
             }
-        } // Column
+
+            property var pages:  [
+                "PX4TuningComponentCopterRate.qml",
+                "PX4TuningComponentCopterVelocity.qml"
+            ]
+
+            Loader {
+                source:         pages[bar.currentIndex]
+                width:          parent.width
+                anchors.top:    bar.bottom
+            }
+        }
     } // Component - pageComponent
 } // SetupPage

src/AutoPilotPlugins/PX4/PX4TuningComponentCopterRate.qml

@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ * (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.
+ *
+ ****************************************************************************/
+
+import QtQuick          2.3
+import QtQuick.Controls 1.2
+import QtQuick.Layouts  1.2
+
+import QGroundControl               1.0
+import QGroundControl.Controls      1.0
+import QGroundControl.FactSystem    1.0
+import QGroundControl.FactControls  1.0
+import QGroundControl.ScreenTools   1.0
+import QGroundControl.Vehicle       1.0
+
+PIDTuning {
+    width: availableWidth
+
+    property var roll: QtObject {
+        property string name: qsTr("Roll")
+        property var plot: [
+            { name: "Response", value: globals.activeVehicle.rollRate.value },
+            { name: "Setpoint", value: globals.activeVehicle.setpoint.rollRate.value }
+        ]
+        property var params: ListModel {
+            ListElement {
+                title:          qsTr("Overall Multiplier (MC_ROLLRATE_K)")
+                description:    qsTr("Multiplier for P, I and D gains: increase for more responsiveness, reduce if the rates overshoot (and increasing D does not help).")
+                param:          "MC_ROLLRATE_K"
+                min:            0.3
+                max:            3
+                step:           0.05
+            }
+            ListElement {
+                title:          qsTr("Differential Gain (MC_ROLLRATE_D)")
+                description:    qsTr("Damping: increase to reduce overshoots and oscillations, but not higher than really needed.")
+                param:          "MC_ROLLRATE_D"
+                min:            0.0004
+                max:            0.01
+                step:           0.0002
+            }
+            ListElement {
+                title:          qsTr("Integral Gain (MC_ROLLRATE_I)")
+                description:    qsTr("Generally does not need much adjustment, reduce this when seeing slow oscillations.")
+                param:          "MC_ROLLRATE_I"
+                min:            0.1
+                max:            0.5
+                step:           0.025
+            }
+        }
+    }
+    property var pitch: QtObject {
+        property string name: qsTr("Pitch")
+        property var plot: [
+            { name: "Response", value: globals.activeVehicle.pitchRate.value },
+            { name: "Setpoint", value: globals.activeVehicle.setpoint.pitchRate.value }
+        ]
+        property var params: ListModel {
+            ListElement {
+                title:          qsTr("Overall Multiplier (MC_PITCHRATE_K)")
+                description:    qsTr("Multiplier for P, I and D gains: increase for more responsiveness, reduce if the rates overshoot (and increasing D does not help).")
+                param:          "MC_PITCHRATE_K"
+                min:            0.3
+                max:            3
+                step:           0.05
+            }
+            ListElement {
+                title:          qsTr("Differential Gain (MC_PITCHRATE_D)")
+                description:    qsTr("Damping: increase to reduce overshoots and oscillations, but not higher than really needed.")
+                param:          "MC_PITCHRATE_D"
+                min:            0.0004
+                max:            0.01
+                step:           0.0002
+            }
+            ListElement {
+                title:          qsTr("Integral Gain (MC_PITCHRATE_I)")
+                description:    qsTr("Generally does not need much adjustment, reduce this when seeing slow oscillations.")
+                param:          "MC_PITCHRATE_I"
+                min:            0.1
+                max:            0.5
+                step:           0.025
+            }
+        }
+    }
+    property var yaw: QtObject {
+        property string name: qsTr("Yaw")
+        property var plot: [
+            { name: "Response", value: globals.activeVehicle.yawRate.value },
+            { name: "Setpoint", value: globals.activeVehicle.setpoint.yawRate.value }
+        ]
+        property var params: ListModel {
+            ListElement {
+                title:          qsTr("Overall Multiplier (MC_YAWRATE_K)")
+                description:    qsTr("Multiplier for P, I and D gains: increase for more responsiveness, reduce if the rates overshoot (and increasing D does not help).")
+                param:          "MC_YAWRATE_K"
+                min:            0.3
+                max:            3
+                step:           0.05
+            }
+            ListElement {
+                title:          qsTr("Integral Gain (MC_YAWRATE_I)")
+                description:    qsTr("Generally does not need much adjustment, reduce this when seeing slow oscillations.")
+                param:          "MC_YAWRATE_I"
+                min:            0.04
+                max:            0.4
+                step:           0.02
+            }
+        }
+    }
+    anchors.left:   parent.left
+    anchors.right:  parent.right
+    title: "Rate"
+    tuningMode: Vehicle.ModeRateAndAttitude
+    unit: "deg/s"
+    axis: [ roll, pitch, yaw ]
+    chartDisplaySec: 3
+}
+

src/AutoPilotPlugins/PX4/PX4TuningComponentCopterVelocity.qml

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ * (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.
+ *
+ ****************************************************************************/
+
+import QtQuick          2.3
+import QtQuick.Controls 1.2
+import QtQuick.Layouts  1.2
+
+import QGroundControl               1.0
+import QGroundControl.Controls      1.0
+import QGroundControl.FactSystem    1.0
+import QGroundControl.FactControls  1.0
+import QGroundControl.ScreenTools   1.0
+import QGroundControl.Vehicle       1.0
+
+PIDTuning {
+    width: availableWidth
+
+    property var horizontal: QtObject {
+        property string name: qsTr("Horizontal")
+        property string plotTitle: qsTr("Horizontal (Y direction, sidewards)")
+        property var plot: [
+            { name: "Response", value: globals.activeVehicle.localPosition.vy.value },
+            { name: "Setpoint", value: globals.activeVehicle.localPositionSetpoint.vy.value }
+        ]
+        property var params: ListModel {
+            ListElement {
+                title:          qsTr("Proportional gain (MPC_XY_VEL_P_ACC)")
+                description:    qsTr("TODO")
+                param:          "MPC_XY_VEL_P_ACC"
+                min:            1.2
+                max:            5
+                step:           0.05
+            }
+            ListElement {
+                title:          qsTr("Integral gain (MPC_XY_VEL_I_ACC)")
+                description:    qsTr("TODO")
+                param:          "MPC_XY_VEL_I_ACC"
+                min:            0.2
+                max:            10
+                step:           0.2
+            }
+            ListElement {
+                title:          qsTr("Differential gain (MPC_XY_VEL_D_ACC)")
+                description:    qsTr("TODO")
+                param:          "MPC_XY_VEL_D_ACC"
+                min:            0.1
+                max:            2
+                step:           0.05
+            }
+        }
+    }
+    property var vertical: QtObject {
+        property string name: qsTr("Vertical")
+        property var plot: [
+            { name: "Response", value: globals.activeVehicle.localPosition.vz.value },
+            { name: "Setpoint", value: globals.activeVehicle.localPositionSetpoint.vz.value }
+        ]
+        property var params: ListModel {
+            ListElement {
+                title:          qsTr("Proportional gain (MPC_Z_VEL_P_ACC)")
+                description:    qsTr("TODO")
+                param:          "MPC_Z_VEL_P_ACC"
+                min:            2
+                max:            15
+                step:           0.5
+            }
+            ListElement {
+                title:          qsTr("Integral gain (MPC_Z_VEL_I_ACC)")
+                description:    qsTr("TODO")
+                param:          "MPC_Z_VEL_I_ACC"
+                min:            0.2
+                max:            3
+                step:           0.05
+            }
+            ListElement {
+                title:          qsTr("Differential gain (MPC_Z_VEL_D_ACC)")
+                description:    qsTr("TODO")
+                param:          "MPC_Z_VEL_D_ACC"
+                min:            0
+                max:            2
+                step:           0.05
+            }
+        }
+    }
+    anchors.left:   parent.left
+    anchors.right:  parent.right
+    title: "Velocity"
+    tuningMode: Vehicle.ModeVelocityAndPosition
+    unit: "m/s"
+    axis: [ horizontal, vertical ]
+}
+
+

src/FirmwarePlugin/PX4/PX4Resources.qrc

@@ -13,6 +13,8 @@
         <file alias="PX4SimpleFlightModes.qml">../../AutoPilotPlugins/PX4/PX4SimpleFlightModes.qml</file>
         <file alias="PX4FlightBehaviorCopter.qml">../../AutoPilotPlugins/PX4/PX4FlightBehaviorCopter.qml</file>
         <file alias="PX4TuningComponentCopter.qml">../../AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml</file>
+        <file alias="PX4TuningComponentCopterRate.qml">../../AutoPilotPlugins/PX4/PX4TuningComponentCopterRate.qml</file>
+        <file alias="PX4TuningComponentCopterVelocity.qml">../../AutoPilotPlugins/PX4/PX4TuningComponentCopterVelocity.qml</file>
         <file alias="PX4TuningComponentPlane.qml">../../AutoPilotPlugins/PX4/PX4TuningComponentPlane.qml</file>
         <file alias="PX4TuningComponentVTOL.qml">../../AutoPilotPlugins/PX4/PX4TuningComponentVTOL.qml</file>
         <file alias="SafetyComponent.qml">../../AutoPilotPlugins/PX4/SafetyComponent.qml</file>

src/QmlControls/PIDTuning.qml

@@ -17,6 +17,7 @@ import QGroundControl.Controls      1.0
 import QGroundControl.FactSystem    1.0
 import QGroundControl.FactControls  1.0
 import QGroundControl.ScreenTools   1.0
+import QGroundControl.Vehicle       1.0
 
 RowLayout {
     layoutDirection: Qt.RightToLeft
@@ -24,6 +25,8 @@ RowLayout {
     property var    axis
     property string unit
     property string title
+    property var    tuningMode
+    property double chartDisplaySec:     8 // number of seconds to display
 
     property real   _margins:           ScreenTools.defaultFontPixelHeight / 2
     property int    _currentAxis:       0
@@ -36,7 +39,6 @@ RowLayout {
 
     readonly property int _tickSeparation:      5
     readonly property int _maxTickSections:     10
-    readonly property int _chartDisplaySec:     3 // number of seconds to display
 
     function adjustYAxisMin(yAxis, newValue) {
         var newMin = Math.min(yAxis.min, newValue)
@@ -92,18 +94,21 @@ RowLayout {
         axis[_currentAxis].plot.forEach(function(e) {
             chart.createSeries(ChartView.SeriesTypeLine, e.name, xAxis, yAxis);
         })
-        chart.title = axis[_currentAxis].name + " " + title
+        var chartTitle = axis[_currentAxis].plotTitle
+        if (chartTitle == null)
+            chartTitle = axis[_currentAxis].name
+        chart.title = chartTitle + " " + title
         saveTuningParamValues()
         resetGraphs()
     }
 
     Component.onCompleted: {
         axisIndexChanged()
-        globals.activeVehicle.setPIDTuningTelemetryMode(true)
+        globals.activeVehicle.setPIDTuningTelemetryMode(tuningMode)
         saveTuningParamValues()
     }
 
-    Component.onDestruction: globals.activeVehicle.setPIDTuningTelemetryMode(false)
+    Component.onDestruction: globals.activeVehicle.setPIDTuningTelemetryMode(Vehicle.ModeDisabled)
     on_CurrentAxisChanged: axisIndexChanged()
 
     ValueAxis {
@@ -131,7 +136,7 @@ RowLayout {
 
         onTriggered: {
             _xAxis.max = _msecs / 1000
-            _xAxis.min = _msecs / 1000 - _chartDisplaySec
+            _xAxis.min = _msecs / 1000 - chartDisplaySec
 
             var len = axis[_currentAxis].plot.length
             for (var i = 0; i < len; ++i) {
@@ -162,7 +167,7 @@ RowLayout {
             RowLayout {
                 spacing: _margins
 
-                QGCLabel { text: qsTr("Tuning Axis:") }
+                QGCLabel { text: qsTr("Select Tuning:") }
 
                 Repeater {
                     model: axis

src/Vehicle/MAVLinkStreamConfig.cc

@@ -29,6 +29,26 @@ void MAVLinkStreamConfig::setHighRateRateAndAttitude()
     setNextState(State::Configuring);
 }
 
+void MAVLinkStreamConfig::setHighRateVelAndPos()
+{
+    int requestedRate = (int)(1000000.0 / 100.0);
+    _nextDesiredRates = QVector<DesiredStreamRate>{{
+        {MAVLINK_MSG_ID_LOCAL_POSITION_NED, requestedRate},
+        {MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, requestedRate},
+    }};
+    setNextState(State::Configuring);
+}
+
+void MAVLinkStreamConfig::setHighRateAltAirspeed()
+{
+    int requestedRate = (int)(1000000.0 / 100.0);
+    _nextDesiredRates = QVector<DesiredStreamRate>{{
+        {MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, requestedRate},
+        {MAVLINK_MSG_ID_VFR_HUD, requestedRate},
+    }};
+    setNextState(State::Configuring);
+}
+
 void MAVLinkStreamConfig::gotSetMessageIntervalAck()
 {
     switch (_state) {

src/Vehicle/MAVLinkStreamConfig.h

@@ -27,6 +27,8 @@ public:
     MAVLinkStreamConfig(const SetMessageIntervalCb& messageIntervalCb);
 
     void setHighRateRateAndAttitude();
+    void setHighRateVelAndPos();
+    void setHighRateAltAirspeed();
 
     void restoreDefaults();
 

src/Vehicle/Vehicle.cc

@@ -3555,14 +3555,27 @@ int  Vehicle::versionCompare(int major, int minor, int patch)
     return _firmwarePlugin->versionCompare(this, major, minor, patch);
 }
 
-void Vehicle::setPIDTuningTelemetryMode(bool pidTuning)
+void Vehicle::setPIDTuningTelemetryMode(PIDTuningTelemetryMode mode)
 {
-    setLiveUpdates(pidTuning);
+    bool liveUpdate = mode != ModeDisabled;
-    _setpointFactGroup.setLiveUpdates(pidTuning);
+    setLiveUpdates(liveUpdate);
-    if (pidTuning) {
+    _setpointFactGroup.setLiveUpdates(liveUpdate);
-        _mavlinkStreamConfig.setHighRateRateAndAttitude();
+    _localPositionFactGroup.setLiveUpdates(liveUpdate);
-    } else {
+    _localPositionSetpointFactGroup.setLiveUpdates(liveUpdate);
-        _mavlinkStreamConfig.restoreDefaults();
+
+    switch (mode) {
+        case ModeDisabled:
+            _mavlinkStreamConfig.restoreDefaults();
+            break;
+        case ModeRateAndAttitude:
+            _mavlinkStreamConfig.setHighRateRateAndAttitude();
+            break;
+        case ModeVelocityAndPosition:
+            _mavlinkStreamConfig.setHighRateVelAndPos();
+            break;
+        case ModeAltitudeAndAirspeed:
+            _mavlinkStreamConfig.setHighRateAltAirspeed();
+            break;
     }
 }
 

src/Vehicle/Vehicle.h

@@ -384,7 +384,15 @@ public:
     ///     @param timeoutSec Disabled motor after this amount of time
     Q_INVOKABLE void motorTest(int motor, int percent, int timeoutSecs, bool showError);
 
-    Q_INVOKABLE void setPIDTuningTelemetryMode(bool pidTuning);
+    enum PIDTuningTelemetryMode {
+        ModeDisabled,
+        ModeRateAndAttitude,
+        ModeVelocityAndPosition,
+        ModeAltitudeAndAirspeed,
+    };
+    Q_ENUM(PIDTuningTelemetryMode)
+
+    Q_INVOKABLE void setPIDTuningTelemetryMode(PIDTuningTelemetryMode mode);
 
     Q_INVOKABLE void gimbalControlValue (double pitch, double yaw);
     Q_INVOKABLE void gimbalPitchStep    (int direction);