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/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",