Prefer ALTITUDE message over GPS messages

src/Vehicle/Vehicle.cc

@@ -138,8 +138,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _onboardControlSensorsEnabled(0)
     , _onboardControlSensorsHealth(0)
     , _onboardControlSensorsUnhealthy(0)
-    , _gpsRawIntMessageAvailable(false)
-    , _globalPositionIntMessageAvailable(false)
     , _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
     , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
     , _telemetryRRSSI(0)
@@ -332,8 +330,6 @@ Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
     , _onboardControlSensorsEnabled(0)
     , _onboardControlSensorsHealth(0)
     , _onboardControlSensorsUnhealthy(0)
-    , _gpsRawIntMessageAvailable(false)
-    , _globalPositionIntMessageAvailable(false)
     , _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
     , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
     , _mavlinkProtocolRequestComplete(true)
@@ -1260,9 +1256,11 @@ void Vehicle::_handleGpsRawInt(mavlink_message_t& message)
                 _coordinate = newPosition;
                 emit coordinateChanged(_coordinate);
             }
+            if (!_altitudeMessageAvailable) {
                 _altitudeAMSLFact.setRawValue(gpsRawInt.alt / 1000.0);
             }
         }
+    }
 
     _gpsFactGroup.lat()->setRawValue(gpsRawInt.lat * 1e-7);
     _gpsFactGroup.lon()->setRawValue(gpsRawInt.lon * 1e-7);
@@ -1279,8 +1277,10 @@ void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message)
     mavlink_global_position_int_t globalPositionInt;
     mavlink_msg_global_position_int_decode(&message, &globalPositionInt);
 
+    if (!_altitudeMessageAvailable) {
         _altitudeRelativeFact.setRawValue(globalPositionInt.relative_alt / 1000.0);
         _altitudeAMSLFact.setRawValue(globalPositionInt.alt / 1000.0);
+    }
 
     // ArduPilot sends bogus GLOBAL_POSITION_INT messages with lat/lat 0/0 even when it has no gps signal
     // Apparently, this is in order to transport relative altitude information.
@@ -1438,14 +1438,11 @@ void Vehicle::_handleAltitude(mavlink_message_t& message)
     mavlink_altitude_t altitude;
     mavlink_msg_altitude_decode(&message, &altitude);
 
-    // If data from GPS is available it takes precedence over ALTITUDE message
+    // Data from ALTITUDE message takes precedence over gps messages
-    if (!_globalPositionIntMessageAvailable) {
+    _altitudeMessageAvailable = true;
     _altitudeRelativeFact.setRawValue(altitude.altitude_relative);
-        if (!_gpsRawIntMessageAvailable) {
     _altitudeAMSLFact.setRawValue(altitude.altitude_amsl);
 }
-    }
-}
 
 void Vehicle::_setCapabilities(uint64_t capabilityBits)
 {

src/Vehicle/Vehicle.h

@@ -1424,8 +1424,9 @@ private:
     uint32_t        _onboardControlSensorsEnabled;
     uint32_t        _onboardControlSensorsHealth;
     uint32_t        _onboardControlSensorsUnhealthy;
-    bool            _gpsRawIntMessageAvailable;
+    bool            _gpsRawIntMessageAvailable          = false;
-    bool            _globalPositionIntMessageAvailable;
+    bool            _globalPositionIntMessageAvailable  = false;
+    bool            _altitudeMessageAvailable           = false;
     double          _defaultCruiseSpeed;
     double          _defaultHoverSpeed;
     int             _telemetryRRSSI;