Merge pull request #4306 from DonLakeFlyer/CorrectMavlinkHandling

Correct message handling: GPS_RAW_INT, GLOBAL_POSITION_INT, VFR_HUD, ALTITUDE

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -120,16 +120,11 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
 
 void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
 {
-    if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
-        qgcApp()->showMessage(QStringLiteral("Unable to takeoff, vehicle position not known."));
-        return;
-    }
-
     vehicle->sendMavCommand(vehicle->defaultComponentId(),
                             MAV_CMD_NAV_TAKEOFF,
                             true, // show error
                             0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
-                            vehicle->altitudeAMSL()->rawValue().toFloat() +  altitudeRel); // AMSL meters
+                            altitudeRel);
 }
 
 void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)

src/FlightDisplay/FlightDisplayViewWidgets.qml

@@ -437,7 +437,7 @@ Item {
                 break;
             case confirmChangeAlt:
                 altitudeSlider.visible = true
-                altitudeSlider.setInitialValueAppSettingsDistanceUnits(_activeVehicle.altitudeAMSL.value)
+                altitudeSlider.setInitialValueAppSettingsDistanceUnits(_activeVehicle.altitudeRelative.value)
                 guidedModeConfirm.confirmText = qsTr("change altitude")
                 break;
             case confirmGoTo:

src/Vehicle/GPSFact.json

@@ -29,7 +29,6 @@
 {
     "name":             "count",
     "shortDescription": "Sat Count",
-    "type":             "double",
-    "decimalPlaces":    0
+    "type":             "uint32"
 }
 ]

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;

src/uas/UASInterface.h

@@ -50,12 +50,6 @@ public:
     /** @brief The time interval the robot is switched on **/
     virtual quint64 getUptime() const = 0;
 
-    virtual double getLatitude() const = 0;
-    virtual double getLongitude() const = 0;
-    virtual double getAltitudeAMSL() const = 0;
-    virtual double getAltitudeRelative() const = 0;
-    virtual bool globalPositionKnown() const = 0;
-
     virtual double getRoll() const = 0;
     virtual double getPitch() const = 0;
     virtual double getYaw() const = 0;
@@ -172,16 +166,6 @@ protected:
     QColor color;
 
 signals:
-    /** @brief The robot state has changed */
-    void statusChanged(int stateFlag);
-    /** @brief The robot state has changed
-     *
-     * @param uas this robot
-     * @param status short description of status, e.g. "connected"
-     * @param description longer textual description. Should be however limited to a short text, e.g. 200 chars.
-     */
-    void statusChanged(UASInterface* uas, QString status, QString description);
-
     /** @brief A text message from the system has been received */
     void textMessageReceived(int uasid, int componentid, int severity, QString text);
 
@@ -200,13 +184,6 @@ signals:
      */
     void errCountChanged(int uasid, QString component, QString device, int count);
 
-    /**
-     * @brief Drop rate of communication link updated
-     *
-     * @param systemId id of the air system
-     * @param receiveDrop drop rate of packets this MAV receives (sent from GCS or other MAVs)
-     */
-    void dropRateChanged(int systemId,  float receiveDrop);
     /** @brief The robot is connected **/
     void connected();
     /** @brief The robot is disconnected **/
@@ -238,39 +215,12 @@ signals:
      */
     void batteryChanged(UASInterface* uas, double voltage, double current, double percent, int seconds);
     void statusChanged(UASInterface* uas, QString status);
-    void thrustChanged(UASInterface*, double thrust);
     void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
     void attitudeChanged(UASInterface*, int component, double roll, double pitch, double yaw, quint64 usec);
-    void attitudeRotationRatesChanged(int uas, double rollrate, double pitchrate, double yawrate, quint64 usec);
-    void attitudeThrustSetPointChanged(UASInterface*, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec);
-    /** @brief The MAV set a new setpoint in the local (not body) NED X, Y, Z frame */
-    void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
-    /** @brief A user (or an autonomous mission or obstacle avoidance planner) requested to set a new setpoint */
-    void userPositionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired);
-    void globalPositionChanged(UASInterface*, double lat, double lon, double altAMSL, quint64 usec);
-    void altitudeChanged(UASInterface*, double altitudeAMSL, double altitudeRelative, double climbRate, quint64 usec);
-    /** @brief Update the status of one satellite used for localization */
-    void gpsSatelliteStatusChanged(int uasid, int satid, float azimuth, float direction, float snr, bool used);
-
-    // The horizontal speed (a scalar)
-    void speedChanged(UASInterface* uas, double groundSpeed, double airSpeed, quint64 usec);
-    // Consider adding a MAV_FRAME parameter to this; could help specifying what the 3 scalars are.
-    void velocityChanged_NED(UASInterface*, double vx, double vy, double vz, quint64 usec);
-
-    void navigationControllerErrorsChanged(UASInterface*, double altitudeError, double speedError, double xtrackError);
-    void NavigationControllerDataChanged(UASInterface *uas, float navRoll, float navPitch, float navBearing, float targetBearing, float targetDist);
 
     void imageStarted(int imgid, int width, int height, int depth, int channels);
     void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex);
 
-    /** @brief Attitude control enabled/disabled */
-    void attitudeControlEnabled(bool enabled);
-    /** @brief Position 2D control enabled/disabled */
-    void positionXYControlEnabled(bool enabled);
-    /** @brief Altitude control enabled/disabled */
-    void positionZControlEnabled(bool enabled);
-    /** @brief Heading control enabled/disabled */
-    void positionYawControlEnabled(bool enabled);
     /** @brief Optical flow status changed */
     void opticalFlowStatusChanged(bool supported, bool enabled, bool ok);
     /** @brief Vision based localization status changed */
@@ -288,12 +238,6 @@ signals:
     /** @brief Differential pressure / airspeed status changed */
     void airspeedStatusChanged(bool supported, bool enabled, bool ok);
 
-    /**
-     * @brief Localization quality changed
-     * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
-     */
-    void localizationChanged(UASInterface* uas, int fix);
-
     // ERROR AND STATUS SIGNALS
     /** @brief Name of system changed */
     void nameChanged(QString newName);