PX4 PID tuning support

qgroundcontrol.qrc

@@ -224,6 +224,7 @@
         <file alias="Vehicle/ClockFact.json">src/Vehicle/ClockFact.json</file>
         <file alias="Vehicle/GPSFact.json">src/Vehicle/GPSFact.json</file>
         <file alias="Vehicle/GPSRTKFact.json">src/Vehicle/GPSRTKFact.json</file>
+		<file alias="Vehicle/SetpointFact.json">src/Vehicle/SetpointFact.json</file>
         <file alias="Vehicle/SubmarineFact.json">src/Vehicle/SubmarineFact.json</file>
         <file alias="Vehicle/TemperatureFact.json">src/Vehicle/TemperatureFact.json</file>
         <file alias="Vehicle/VehicleFact.json">src/Vehicle/VehicleFact.json</file>

src/AutoPilotPlugins/Common/SetupPage.qml

@@ -10,6 +10,7 @@
 import QtQuick          2.3
 import QtQuick.Controls 1.2
 import QtQuick.Dialogs  1.2
+import QtQuick.Layouts  1.2
 
 import QGroundControl               1.0
 import QGroundControl.FactSystem    1.0
@@ -30,6 +31,8 @@ QGCView {
     property string pageDescription:    vehicleComponent ? vehicleComponent.description : ""
     property real   availableWidth:     width - pageLoader.x
     property real   availableHeight:    height - pageLoader.y
+    property bool   showAdvanced:       false
+    property alias  advanced:           advancedCheckBox.checked
 
     property var    _activeVehicle:         QGroundControl.multiVehicleManager.activeVehicle
     property bool   _vehicleArmed:          _activeVehicle ? _activeVehicle.armed : false
@@ -53,31 +56,45 @@ QGCView {
             contentHeight:  pageLoader.y + pageLoader.item.height
             clip:           true
 
-            Column {
-                id:                 headingColumn
-                width:              setupPanel.width
+            RowLayout {
+                id:                 headingRow
+                anchors.left:       parent.left
+                anchors.right:      parent.right
                 spacing:            _margins
+                layoutDirection:    Qt.RightToLeft
 
-                QGCLabel {
-                    font.pointSize: ScreenTools.largeFontPointSize
-                    text:           !setupView.enabled ? _pageTitle + "<font color=\"red\">" + qsTr(" (Disabled while the vehicle is %1)").arg(_disableReason) + "</font>" : _pageTitle
-                    visible:        !ScreenTools.isShortScreen
+                QGCCheckBox {
+                    id:         advancedCheckBox
+                    text:       qsTr("Advanced")
+                    visible:    showAdvanced
                 }
 
-                QGCLabel {
-                    anchors.left:   parent.left
-                    anchors.right:  parent.right
-                    wrapMode:       Text.WordWrap
-                    text:           pageDescription
-                    visible:        !ScreenTools.isShortScreen
+                Column {
+                    spacing:            _margins
+                    Layout.fillWidth:   true
+
+                    QGCLabel {
+                        font.pointSize: ScreenTools.largeFontPointSize
+                        text:           !setupView.enabled ? _pageTitle + "<font color=\"red\">" + qsTr(" (Disabled while the vehicle is %1)").arg(_disableReason) + "</font>" : _pageTitle
+                        visible:        !ScreenTools.isShortScreen
+                    }
+
+                    QGCLabel {
+                        anchors.left:   parent.left
+                        anchors.right:  parent.right
+                        wrapMode:       Text.WordWrap
+                        text:           pageDescription
+                        visible:        !ScreenTools.isShortScreen
+                    }
                 }
             }
 
             Loader {
                 id:                 pageLoader
                 anchors.topMargin:  _margins
-                anchors.top:        headingColumn.bottom
+                anchors.top:        headingRow.bottom
             }
+
             // Overlay to display when vehicle is armed and this setup page needs
             // to be disabled
             Rectangle {

src/AutoPilotPlugins/PX4/PX4TuningComponent.h

@@ -32,6 +32,7 @@ public:
     QUrl setupSource(void) const final;
     QUrl summaryQmlSource(void) const final;
     bool allowSetupWhileArmed(void) const final { return true; }
+    bool allowSetupWhileFlying(void) const final { return true; }
 
 private:
     const QString   _name;

src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml

@@ -7,11 +7,16 @@
  *
  ****************************************************************************/
 
+import QtQuick          2.3
+import QtQuick.Controls 1.2
+import QtCharts         2.2
+import QtQuick.Layouts  1.2
 
-import QtQuick              2.3
-import QtQuick.Controls     1.2
-
-import QGroundControl.Controls  1.0
+import QGroundControl               1.0
+import QGroundControl.Controls      1.0
+import QGroundControl.FactSystem    1.0
+import QGroundControl.FactControls  1.0
+import QGroundControl.ScreenTools   1.0
 
 SetupPage {
     id:             tuningPage
@@ -20,47 +25,486 @@ SetupPage {
     Component {
         id: pageComponent
 
-        FactSliderPanel {
-            width:          availableWidth
-            qgcViewPanel:   tuningPage.viewPanel
-
-            sliderModel: ListModel {
-                ListElement {
-                    title:          qsTr("Hover Throttle")
-                    description:    qsTr("Adjust throttle so hover is at mid-throttle. Slide to the left if hover is lower than throttle center. Slide to the right if hover is higher than throttle center.")
-                    param:          "MPC_THR_HOVER"
-                    min:            20
-                    max:            80
-                    step:           1
+        Column {
+            width: availableWidth
+
+            property real   _chartHeight:       ScreenTools.defaultFontPixelHeight * 20
+            property real   _margins:           ScreenTools.defaultFontPixelHeight / 2
+            property string _currentTuneType:   _tuneList[0]
+            property real   _roll:              _activeVehicle.roll.value
+            property real   _rollSetpoint:      _activeVehicle.setpoint.roll.value
+            property real   _rollRate:          _activeVehicle.rollRate.value
+            property real   _rollRateSetpoint:  _activeVehicle.setpoint.rollRate.value
+            property real   _pitch:             _activeVehicle.pitch.value
+            property real   _pitchSetpoint:     _activeVehicle.setpoint.pitch.value
+            property real   _pitchRate:         _activeVehicle.pitchRate.value
+            property real   _pitchRateSetpoint: _activeVehicle.setpoint.pitchRate.value
+            property real   _yaw:               _activeVehicle.heading.value
+            property real   _yawSetpoint:       _activeVehicle.setpoint.yaw.value
+            property real   _yawRate:           _activeVehicle.yawRate.value
+            property real   _yawRateSetpoint:   _activeVehicle.setpoint.yawRate.value
+            property var    _valueXAxis:        valueXAxis
+            property var    _valueRateXAxis:    valueRateXAxis
+            property var    _valueYAxis:        valueYAxis
+            property var    _valueRateYAxis:    valueRateYAxis
+            property int    _msecs:             0
+            property var    _savedTuningParamValues:    [ ]
+
+            // The following are set when getValues is called
+            property real   _value
+            property real   _valueSetpoint
+            property real   _valueRate
+            property real   _valueRateSetpoint
+
+            readonly property int _tickSeparation:      5
+            readonly property int _tuneListRollIndex:   0
+            readonly property int _tuneListPitchIndex:  1
+            readonly property int _tuneListYawIndex:    2
+            readonly property var _tuneList:            [ qsTr("Roll"), qsTr("Pitch"), qsTr("Yaw") ]
+            readonly property var _params:              [
+                [ controller.getParameterFact(-1, "MC_ROLL_P"),
+                 controller.getParameterFact(-1, "MC_ROLLRATE_P"),
+                 controller.getParameterFact(-1, "MC_ROLLRATE_I"),
+                 controller.getParameterFact(-1, "MC_ROLLRATE_D"),
+                 controller.getParameterFact(-1, "MC_ROLLRATE_FF") ],
+                [ controller.getParameterFact(-1, "MC_PITCH_P"),
+                 controller.getParameterFact(-1, "MC_PITCHRATE_P"),
+                 controller.getParameterFact(-1, "MC_PITCHRATE_I"),
+                 controller.getParameterFact(-1, "MC_PITCHRATE_D"),
+                 controller.getParameterFact(-1, "MC_PITCHRATE_FF") ],
+                [ controller.getParameterFact(-1, "MC_YAW_P"),
+                 controller.getParameterFact(-1, "MC_YAWRATE_P"),
+                 controller.getParameterFact(-1, "MC_YAWRATE_I"),
+                 controller.getParameterFact(-1, "MC_YAWRATE_D"),
+                 controller.getParameterFact(-1, "MC_YAW_FF"),
+                 controller.getParameterFact(-1, "MC_YAWRATE_FF") ] ]
+
+            function adjustYAxisMin(yAxis, newValue) {
+                var newMin = Math.min(yAxis.min, newValue)
+                if (newMin % 5 != 0) {
+                    newMin -= 5
+                    newMin = Math.floor(newMin / _tickSeparation) * _tickSeparation
+                }
+                yAxis.min = newMin
+            }
+
+            function adjustYAxisMax(yAxis, newValue) {
+                var newMax = Math.max(yAxis.max, newValue)
+                if (newMax % 5 != 0) {
+                    newMax += 5
+                    newMax = Math.floor(newMax / _tickSeparation) * _tickSeparation
+                }
+                yAxis.max = newMax
+            }
+
+            function getValues() {
+                if (_currentTuneType === _tuneList[_tuneListRollIndex]) {
+                    _value = _roll
+                    _valueSetpoint = _rollSetpoint
+                    _valueRate = _rollRate
+                    _valueRateSetpoint = _rollRateSetpoint
+                } else if (_currentTuneType === _tuneList[_tuneListPitchIndex]) {
+                    _value = _pitch
+                    _valueSetpoint = _pitchSetpoint
+                    _valueRate = _pitchRate
+                    _valueRateSetpoint = _pitchRateSetpoint
+                } else if (_currentTuneType === _tuneList[_tuneListYawIndex]) {
+                    _value = _yaw
+                    _valueSetpoint = _yawSetpoint
+                    _valueRate = _yawRate
+                    _valueRateSetpoint = _yawRateSetpoint
+                }
+            }
+
+            function resetGraphs() {
+                valueSeries.removePoints(0, valueSeries.count)
+                valueSetpointSeries.removePoints(0, valueSetpointSeries.count)
+                valueRateSeries.removePoints(0, valueRateSeries.count)
+                valueRateSetpointSeries.removePoints(0, valueRateSetpointSeries.count)
+                _valueXAxis.min = 0
+                _valueXAxis.max = 0
+                _valueRateXAxis.min = 0
+                _valueRateXAxis.max = 0
+                _valueYAxis.min = 0
+                _valueYAxis.max = 10
+                _valueRateYAxis.min = 0
+                _valueRateYAxis.max = 10
+                _msecs = 0
+            }
+
+            function currentTuneTypeIndex() {
+                if (_currentTuneType === _tuneList[_tuneListRollIndex]) {
+                    return _tuneListRollIndex
+                } else if (_currentTuneType === _tuneList[_tuneListPitchIndex]) {
+                    return _tuneListPitchIndex
+                } else if (_currentTuneType === _tuneList[_tuneListYawIndex]) {
+                    return _tuneListYawIndex
                 }
+            }
+
+            // Save the current set of tuning values so we can reset to them
+            function saveTuningParamValues() {
+                var tuneTypeIndex = currentTuneTypeIndex()
 
-                ListElement {
-                    title:          qsTr("Manual minimum throttle")
-                    description:    qsTr("Slide to the left to start the motors with less idle power. Slide to the right if descending in manual flight becomes unstable.")
-                    param:          "MPC_MANTHR_MIN"
-                    min:            0
-                    max:            15
-                    step:           1
+                _savedTuningParamValues = [ ]
+                var currentTuneParams = _params[tuneTypeIndex]
+                for (var i=0; i<currentTuneParams.length; i++) {
+                    _savedTuningParamValues.push(currentTuneParams[i].valueString)
                 }
+                savedRepeater.model = _savedTuningParamValues
+            }
+
+            function resetToSavedTuningParamValues() {
+                var tuneTypeIndex = currentTuneTypeIndex()
 
-                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
+                for (var i=0; i<_savedTuningParamValues.length; i++) {
+                    _params[tuneTypeIndex][i].value = _savedTuningParamValues[i]
                 }
+            }
+
+            Component.onCompleted: {
+                showAdvanced = true
+                saveTuningParamValues()
+            }
+
+            on_CurrentTuneTypeChanged: {
+                saveTuningParamValues()
+                resetGraphs()
+            }
+
+            FactPanelController {
+                id:         controller
+                factPanel:  tuningPage.viewPanel
+            }
+
+            ExclusiveGroup {
+                id: tuneTypeRadios
+            }
+
+            ValueAxis {
+                id:             valueXAxis
+                min:            0
+                max:            0
+                labelFormat:    "%d"
+                titleText:      "sec"
+            }
+
+            ValueAxis {
+                id:             valueRateXAxis
+                min:            0
+                max:            0
+                labelFormat:    "%d"
+                titleText:      "sec"
+            }
+
+            ValueAxis {
+                id:         valueYAxis
+                min:        0
+                max:        10
+                titleText:  "deg"
+                tickCount:  ((max - min) / _tickSeparation) + 1
+            }
+
+            ValueAxis {
+                id:         valueRateYAxis
+                min:        0
+                max:        10
+                titleText:  "deg/s"
+                tickCount:  ((max - min) / _tickSeparation) + 1
+            }
+
+            Timer {
+                id:         dataTimer
+                interval:   50
+                running:    false
+                repeat:     true
+
+                onTriggered: {
+                    var seconds = _msecs / 1000
+                    _valueXAxis.max = seconds
+                    _valueRateXAxis.max = seconds
+
+                    getValues()
+
+                    valueSeries.append(seconds, _value)
+                    adjustYAxisMin(_valueYAxis, _value)
+                    adjustYAxisMax(_valueYAxis, _value)
+
+                    valueSetpointSeries.append(seconds, _valueSetpoint)
+                    adjustYAxisMin(_valueYAxis, _valueSetpoint)
+                    adjustYAxisMax(_valueYAxis, _valueSetpoint)
+
+                    valueRateSeries.append(seconds, _valueRate)
+                    adjustYAxisMin(_valueRateYAxis, _valueRate)
+                    adjustYAxisMax(_valueRateYAxis, _valueRate)
+
+                    valueRateSetpointSeries.append(seconds, _valueRateSetpoint)
+                    adjustYAxisMin(_valueRateYAxis, _valueRateSetpoint)
+                    adjustYAxisMax(_valueRateYAxis, _valueRateSetpoint)
+
+                    _msecs += interval
+                    if (valueSeries.count > _maxPointCount) {
+                        valueSeries.remove(0)
+                        valueSetpointSeries.remove(0)
+                        valueRateSeries.remove(0)
+                        valueRateSetpointSeries.remove(0)
+                        valueXAxis.min = valueSeries.at(0).x
+                        valueRateXAxis.min = valueSeries.at(0).x
+                    }
+                }
+
+                property var _activeVehicle:    QGroundControl.multiVehicleManager.activeVehicle
+                property int _maxPointCount:    10000 / interval
+            }
+
+            // Standard tuning page
+            FactSliderPanel {
+                width:          availableWidth
+                qgcViewPanel:   tuningPage.viewPanel
+                visible:        !advanced
+
+                sliderModel: ListModel {
+                    ListElement {
+                        title:          qsTr("Hover Throttle")
+                        description:    qsTr("Adjust throttle so hover is at mid-throttle. Slide to the left if hover is lower than throttle center. Slide to the right if hover is higher than throttle center.")
+                        param:          "MPC_THR_HOVER"
+                        min:            20
+                        max:            80
+                        step:           1
+                    }
 
-                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
+                    ListElement {
+                        title:          qsTr("Manual minimum throttle")
+                        description:    qsTr("Slide to the left to start the motors with less idle power. Slide to the right if descending in manual flight becomes unstable.")
+                        param:          "MPC_MANTHR_MIN"
+                        min:            0
+                        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
+                    }
+*/
                 }
             }
-        }
-    } // Component
+
+            // Advanced page
+            RowLayout {
+                anchors.left:       parent.left
+                anchors.right:      parent.right
+                layoutDirection:    Qt.RightToLeft
+                visible:            advanced
+
+                Column {
+                    spacing:            _margins
+                    Layout.alignment:   Qt.AlignTop
+
+                    QGCLabel { text: qsTr("Tuning Axis:") }
+
+                    RowLayout {
+                        spacing: _margins
+
+                        Repeater {
+                            model: _tuneList
+                            QGCRadioButton {
+                                text:           modelData
+                                checked:        _currentTuneType === modelData
+                                exclusiveGroup: tuneTypeRadios
+
+                                onClicked: _currentTuneType = modelData
+                            }
+                        }
+                    }
+
+                    Item { width: 1; height: 1 }
+
+                    QGCLabel { text: qsTr("Tuning Values:") }
+
+                    GridLayout {
+                        rows:           factList.length
+                        flow:           GridLayout.TopToBottom
+                        rowSpacing:     _margins
+                        columnSpacing:  _margins
+
+                        property var factList: _params[_tuneList.indexOf(_currentTuneType)]
+
+                        Repeater {
+                            model: parent.factList
+
+                            QGCLabel { text: modelData.name }
+                        }
+
+                        Repeater {
+                            model: parent.factList
+
+                            QGCButton {
+                                text: "-"
+                                onClicked: {
+                                    var value = modelData.value
+                                    modelData.value -= value * adjustPercentModel.get(adjustPercentCombo.currentIndex).value
+                                }
+                            }
+                        }
+
+                        Repeater {
+                            model: parent.factList
+
+                            FactTextField {
+                                Layout.fillWidth:   true
+                                fact:               modelData
+                                showUnits:          false
+                            }
+                        }
+
+                        Repeater {
+                            model: parent.factList
+
+                            QGCButton {
+                                text: "+"
+                                onClicked: {
+                                    var value = modelData.value
+                                    modelData.value += value * adjustPercentModel.get(adjustPercentCombo.currentIndex).value
+                                }
+                            }
+                        }
+                    }
+
+                    RowLayout {
+                        QGCLabel { text: qsTr("Increment/Decrement %") }
+
+                        QGCComboBox {
+                            id:     adjustPercentCombo
+                            model:  ListModel {
+                                id: adjustPercentModel
+                                ListElement { text: "5"; value: 0.05 }
+                                ListElement { text: "10"; value: 0.10 }
+                                ListElement { text: "15"; value: 0.15 }
+                                ListElement { text: "20"; value: 0.20 }
+                            }
+                        }
+                    }
+                    Item { width: 1; height: 1 }
+
+                    QGCLabel { text: qsTr("Saved Tuning Values:") }
+
+                    GridLayout {
+                        rows:           savedRepeater.model.length
+                        flow:           GridLayout.TopToBottom
+                        rowSpacing:     _margins
+                        columnSpacing:  _margins
+
+                        Repeater {
+                            model: _params[_tuneList.indexOf(_currentTuneType)]
+
+                            QGCLabel { text: modelData.name }
+                        }
+
+                        Repeater {
+                            id: savedRepeater
+
+                            QGCLabel { text: modelData }
+                        }
+                    }
+
+                    RowLayout {
+                        spacing: _margins
+
+                        QGCButton {
+                            text:       qsTr("Save Values")
+                            onClicked:  saveTuningParamValues()
+                        }
+
+                        QGCButton {
+                            text:       qsTr("Reset To Saved Values")
+                            onClicked:  resetToSavedTuningParamValues()
+                        }
+                    }
+
+                    Item { width: 1; height: 1 }
+
+                    QGCLabel { text: qsTr("Chart:") }
+
+                    RowLayout {
+                        spacing: _margins
+
+                        QGCButton {
+                            text:       qsTr("Clear")
+                            onClicked:  resetGraphs()
+                        }
+
+                        QGCButton {
+                            text:       dataTimer.running ? qsTr("Stop") : qsTr("Start")
+                            onClicked:  dataTimer.running = !dataTimer.running
+                        }
+                    }
+                }
+
+                Column {
+                    Layout.fillWidth: true
+
+                    ChartView {
+                        anchors.left:       parent.left
+                        anchors.right:      parent.right
+                        height:             availableHeight / 2
+                        title:              _currentTuneType
+                        antialiasing:       true
+                        legend.alignment:   Qt.AlignRight
+
+                        LineSeries {
+                            id:         valueSeries
+                            name:       "Response"
+                            axisY:      valueYAxis
+                            axisX:      valueXAxis
+                        }
+
+                        LineSeries {
+                            id:         valueSetpointSeries
+                            name:       "Command"
+                            axisY:      valueYAxis
+                            axisX:      valueXAxis
+                        }
+                    }
+
+                    ChartView {
+                        anchors.left:       parent.left
+                        anchors.right:      parent.right
+                        height:             availableHeight / 2
+                        title:              _currentTuneType + qsTr(" Rate")
+                        antialiasing:       true
+                        legend.alignment:   Qt.AlignRight
+
+                        LineSeries {
+                            id:         valueRateSeries
+                            name:       "Response"
+                            axisY:      valueRateYAxis
+                            axisX:      valueRateXAxis
+                        }
+
+                        LineSeries {
+                            id:         valueRateSetpointSeries
+                            name:       "Command"
+                            axisY:      valueRateYAxis
+                            axisX:      valueRateXAxis
+                        }
+                    }
+                }
+            } // RowLayout - Advanced Page
+        } // Column
+    } // Component - pageComponent
 } // SetupPage

src/Vehicle/SetpointFact.json

+[
+{
+    "name":             "roll",
+    "shortDescription": "Roll Setpoint",
+    "type":             "double",
+    "decimalPlaces":    1,
+    "units":            "deg"
+},
+{
+    "name":             "pitch",
+    "shortDescription": "Pitch Setpoint",
+    "type":             "double",
+    "decimalPlaces":    1,
+    "units":            "deg"
+},
+{
+    "name":             "yaw",
+    "shortDescription": "Yaw Setpoint",
+    "type":             "double",
+    "decimalPlaces":    1,
+    "units":            "deg"
+},
+{
+    "name":             "rollRate",
+    "shortDescription": "Roll Rate Setpoint",
+    "type":             "double",
+    "decimalPlaces":    1,
+    "units":            "deg/s"
+},
+{
+    "name":             "pitchRate",
+    "shortDescription": "Pitch Rate Setpoint",
+    "type":             "double",
+    "decimalPlaces":    1,
+    "units":            "deg/s"
+},
+{
+    "name":             "yawRate",
+    "shortDescription": "Yaw Rate Setpoint",
+    "type":             "double",
+    "decimalPlaces":    1,
+    "units":            "deg/s"
+}
+]

src/Vehicle/Vehicle.cc

@@ -10,6 +10,7 @@
 #include <QTime>
 #include <QDateTime>
 #include <QLocale>
+#include <QQuaternion>
 
 #include "Vehicle.h"
 #include "MAVLinkProtocol.h"
@@ -54,6 +55,9 @@ const char* Vehicle::_joystickEnabledSettingsKey =  "JoystickEnabled";
 const char* Vehicle::_rollFactName =                "roll";
 const char* Vehicle::_pitchFactName =               "pitch";
 const char* Vehicle::_headingFactName =             "heading";
+const char* Vehicle::_rollRateFactName =             "rollRate";
+const char* Vehicle::_pitchRateFactName =           "pitchRate";
+const char* Vehicle::_yawRateFactName =             "yawRate";
 const char* Vehicle::_airSpeedFactName =            "airSpeed";
 const char* Vehicle::_groundSpeedFactName =         "groundSpeed";
 const char* Vehicle::_climbRateFactName =           "climbRate";
@@ -166,6 +170,9 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
     , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
     , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
+    , _rollRateFact         (0, _rollRateFactName,          FactMetaData::valueTypeDouble)
+    , _pitchRateFact        (0, _pitchRateFactName,         FactMetaData::valueTypeDouble)
+    , _yawRateFact          (0, _yawRateFactName,           FactMetaData::valueTypeDouble)
     , _groundSpeedFact      (0, _groundSpeedFactName,       FactMetaData::valueTypeDouble)
     , _airSpeedFact         (0, _airSpeedFactName,          FactMetaData::valueTypeDouble)
     , _climbRateFact        (0, _climbRateFactName,         FactMetaData::valueTypeDouble)
@@ -353,6 +360,9 @@ Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
     , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
     , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
     , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
+    , _rollRateFact         (0, _rollRateFactName,          FactMetaData::valueTypeDouble)
+    , _pitchRateFact        (0, _pitchRateFactName,         FactMetaData::valueTypeDouble)
+    , _yawRateFact          (0, _yawRateFactName,           FactMetaData::valueTypeDouble)
     , _groundSpeedFact      (0, _groundSpeedFactName,       FactMetaData::valueTypeDouble)
     , _airSpeedFact         (0, _airSpeedFactName,          FactMetaData::valueTypeDouble)
     , _climbRateFact        (0, _climbRateFactName,         FactMetaData::valueTypeDouble)
@@ -413,6 +423,9 @@ void Vehicle::_commonInit(void)
     _addFact(&_rollFact,                _rollFactName);
     _addFact(&_pitchFact,               _pitchFactName);
     _addFact(&_headingFact,             _headingFactName);
+    _addFact(&_rollRateFact,            _rollRateFactName);
+    _addFact(&_pitchRateFact,           _pitchRateFactName);
+    _addFact(&_yawRateFact,             _yawRateFactName);
     _addFact(&_groundSpeedFact,         _groundSpeedFactName);
     _addFact(&_airSpeedFact,            _airSpeedFactName);
     _addFact(&_climbRateFact,           _climbRateFactName);
@@ -696,6 +709,12 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_HIGH_LATENCY2:
         _handleHighLatency2(message);
         break;
+    case MAVLINK_MSG_ID_ATTITUDE:
+        _handleAttitude(message);
+        break;
+    case MAVLINK_MSG_ID_ATTITUDE_TARGET:
+        _handleAttitudeTarget(message);
+        break;
 
     case MAVLINK_MSG_ID_SERIAL_CONTROL:
     {
@@ -760,6 +779,35 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message)
     _climbRateFact.setRawValue(qIsNaN(vfrHud.climb) ? 0 : vfrHud.climb);
 }
 
+void Vehicle::_handleAttitudeTarget(mavlink_message_t& message)
+{
+    mavlink_attitude_target_t attitudeTarget;
+
+    mavlink_msg_attitude_target_decode(&message, &attitudeTarget);
+
+    float roll, pitch, yaw;
+    mavlink_quaternion_to_euler(attitudeTarget.q, &roll, &pitch, &yaw);
+
+    _setpointFactGroup.roll()->setRawValue(roll);
+    _setpointFactGroup.pitch()->setRawValue(pitch);
+    _setpointFactGroup.yaw()->setRawValue(yaw);
+
+    _setpointFactGroup.rollRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_roll_rate));
+    _setpointFactGroup.pitchRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_pitch_rate));
+    _setpointFactGroup.yawRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_yaw_rate));
+}
+
+void Vehicle::_handleAttitude(mavlink_message_t& message)
+{
+    mavlink_attitude_t attitude;
+
+    mavlink_msg_attitude_decode(&message, &attitude);
+
+    rollRate()->setRawValue(qRadiansToDegrees(attitude.rollspeed));
+    pitchRate()->setRawValue(qRadiansToDegrees(attitude.pitchspeed));
+    yawRate()->setRawValue(qRadiansToDegrees(attitude.yawspeed));
+}
+
 void Vehicle::_handleGpsRawInt(mavlink_message_t& message)
 {
     mavlink_gps_raw_int_t gpsRawInt;
@@ -869,18 +917,6 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message)
         { HL_FAILURE_FLAG_3D_ACCEL,                 MAV_SYS_STATUS_SENSOR_3D_ACCEL },
         { HL_FAILURE_FLAG_3D_GYRO,                  MAV_SYS_STATUS_SENSOR_3D_GYRO },
         { HL_FAILURE_FLAG_3D_MAG,                   MAV_SYS_STATUS_SENSOR_3D_MAG },
-#if 0
-        // FIXME: These don't currently map to existing sensor health bits. Support needs to be added to show them
-        // on health page of instrument panel as well.
-        { HL_FAILURE_FLAG_TERRAIN=64, /* Terrain subsystem failure. | */
-        { HL_FAILURE_FLAG_BATTERY=128, /* Battery failure/critical low battery. | */
-        { HL_FAILURE_FLAG_RC_RECEIVER=256, /* RC receiver failure/no rc connection. | */
-        { HL_FAILURE_FLAG_OFFBOARD_LINK=512, /* Offboard link failure. | */
-        { HL_FAILURE_FLAG_ENGINE=1024, /* Engine failure. | */
-        { HL_FAILURE_FLAG_GEOFENCE=2048, /* Geofence violation. | */
-        { HL_FAILURE_FLAG_ESTIMATOR=4096, /* Estimator failure, for example measurement rejection or large variances. | */
-        { HL_FAILURE_FLAG_MISSION=8192, /* Mission failure. | */
-#endif
     };
 
     // Map from MAV_FAILURE bits to standard SYS_STATUS message handling
@@ -3217,7 +3253,6 @@ VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent)
     _zAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
 }
 
-
 const char* VehicleTemperatureFactGroup::_temperature1FactName =      "temperature1";
 const char* VehicleTemperatureFactGroup::_temperature2FactName =      "temperature2";
 const char* VehicleTemperatureFactGroup::_temperature3FactName =      "temperature3";
@@ -3261,3 +3296,35 @@ void VehicleClockFactGroup::_updateAllValues(void)
 
     FactGroup::_updateAllValues();
 }
+
+const char* VehicleSetpointFactGroup::_rollFactName =       "roll";
+const char* VehicleSetpointFactGroup::_pitchFactName =      "pitch";
+const char* VehicleSetpointFactGroup::_yawFactName =        "yaw";
+const char* VehicleSetpointFactGroup::_rollRateFactName =   "rollRate";
+const char* VehicleSetpointFactGroup::_pitchRateFactName =  "pitchRate";
+const char* VehicleSetpointFactGroup::_yawRateFactName =    "yawRate";
+
+VehicleSetpointFactGroup::VehicleSetpointFactGroup(QObject* parent)
+    : FactGroup     (1000, ":/json/Vehicle/SetpointFact.json", parent)
+    , _rollFact     (0, _rollFactName,      FactMetaData::valueTypeDouble)
+    , _pitchFact    (0, _pitchFactName,     FactMetaData::valueTypeDouble)
+    , _yawFact      (0, _yawFactName,       FactMetaData::valueTypeDouble)
+    , _rollRateFact (0, _rollRateFactName,  FactMetaData::valueTypeDouble)
+    , _pitchRateFact(0, _pitchRateFactName, FactMetaData::valueTypeDouble)
+    , _yawRateFact  (0, _yawRateFactName,   FactMetaData::valueTypeDouble)
+{
+    _addFact(&_rollFact,        _rollFactName);
+    _addFact(&_pitchFact,       _pitchFactName);
+    _addFact(&_yawFact,         _yawFactName);
+    _addFact(&_rollRateFact,    _rollRateFactName);
+    _addFact(&_pitchRateFact,   _pitchRateFactName);
+    _addFact(&_yawRateFact,     _yawRateFactName);
+
+    // Start out as not available "--.--"
+    _rollFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _pitchFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _yawFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _rollRateFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _pitchRateFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _yawRateFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+}

src/Vehicle/Vehicle.h

@@ -40,6 +40,43 @@ Q_DECLARE_LOGGING_CATEGORY(VehicleLog)
 
 class Vehicle;
 
+class VehicleSetpointFactGroup : public FactGroup
+{
+    Q_OBJECT
+
+public:
+    VehicleSetpointFactGroup(QObject* parent = NULL);
+
+    Q_PROPERTY(Fact* roll       READ roll       CONSTANT)
+    Q_PROPERTY(Fact* pitch      READ pitch      CONSTANT)
+    Q_PROPERTY(Fact* yaw        READ yaw        CONSTANT)
+    Q_PROPERTY(Fact* rollRate   READ rollRate   CONSTANT)
+    Q_PROPERTY(Fact* pitchRate  READ pitchRate  CONSTANT)
+    Q_PROPERTY(Fact* yawRate    READ yawRate    CONSTANT)
+
+    Fact* roll      (void) { return &_rollFact; }
+    Fact* pitch     (void) { return &_pitchFact; }
+    Fact* yaw       (void) { return &_yawFact; }
+    Fact* rollRate  (void) { return &_rollRateFact; }
+    Fact* pitchRate (void) { return &_pitchRateFact; }
+    Fact* yawRate   (void) { return &_yawRateFact; }
+
+    static const char* _rollFactName;
+    static const char* _pitchFactName;
+    static const char* _yawFactName;
+    static const char* _rollRateFactName;
+    static const char* _pitchRateFactName;
+    static const char* _yawRateFactName;
+
+private:
+    Fact _rollFact;
+    Fact _pitchFact;
+    Fact _yawFact;
+    Fact _rollRateFact;
+    Fact _pitchRateFact;
+    Fact _yawRateFact;
+};
+
 class VehicleVibrationFactGroup : public FactGroup
 {
     Q_OBJECT
@@ -370,6 +407,9 @@ public:
     Q_PROPERTY(Fact* roll               READ roll               CONSTANT)
     Q_PROPERTY(Fact* pitch              READ pitch              CONSTANT)
     Q_PROPERTY(Fact* heading            READ heading            CONSTANT)
+    Q_PROPERTY(Fact* rollRate           READ rollRate           CONSTANT)
+    Q_PROPERTY(Fact* pitchRate          READ pitchRate          CONSTANT)
+    Q_PROPERTY(Fact* yawRate            READ yawRate            CONSTANT)
     Q_PROPERTY(Fact* groundSpeed        READ groundSpeed        CONSTANT)
     Q_PROPERTY(Fact* airSpeed           READ airSpeed           CONSTANT)
     Q_PROPERTY(Fact* climbRate          READ climbRate          CONSTANT)
@@ -385,6 +425,7 @@ public:
     Q_PROPERTY(FactGroup* vibration   READ vibrationFactGroup   CONSTANT)
     Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT)
     Q_PROPERTY(FactGroup* clock       READ clockFactGroup       CONSTANT)
+    Q_PROPERTY(FactGroup* setpoint    READ setpointFactGroup    CONSTANT)
 
     Q_PROPERTY(int      firmwareMajorVersion        READ firmwareMajorVersion       NOTIFY firmwareVersionChanged)
     Q_PROPERTY(int      firmwareMinorVersion        READ firmwareMinorVersion       NOTIFY firmwareVersionChanged)
@@ -645,8 +686,11 @@ public:
     unsigned        maxProtoVersion         () const { return _maxProtoVersion; }
 
     Fact* roll              (void) { return &_rollFact; }
-    Fact* heading           (void) { return &_headingFact; }
     Fact* pitch             (void) { return &_pitchFact; }
+    Fact* heading           (void) { return &_headingFact; }
+    Fact* rollRate          (void) { return &_rollRateFact; }
+    Fact* pitchRate         (void) { return &_pitchRateFact; }
+    Fact* yawRate           (void) { return &_yawRateFact; }
     Fact* airSpeed          (void) { return &_airSpeedFact; }
     Fact* groundSpeed       (void) { return &_groundSpeedFact; }
     Fact* climbRate         (void) { return &_climbRateFact; }
@@ -662,6 +706,7 @@ public:
     FactGroup* vibrationFactGroup   (void) { return &_vibrationFactGroup; }
     FactGroup* temperatureFactGroup (void) { return &_temperatureFactGroup; }
     FactGroup* clockFactGroup       (void) { return &_clockFactGroup; }
+    FactGroup* setpointFactGroup    (void) { return &_setpointFactGroup; }
 
     void setConnectionLostEnabled(bool connectionLostEnabled);
 
@@ -916,6 +961,8 @@ private:
     void _handleScaledPressure2(mavlink_message_t& message);
     void _handleScaledPressure3(mavlink_message_t& message);
     void _handleHighLatency2(mavlink_message_t& message);
+    void _handleAttitude(mavlink_message_t& message);
+    void _handleAttitudeTarget(mavlink_message_t& message);
     // ArduPilot dialect messages
 #if !defined(NO_ARDUPILOT_DIALECT)
     void _handleCameraFeedback(const mavlink_message_t& message);
@@ -1106,6 +1153,9 @@ private:
     Fact _rollFact;
     Fact _pitchFact;
     Fact _headingFact;
+    Fact _rollRateFact;
+    Fact _pitchRateFact;
+    Fact _yawRateFact;
     Fact _groundSpeedFact;
     Fact _airSpeedFact;
     Fact _climbRateFact;
@@ -1122,10 +1172,14 @@ private:
     VehicleVibrationFactGroup   _vibrationFactGroup;
     VehicleTemperatureFactGroup _temperatureFactGroup;
     VehicleClockFactGroup       _clockFactGroup;
+    VehicleSetpointFactGroup    _setpointFactGroup;
 
     static const char* _rollFactName;
     static const char* _pitchFactName;
     static const char* _headingFactName;
+    static const char* _rollRateFactName;
+    static const char* _pitchRateFactName;
+    static const char* _yawRateFactName;
     static const char* _groundSpeedFactName;
     static const char* _airSpeedFactName;
     static const char* _climbRateFactName;

src/Vehicle/VehicleFact.json

@@ -20,6 +20,27 @@
     "decimalPlaces":    0,
     "units":            "deg"
 },
+{
+    "name":             "rollRate",
+    "shortDescription": "Roll Rate",
+    "type":             "double",
+    "decimalPlaces":    1,
+    "units":            "deg/s"
+},
+{
+    "name":             "pitchRate",
+    "shortDescription": "Pitch Rate",
+    "type":             "double",
+    "decimalPlaces":    1,
+    "units":            "deg/s"
+},
+{
+    "name":             "yawRate",
+    "shortDescription": "Yaw Rate",
+    "type":             "double",
+    "decimalPlaces":    1,
+    "units":            "deg/s"
+},
 {
     "name":             "groundSpeed",
     "shortDescription": "Ground Speed",