Add support for mavlink terrain frame

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -102,6 +102,7 @@ public:
     QObject*            loadParameterMetaData           (const QString& metaDataFile) override;
     QString             brandImageIndoor                (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
     QString             brandImageOutdoor               (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
+    bool                supportsTerrainFrame            (void) const override { return true; }
 
 protected:
     /// All access to singleton is through stack specific implementation

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -150,6 +150,12 @@ bool FirmwarePlugin::supportsJSButton(void)
     return false;
 }
 
+bool FirmwarePlugin::supportsTerrainFrame(void) const
+{
+    // Generic firmware supports this since we don't know
+    return true;
+}
+
 bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
 {
     Q_UNUSED(vehicle);

src/FirmwarePlugin/FirmwarePlugin.h

@@ -180,6 +180,9 @@ public:
     /// (CompassMot). Default is true.
     virtual bool supportsMotorInterference(void);
 
+    /// Returns true if the firmware supports MAV_FRAME_GLOBAL_TERRAIN_ALT
+    virtual bool supportsTerrainFrame(void) const;
+
     /// Called before any mavlink message is processed by Vehicle such that the firmwre plugin
     /// can adjust any message characteristics. This is handy to adjust or differences in mavlink
     /// spec implementations such that the base code can remain mavlink generic.

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -71,6 +71,7 @@ public:
     QGCCameraManager*   createCameraManager             (Vehicle* vehicle) override;
     QGCCameraControl*   createCameraControl             (const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = NULL) override;
     uint32_t            highLatencyCustomModeTo32Bits   (uint16_t hlCustomMode) override;
+    bool                supportsTerrainFrame            (void) const override { return false; }
 
 protected:
     typedef struct {

src/MissionManager/SimpleMissionItem.cc

@@ -80,12 +80,6 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, QObject* parent)
 
     setDefaultsForCommand();
     _rebuildFacts();
-
-    connect(&_missionItem, &MissionItem::specifiedFlightSpeedChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged);
-
-    connect(this, &SimpleMissionItem::sequenceNumberChanged,        this, &SimpleMissionItem::lastSequenceNumberChanged);
-    connect(this, &SimpleMissionItem::cameraSectionChanged,         this, &SimpleMissionItem::_setDirty);
-    connect(this, &SimpleMissionItem::cameraSectionChanged,         this, &SimpleMissionItem::_updateLastSequenceNumber);
 }
 
 SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool editMode, const MissionItem& missionItem, QObject* parent)
@@ -98,7 +92,6 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool editMode, const Miss
     , _cameraSection            (NULL)
     , _commandTree              (qgcApp()->toolbox()->missionCommandTree())
     , _supportedCommandFact     (0,         "Command:",             FactMetaData::valueTypeUint32)
-    , _altitudeMode             (missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAMSL)
     , _altitudeFact             (0,         "Altitude",             FactMetaData::valueTypeDouble)
     , _amslAltAboveTerrainFact  (0,         "Alt above terrain",    FactMetaData::valueTypeDouble)
     , _param1MetaData           (FactMetaData::valueTypeDouble)
@@ -112,6 +105,25 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool editMode, const Miss
 {
     _editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml");
 
+    struct MavFrame2AltMode_s {
+        MAV_FRAME       mavFrame;
+        AltitudeMode    altMode;
+    };
+
+    const struct MavFrame2AltMode_s rgMavFrame2AltMode[] = {
+        { MAV_FRAME_GLOBAL_TERRAIN_ALT,     AltitudeTerrainFrame },
+        { MAV_FRAME_GLOBAL,                 AltitudeAbsolute },
+        { MAV_FRAME_GLOBAL_RELATIVE_ALT,    AltitudeRelative },
+    };
+    _altitudeMode = AltitudeRelative;
+    for (size_t i=0; i<sizeof(rgMavFrame2AltMode)/sizeof(rgMavFrame2AltMode[0]); i++) {
+        const MavFrame2AltMode_s& pMavFrame2AltMode = rgMavFrame2AltMode[i];
+        if (pMavFrame2AltMode.mavFrame == missionItem.frame()) {
+            _altitudeMode = pMavFrame2AltMode.altMode;
+            break;
+        }
+    }
+
     _isCurrentItem = missionItem.isCurrentItem();
     _altitudeFact.setRawValue(specifiesCoordinate() ? _missionItem._param7Fact.rawValue() : qQNaN());
     _amslAltAboveTerrainFact.setRawValue(qQNaN());
@@ -176,12 +188,14 @@ void SimpleMissionItem::_connectSignals(void)
     connect(&_missionItem,              &MissionItem::sequenceNumberChanged,    this, &SimpleMissionItem::_setDirty);
     connect(this,                       &SimpleMissionItem::altitudeModeChanged,this, &SimpleMissionItem::_setDirty);
 
-    // These changes may need to trigger terrain queries
     connect(&_altitudeFact,             &Fact::valueChanged,                    this, &SimpleMissionItem::_altitudeChanged);
     connect(this,                       &SimpleMissionItem::altitudeModeChanged,this, &SimpleMissionItem::_altitudeModeChanged);
-
     connect(this,                       &SimpleMissionItem::terrainAltitudeChanged,this, &SimpleMissionItem::_terrainAltChanged);
 
+    connect(this, &SimpleMissionItem::sequenceNumberChanged,        this, &SimpleMissionItem::lastSequenceNumberChanged);
+    connect(this, &SimpleMissionItem::cameraSectionChanged,         this, &SimpleMissionItem::_setDirty);
+    connect(this, &SimpleMissionItem::cameraSectionChanged,         this, &SimpleMissionItem::_updateLastSequenceNumber);
+
     // These are coordinate parameters, they must emit coordinateChanged signal
     connect(&_missionItem._param5Fact,  &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged);
     connect(&_missionItem._param6Fact,  &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged);
@@ -208,8 +222,12 @@ void SimpleMissionItem::_connectSignals(void)
     // These fact signals must alway signal out through SimpleMissionItem signals
     connect(&_missionItem._commandFact,     &Fact::valueChanged, this, &SimpleMissionItem::_sendCommandChanged);
 
-    // Sequence number is kept in mission iteem, so we need to propagate signal up as well
+    // Propogate signals from MissionItem up to SimpleMissionItem
     connect(&_missionItem, &MissionItem::sequenceNumberChanged,         this, &SimpleMissionItem::sequenceNumberChanged);
+    connect(&_missionItem, &MissionItem::specifiedFlightSpeedChanged,   this, &SimpleMissionItem::specifiedFlightSpeedChanged);
+
+    // Firmware type change can affect supportsTerrainFrame
+    connect(_vehicle, &Vehicle::firmwareTypeChanged, this, &SimpleMissionItem::supportsTerrainFrameChanged);
 }
 
 void SimpleMissionItem::_setupMetaData(void)
@@ -295,7 +313,7 @@ bool SimpleMissionItem::load(QTextStream &loadStream)
     bool success;
     if ((success = _missionItem.load(loadStream))) {
         if (specifiesCoordinate()) {
-            _altitudeMode = _missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAMSL;
+            _altitudeMode = _missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAbsolute;
             _altitudeFact.setRawValue(_missionItem._param7Fact.rawValue());
             _amslAltAboveTerrainFact.setRawValue(qQNaN());
         }
@@ -325,7 +343,7 @@ bool SimpleMissionItem::load(const QJsonObject& json, int sequenceNumber, QStrin
             _altitudeFact.setRawValue(JsonHelper::possibleNaNJsonValue(json[_jsonAltitudeKey]));
             _amslAltAboveTerrainFact.setRawValue(JsonHelper::possibleNaNJsonValue(json[_jsonAltitudeKey]));
         } else {
-            _altitudeMode = _missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAMSL;
+            _altitudeMode = _missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAbsolute;
             _altitudeFact.setRawValue(_missionItem._param7Fact.rawValue());
             _amslAltAboveTerrainFact.setRawValue(qQNaN());
         }
@@ -585,6 +603,8 @@ bool SimpleMissionItem::friendlyEditAllowed(void) const
                 return true;
                 break;
 
+            case MAV_FRAME_GLOBAL_TERRAIN_ALT:
+                return supportsTerrainFrame();
             default:
                 return false;
             }
@@ -636,21 +656,27 @@ void SimpleMissionItem::_sendCoordinateChanged(void)
 void SimpleMissionItem::_altitudeModeChanged(void)
 {
     switch (_altitudeMode) {
+    case AltitudeTerrainFrame:
+        _missionItem.setFrame(MAV_FRAME_GLOBAL_TERRAIN_ALT);
+        break;
     case AltitudeAboveTerrain:
-        // Terrain altitudes are AMSL
+        // Terrain altitudes are Absolute
         _missionItem.setFrame(MAV_FRAME_GLOBAL);
-        // Clear any old values
+        // Clear any old calculated values
         _missionItem._param7Fact.setRawValue(qQNaN());
         _amslAltAboveTerrainFact.setRawValue(qQNaN());
-        _altitudeChanged();
         break;
-    case AltitudeAMSL:
+    case AltitudeAbsolute:
         _missionItem.setFrame(MAV_FRAME_GLOBAL);
         break;
     case AltitudeRelative:
         _missionItem.setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT);
         break;
     }
+
+    // We always call _altitudeChanged to make sure that param7 is always setup correctly on mode change
+    _altitudeChanged();
+
     emit coordinateHasRelativeAltitudeChanged(_altitudeMode == AltitudeRelative);
 }
 

src/MissionManager/SimpleMissionItem.h

@@ -30,9 +30,10 @@ public:
     ~SimpleMissionItem();
 
     enum AltitudeMode {
-        AltitudeRelative,
-        AltitudeAMSL,
-        AltitudeAboveTerrain
+        AltitudeRelative,       // MAV_FRAME_GLOBAL_RELATIVE_ALT
+        AltitudeAbsolute,       // MAV_FRAME_GLOBAL
+        AltitudeAboveTerrain,   // Absolute altitude above terrain calculated from terrain data
+        AltitudeTerrainFrame    // MAV_FRAME_GLOBAL_TERRAIN_ALT
     };
 
     Q_ENUM(AltitudeMode)
@@ -45,6 +46,7 @@ public:
     Q_PROPERTY(AltitudeMode     altitudeMode            READ altitudeMode           WRITE setAltitudeMode       NOTIFY altitudeModeChanged)
     Q_PROPERTY(Fact*            amslAltAboveTerrain     READ amslAltAboveTerrain                                CONSTANT)                           ///< Actual AMSL altitude for item if altitudeMode == AltitudeAboveTerrain
     Q_PROPERTY(int              command                 READ command                WRITE setCommand            NOTIFY commandChanged)
+    Q_PROPERTY(bool             supportsTerrainFrame    READ supportsTerrainFrame                               NOTIFY supportsTerrainFrameChanged)
 
     /// Optional sections
     Q_PROPERTY(QObject*         speedSection            READ speedSection                                       NOTIFY speedSectionChanged)
@@ -72,6 +74,7 @@ public:
     AltitudeMode    altitudeMode        (void) const { return _altitudeMode; }
     Fact*           altitude            (void) { return &_altitudeFact; }
     Fact*           amslAltAboveTerrain (void) { return &_amslAltAboveTerrainFact; }
+    bool            supportsTerrainFrame(void) const { return _vehicle->supportsTerrainFrame(); }
 
     CameraSection*  cameraSection       (void) { return _cameraSection; }
     SpeedSection*   speedSection        (void) { return _speedSection; }
@@ -141,6 +144,7 @@ signals:
     void cameraSectionChanged       (QObject* cameraSection);
     void speedSectionChanged        (QObject* cameraSection);
     void altitudeModeChanged        (void);
+    void supportsTerrainFrameChanged(void);
 
 private slots:
     void _setDirty                      (void);

src/MissionManager/SimpleMissionItemTest.cc

@@ -53,11 +53,11 @@ const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesDoJ
 const SimpleMissionItemTest::ItemExpected_t SimpleMissionItemTest::_rgItemExpected[] = {
     // Text field facts count                                                                                                   Fact Values                                         Altitude    Altitude Mode
     { sizeof(SimpleMissionItemTest::_rgFactValuesWaypoint)/sizeof(SimpleMissionItemTest::_rgFactValuesWaypoint[0]),             SimpleMissionItemTest::_rgFactValuesWaypoint,       70.1234567, SimpleMissionItem::AltitudeRelative },
-    { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim[0]),       SimpleMissionItemTest::_rgFactValuesLoiterUnlim,    70.1234567, SimpleMissionItem::AltitudeAMSL },
+    { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim[0]),       SimpleMissionItemTest::_rgFactValuesLoiterUnlim,    70.1234567, SimpleMissionItem::AltitudeAbsolute },
     { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTurns)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTurns[0]),       SimpleMissionItemTest::_rgFactValuesLoiterTurns,    70.1234567, SimpleMissionItem::AltitudeRelative },
-    { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime[0]),         SimpleMissionItemTest::_rgFactValuesLoiterTime,     70.1234567, SimpleMissionItem::AltitudeAMSL },
+    { sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime[0]),         SimpleMissionItemTest::_rgFactValuesLoiterTime,     70.1234567, SimpleMissionItem::AltitudeAbsolute },
     { 0,                                                                                                                        NULL,                                               70.1234567, SimpleMissionItem::AltitudeRelative },
-    { sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff)/sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff[0]),               SimpleMissionItemTest::_rgFactValuesTakeoff,        70.1234567, SimpleMissionItem::AltitudeAMSL },
+    { sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff)/sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff[0]),               SimpleMissionItemTest::_rgFactValuesTakeoff,        70.1234567, SimpleMissionItem::AltitudeAbsolute },
     { sizeof(SimpleMissionItemTest::_rgFactValuesDoJump)/sizeof(SimpleMissionItemTest::_rgFactValuesDoJump[0]),                 SimpleMissionItemTest::_rgFactValuesDoJump,         qQNaN(),    SimpleMissionItem::AltitudeRelative },
 };
 
@@ -222,7 +222,7 @@ void SimpleMissionItemTest::_testSignals(void)
     QVERIFY(_spyVisualItem->checkOnlySignalByMask(dirtyChangedMask));
     _spyVisualItem->clearAllSignals();
 
-    _simpleItem->setAltitudeMode(_simpleItem->altitudeMode() == SimpleMissionItem::AltitudeRelative ? SimpleMissionItem::AltitudeAMSL : SimpleMissionItem::AltitudeRelative);
+    _simpleItem->setAltitudeMode(_simpleItem->altitudeMode() == SimpleMissionItem::AltitudeRelative ? SimpleMissionItem::AltitudeAbsolute : SimpleMissionItem::AltitudeRelative);
     QVERIFY(_spySimpleItem->checkOnlySignalByMask(dirtyChangedMask | friendlyEditAllowedChangedMask | altitudeModeChangedMask | coordinateHasRelativeAltitudeChangedMask));
     _spySimpleItem->clearAllSignals();
     _spyVisualItem->clearAllSignals();
@@ -314,7 +314,7 @@ void SimpleMissionItemTest::_testAltitudePropogation(void)
     QCOMPARE(_simpleItem->altitude()->rawValue().toDouble(), _simpleItem->missionItem().param7());
     QCOMPARE(_simpleItem->missionItem().frame(), MAV_FRAME_GLOBAL_RELATIVE_ALT);
 
-    _simpleItem->setAltitudeMode(SimpleMissionItem::AltitudeAMSL);
+    _simpleItem->setAltitudeMode(SimpleMissionItem::AltitudeAbsolute);
     _simpleItem->altitude()->setRawValue(_simpleItem->altitude()->rawValue().toDouble() + 1);
     QCOMPARE(_simpleItem->altitude()->rawValue().toDouble(), _simpleItem->missionItem().param7());
     QCOMPARE(_simpleItem->missionItem().frame(), MAV_FRAME_GLOBAL);

src/PlanView/SimpleItemEditor.qml

@@ -18,8 +18,13 @@ Rectangle {
     radius: _radius
 
     property bool _specifiesAltitude:       missionItem.specifiesAltitude
-    property bool _altModeIsTerrain:    missionItem.altitudeMode === 2
     property real _margin:                  ScreenTools.defaultFontPixelHeight / 2
+    property bool _supportsTerrainFrame:    missionItem
+
+    readonly property int _altModeRelative:     0
+    readonly property int _altModeAbsolute:     1
+    readonly property int _altModeAboveTerrain: 2
+    readonly property int _altModeTerrainFrame: 3
 
     ExclusiveGroup {
         id: altRadios
@@ -93,9 +98,10 @@ Rectangle {
                 }
 
                 RowLayout {
-                    QGCRadioButton { text: qsTr("Rel"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: 0 }
-                    QGCRadioButton { text: qsTr("Abs"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: 1 }
-                    QGCRadioButton { text: qsTr("AGL"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: 2 }
+                    QGCRadioButton { text: qsTr("Rel"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: _altModeRelative }
+                    QGCRadioButton { text: qsTr("Abs"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: _altModeAbsolute }
+                    QGCRadioButton { text: qsTr("AGL"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: _altModeAboveTerrain }
+                    QGCRadioButton { text: qsTr("TerrF"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; visible: missionItem.supportsTerrainFrame; readonly property int value: _altModeTerrainFrame }
                 }
 
                 FactValueSlider {
@@ -107,17 +113,22 @@ Rectangle {
 
                 RowLayout {
                     spacing: _margin
+                    visible: missionItem.altitudeMode === _altModeAboveTerrain
 
                     QGCLabel {
                         text:           qsTr("Calculated Abs Alt")
                         font.pointSize: ScreenTools.smallFontPointSize
-                        visible:        _altModeIsTerrain
                     }
                     QGCLabel {
                         text:       missionItem.amslAltAboveTerrain.valueString + " " + missionItem.amslAltAboveTerrain.units
-                        visible:    _altModeIsTerrain
                     }
                 }
+
+                QGCLabel {
+                    text:           qsTr("Using terrain reference frame")
+                    font.pointSize: ScreenTools.smallFontPointSize
+                    visible:        missionItem.altitudeMode === _altModeTerrainFrame
+                }
             }
         }
 

src/Vehicle/Vehicle.cc

@@ -494,6 +494,7 @@ void Vehicle::prepareDelete()
 void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value)
 {
     _firmwareType = static_cast<MAV_AUTOPILOT>(value.toInt());
+    _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
     emit firmwareTypeChanged();
     if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
         _capabilityBits = 0;
@@ -2452,6 +2453,11 @@ bool Vehicle::supportsMotorInterference(void) const
     return _firmwarePlugin->supportsMotorInterference();
 }
 
+bool Vehicle::supportsTerrainFrame(void) const
+{
+    return _firmwarePlugin->supportsTerrainFrame();
+}
+
 QString Vehicle::vehicleTypeName() const {
     static QMap<int, QString> typeNames = {
         { MAV_TYPE_GENERIC,         tr("Generic micro air vehicle" )},

src/Vehicle/Vehicle.h

@@ -457,6 +457,7 @@ public:
     Q_PROPERTY(QString              hobbsMeter              READ hobbsMeter                                             NOTIFY hobbsMeterChanged)
     Q_PROPERTY(bool                 vtolInFwdFlight         READ vtolInFwdFlight        WRITE setVtolInFwdFlight        NOTIFY vtolInFwdFlightChanged)
     Q_PROPERTY(bool                 highLatencyLink         READ highLatencyLink                                        NOTIFY highLatencyLinkChanged)
+    Q_PROPERTY(bool                 supportsTerrainFrame    READ supportsTerrainFrame                                   NOTIFY firmwareTypeChanged)
 
     // Vehicle state used for guided control
     Q_PROPERTY(bool flying                  READ flying NOTIFY flyingChanged)                               ///< Vehicle is flying
@@ -665,11 +666,12 @@ public:
     bool rover(void) const;
     bool sub(void) const;
 
-    bool supportsThrottleModeCenterZero(void) const;
-    bool supportsNegativeThrust(void) const;
-    bool supportsRadio(void) const;
-    bool supportsJSButton(void) const;
-    bool supportsMotorInterference(void) const;
+    bool supportsThrottleModeCenterZero (void) const;
+    bool supportsNegativeThrust         (void) const;
+    bool supportsRadio                  (void) const;
+    bool supportsJSButton               (void) const;
+    bool supportsMotorInterference      (void) const;
+    bool supportsTerrainFrame           (void) const;
 
     void setGuidedMode(bool guidedMode);