Correct handling of GPS_RAW_INT, GLOBAL_POSITION_INT, VFR_HUD

* Moved from UAS
* Remove unused code

src/Vehicle/Vehicle.cc

@@ -85,11 +85,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _currentWarningCount(0)
     , _currentNormalCount(0)
     , _currentMessageType(MessageNone)
-    , _navigationAltitudeError(0.0f)
-    , _navigationSpeedError(0.0f)
-    , _navigationCrosstrackError(0.0f)
-    , _navigationTargetBearing(0.0f)
-    , _refreshTimer(new QTimer(this))
     , _updateCount(0)
     , _rcRSSI(255)
     , _rcRSSIstore(255)
@@ -99,6 +94,9 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _onboardControlSensorsEnabled(0)
     , _onboardControlSensorsHealth(0)
     , _onboardControlSensorsUnhealthy(0)
+    , _useGpsRawIntForPosition(true)
+    , _useGpsRawIntForAltitude(true)
+    , _useAltitudeForAltitude(false)
     , _connectionLost(false)
     , _connectionLostEnabled(true)
     , _missionManager(NULL)
@@ -150,11 +148,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     connect(this, &Vehicle::armedChanged,               this, &Vehicle::_announceArmedChanged);
     _uas = new UAS(_mavlink, this, _firmwarePluginManager);
 
-    setLatitude(_uas->getLatitude());
-    setLongitude(_uas->getLongitude());
-
-    connect(_uas, &UAS::latitudeChanged,                this, &Vehicle::setLatitude);
-    connect(_uas, &UAS::longitudeChanged,               this, &Vehicle::setLongitude);
     connect(_uas, &UAS::imageReady,                     this, &Vehicle::_imageReady);
     connect(this, &Vehicle::remoteControlRSSIChanged,   this, &Vehicle::_remoteControlRSSIChanged);
 
@@ -164,11 +157,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     // connect this vehicle to the follow me handle manager
     connect(this, &Vehicle::flightModeChanged,qgcApp()->toolbox()->followMe(), &FollowMe::followMeHandleManager);
 
-    // Refresh timer
-    connect(_refreshTimer, &QTimer::timeout, this, &Vehicle::_checkUpdate);
-    _refreshTimer->setInterval(UPDATE_TIMER);
-    _refreshTimer->start(UPDATE_TIMER);
-
     // PreArm Error self-destruct timer
     connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout);
     _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs);
@@ -196,11 +184,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     connect(_mav, SIGNAL(attitudeChanged                    (UASInterface*,int,double,double,double,quint64)),          this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
     connect(_mav, SIGNAL(statusChanged                      (UASInterface*,QString,QString)),                           this, SLOT(_updateState(UASInterface*, QString,QString)));
 
-    connect(_mav, &UASInterface::speedChanged, this, &Vehicle::_updateSpeed);
-    connect(_mav, &UASInterface::altitudeChanged, this, &Vehicle::_updateAltitude);
-    connect(_mav, &UASInterface::navigationControllerErrorsChanged,this, &Vehicle::_updateNavigationControllerErrors);
-    connect(_mav, &UASInterface::NavigationControllerDataChanged,   this, &Vehicle::_updateNavigationControllerData);
-
     _loadSettings();
 
     _missionManager = new MissionManager(this);
@@ -284,11 +267,6 @@ Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
     , _currentWarningCount(0)
     , _currentNormalCount(0)
     , _currentMessageType(MessageNone)
-    , _navigationAltitudeError(0.0f)
-    , _navigationSpeedError(0.0f)
-    , _navigationCrosstrackError(0.0f)
-    , _navigationTargetBearing(0.0f)
-    , _refreshTimer(new QTimer(this))
     , _updateCount(0)
     , _rcRSSI(255)
     , _rcRSSIstore(255)
@@ -298,6 +276,9 @@ Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
     , _onboardControlSensorsEnabled(0)
     , _onboardControlSensorsHealth(0)
     , _onboardControlSensorsUnhealthy(0)
+    , _useGpsRawIntForPosition(true)
+    , _useGpsRawIntForAltitude(true)
+    , _useAltitudeForAltitude(false)
     , _connectionLost(false)
     , _connectionLostEnabled(true)
     , _missionManager(NULL)
@@ -496,6 +477,18 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_LOGGING_DATA_ACKED:
         _handleMavlinkLoggingDataAcked(message);
         break;
+    case MAVLINK_MSG_ID_GPS_RAW_INT:
+        _handleGpsRawInt(message);
+        break;
+    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
+        _handleGlobalPositionInt(message);
+        break;
+    case MAVLINK_MSG_ID_ALTITUDE:
+        _handleAltitude(message);
+        break;
+    case MAVLINK_MSG_ID_VFR_HUD:
+        _handleVfrHud(message);
+        break;
 
     // Following are ArduPilot dialect messages
 
@@ -509,6 +502,70 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     _uas->receiveMessage(message);
 }
 
+void Vehicle::_handleVfrHud(mavlink_message_t& message)
+{
+    mavlink_vfr_hud_t vfrHud;
+    mavlink_msg_vfr_hud_decode(&message, &vfrHud);
+
+    _airSpeedFact.setRawValue(qIsNaN(vfrHud.airspeed) ? 0 : vfrHud.airspeed);
+    _groundSpeedFact.setRawValue(qIsNaN(vfrHud.groundspeed) ? 0 : vfrHud.groundspeed);
+    _climbRateFact.setRawValue(qIsNaN(vfrHud.climb) ? 0 : vfrHud.climb);
+}
+
+void Vehicle::_handleGpsRawInt(mavlink_message_t& message)
+{
+    mavlink_gps_raw_int_t gpsRawInt;
+    mavlink_msg_gps_raw_int_decode(&message, &gpsRawInt);
+
+    if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) {
+        if (_useGpsRawIntForPosition) {
+            setLatitude(gpsRawInt.lat / (double)1E7);
+            setLongitude(gpsRawInt.lon / (double)1E7);
+        }
+        if (_useGpsRawIntForAltitude) {
+            _altitudeRelativeFact.setRawValue(gpsRawInt.alt / 1000.0);
+        }
+    }
+
+    _gpsFactGroup.count()->setRawValue(gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible);
+    _gpsFactGroup.hdop()->setRawValue(gpsRawInt.eph == UINT16_MAX ? 1e10f : gpsRawInt.eph / 100.0);
+    _gpsFactGroup.vdop()->setRawValue(gpsRawInt.epv == UINT16_MAX ? 1e10f : gpsRawInt.epv / 100.0);
+    _gpsFactGroup.courseOverGround()->setRawValue(gpsRawInt.cog == UINT16_MAX ? 0.0 : gpsRawInt.cog / 100.0);
+    _gpsFactGroup.lock()->setRawValue(gpsRawInt.fix_type);
+
+    if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) {
+        _setCoordinateValid(true);
+    }
+}
+
+void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message)
+{
+    mavlink_global_position_int_t globalPositionInt;
+    mavlink_msg_global_position_int_decode(&message, &globalPositionInt);
+
+    _useGpsRawIntForPosition = false;
+    _useGpsRawIntForAltitude = false;
+
+    setLatitude(globalPositionInt.lat / (double)1E7);
+    setLongitude(globalPositionInt.lon / (double)1E7);
+    if (!_useAltitudeForAltitude) {
+        _altitudeRelativeFact.setRawValue(globalPositionInt.relative_alt / 1000.0);
+        _altitudeAMSLFact.setRawValue(globalPositionInt.alt / 1000.0);
+    }
+}
+
+void Vehicle::_handleAltitude(mavlink_message_t& message)
+{
+    mavlink_altitude_t altitude;
+    mavlink_msg_altitude_decode(&message, &altitude);
+
+    _useAltitudeForAltitude = true;
+    _useGpsRawIntForAltitude = false;
+    _altitudeRelativeFact.setRawValue(altitude.altitude_relative / 1000.0);
+    _altitudeAMSLFact.setRawValue(altitude.altitude_amsl / 1000.0);
+
+}
+
 void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
 {
     mavlink_autopilot_version_t autopilotVersion;
@@ -986,31 +1043,6 @@ void Vehicle::_updateAttitude(UASInterface* uas, int, double roll, double pitch,
     _updateAttitude(uas, roll, pitch, yaw, timestamp);
 }
 
-void Vehicle::_updateSpeed(UASInterface*, double groundSpeed, double airSpeed, quint64)
-{
-    _groundSpeedFact.setRawValue(groundSpeed);
-    _airSpeedFact.setRawValue(airSpeed);
-}
-
-void Vehicle::_updateAltitude(UASInterface*, double altitudeAMSL, double altitudeRelative, double climbRate, quint64)
-{
-    _altitudeAMSLFact.setRawValue(altitudeAMSL);
-    _altitudeRelativeFact.setRawValue(altitudeRelative);
-    _climbRateFact.setRawValue(climbRate);
-}
-
-void Vehicle::_updateNavigationControllerErrors(UASInterface*, double altitudeError, double speedError, double xtrackError) {
-    _navigationAltitudeError   = altitudeError;
-    _navigationSpeedError      = speedError;
-    _navigationCrosstrackError = xtrackError;
-}
-
-void Vehicle::_updateNavigationControllerData(UASInterface *uas, float, float, float, float targetBearing, float) {
-    if (_mav == uas) {
-        _navigationTargetBearing = targetBearing;
-    }
-}
-
 int Vehicle::motorCount(void)
 {
     switch (_vehicleType) {
@@ -1046,19 +1078,6 @@ bool Vehicle::xConfigMotors(void)
  * Internal
  */
 
-void Vehicle::_checkUpdate()
-{
-    // Update current location
-    if(_mav) {
-        if(latitude() != _mav->getLatitude()) {
-            setLatitude(_mav->getLatitude());
-        }
-        if(longitude() != _mav->getLongitude()) {
-            setLongitude(_mav->getLongitude());
-        }
-    }
-}
-
 QString Vehicle::getMavIconColor()
 {
     // TODO: Not using because not only the colors are ghastly, it doesn't respect dark/light palette
@@ -2115,53 +2134,6 @@ void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData)
 void VehicleGPSFactGroup::setVehicle(Vehicle* vehicle)
 {
     _vehicle = vehicle;
-
-    if (!vehicle) {
-        // Disconnected Vehicle
-        return;
-    }
-
-    connect(_vehicle->uas(), &UASInterface::localizationChanged, this, &VehicleGPSFactGroup::_setSatLoc);
-
-    UAS* pUas = dynamic_cast<UAS*>(_vehicle->uas());
-    connect(pUas, &UAS::satelliteCountChanged,  this, &VehicleGPSFactGroup::_setSatelliteCount);
-    connect(pUas, &UAS::satRawHDOPChanged,      this, &VehicleGPSFactGroup::_setSatRawHDOP);
-    connect(pUas, &UAS::satRawVDOPChanged,      this, &VehicleGPSFactGroup::_setSatRawVDOP);
-    connect(pUas, &UAS::satRawCOGChanged,       this, &VehicleGPSFactGroup::_setSatRawCOG);
-}
-
-void VehicleGPSFactGroup::_setSatelliteCount(double val, QString)
-{
-    // I'm assuming that a negative value or over 99 means there is no GPS
-    if(val < 0.0)  val = -1.0;
-    if(val > 99.0) val = -1.0;
-
-    _countFact.setRawValue(val);
-}
-
-void VehicleGPSFactGroup::_setSatRawHDOP(double val)
-{
-    _hdopFact.setRawValue(val);
-}
-
-void VehicleGPSFactGroup::_setSatRawVDOP(double val)
-{
-    _vdopFact.setRawValue(val);
-}
-
-void VehicleGPSFactGroup::_setSatRawCOG(double val)
-{
-    _courseOverGroundFact.setRawValue(val);
-}
-
-void VehicleGPSFactGroup::_setSatLoc(UASInterface*, int fix)
-{
-    _lockFact.setRawValue(fix);
-
-    // fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D lock, 3: 3D lock
-    if (fix > 2) {
-        _vehicle->_setCoordinateValid(true);
-    }
 }
 
 const char* VehicleBatteryFactGroup::_voltageFactName =                     "voltage";

src/Vehicle/Vehicle.h

@@ -137,13 +137,6 @@ public:
     static const char* _countFactName;
     static const char* _lockFactName;
 
-private slots:
-    void _setSatelliteCount(double val, QString);
-    void _setSatRawHDOP(double val);
-    void _setSatRawVDOP(double val);
-    void _setSatRawCOG(double val);
-    void _setSatLoc(UASInterface*, int fix);
-
 private:
     Vehicle*    _vehicle;
     Fact        _hdopFact;
@@ -683,11 +676,6 @@ private slots:
     void _updateAttitude                    (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
     /** @brief Attitude from one specific component / redundant autopilot */
     void _updateAttitude                    (UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp);
-    void _updateSpeed                       (UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp);
-    void _updateAltitude                    (UASInterface* uas, double _altitudeAMSL, double _altitudeRelative, double _climbRate, quint64 timestamp);
-    void _updateNavigationControllerErrors  (UASInterface* uas, double altitudeError, double speedError, double xtrackError);
-    void _updateNavigationControllerData    (UASInterface *uas, float navRoll, float navPitch, float navBearing, float targetBearing, float targetDistance);
-    void _checkUpdate                       ();
     void _updateState                       (UASInterface* system, QString name, QString description);
     /** @brief A new camera image has arrived */
     void _imageReady                        (UASInterface* uas);
@@ -716,6 +704,10 @@ private:
     void _handleCommandAck(mavlink_message_t& message);
     void _handleAutopilotVersion(LinkInterface* link, mavlink_message_t& message);
     void _handleHilActuatorControls(mavlink_message_t& message);
+    void _handleGpsRawInt(mavlink_message_t& message);
+    void _handleGlobalPositionInt(mavlink_message_t& message);
+    void _handleAltitude(mavlink_message_t& message);
+    void _handleVfrHud(mavlink_message_t& message);
     void _missionManagerError(int errorCode, const QString& errorMsg);
     void _geoFenceManagerError(int errorCode, const QString& errorMsg);
     void _rallyPointManagerError(int errorCode, const QString& errorMsg);
@@ -763,11 +755,6 @@ private:
     int             _currentNormalCount;
     MessageType_t   _currentMessageType;
     QString         _latestError;
-    float           _navigationAltitudeError;
-    float           _navigationSpeedError;
-    float           _navigationCrosstrackError;
-    float           _navigationTargetBearing;
-    QTimer*         _refreshTimer;
     QString         _currentState;
     int             _updateCount;
     QString         _formatedMessage;
@@ -779,6 +766,9 @@ private:
     uint32_t        _onboardControlSensorsEnabled;
     uint32_t        _onboardControlSensorsHealth;
     uint32_t        _onboardControlSensorsUnhealthy;
+    bool            _useGpsRawIntForPosition;
+    bool            _useGpsRawIntForAltitude;
+    bool            _useAltitudeForAltitude;
 
     typedef struct {
         int     component;