Impl. Lorenz suggestions wrt one velocityChanged, altitudeChanged,...

Impl. Lorenz suggestions wrt one velocityChanged, altitudeChanged, speedChanged signal per reference frame. Impl. CDI on compass disk.

src/uas/UAS.cc

@@ -711,14 +711,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
             }
 
-            // dongfang: For APM, this altitude is the mix altitude[m]
-            emit altitudeChanged(this, PRIMARY_ALTITUDE, hud.alt, time);
+            // The primary altitude is the one that the UAV uses for navigation.
+            // We assume! that the HUD message reports that as altitude.
+            emit primaryAltitudeChanged(this, hud.alt, time);
 
-            // This makes the lateral velocity zero when it might really not be, problematic.
-            // emit speedChanged(this, PRIMARY_SPEED, hud.airspeed, 0.0f, hud.climb, time);
-            emit speedChanged(this, PRIMARY_SPEED, hud.airspeed, time);
-            emit speedChanged(this, GROUNDSPEED_BY_UAV, hud.groundspeed, time);
-            emit climbRateChanged(this, BAROMETRIC_ALTITUDE, hud.groundspeed, time);
+            emit primarySpeedChanged(this, hud.airspeed, time);
+            emit gpsSpeedChanged(this, hud.groundspeed, time);
+            emit climbRateChanged(this, hud.climb, time);
         }
             break;
         case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
@@ -739,9 +738,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 localZ = pos.z;
 
                 // Emit
-
                 emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
-                emit velocityChanged(this, MAV_FRAME_LOCAL_NED, pos.vx, pos.vy, pos.vz, time);
+                emit velocityChanged_NED(this, pos.vx, pos.vy, pos.vz, time);
 
                 // Set internal state
                 if (!positionLock) {
@@ -786,12 +784,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
             emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time);
             // dongfang: The altitude is GPS altitude. Bugger. It needs to be changed to primary.
-            emit altitudeChanged(this, GPS_ALTITUDE, getAltitude(), time);
+            emit gpsAltitudeChanged(this, getAltitude(), time);
             // We had some frame mess here, global and local axes were mixed.
-            emit velocityChanged(this, MAV_FRAME_GLOBAL, speedX, speedY, speedZ, time);
+            emit velocityChanged_NED(this, speedX, speedY, speedZ, time);
 
             double groundspeed = qSqrt(speedX*speedX+speedY*speedY);
-            emit speedChanged(this, GROUNDSPEED_BY_GPS, groundspeed, time);
+            emit gpsSpeedChanged(this, groundspeed, time);
 
             // Set internal state
             if (!positionLock)
@@ -838,7 +836,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     setLongitude(longitude_gps);
                     setAltitude(altitude_gps);
                     emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time);
-                    emit altitudeChanged(this, GPS_ALTITUDE, getAltitude(), time);
+                    emit gpsAltitudeChanged(this, getAltitude(), time);
                 }
 
                 positionLock = true;
@@ -853,7 +851,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     if ((vel < 1000000) && !isnan(vel) && !isinf(vel))
                     {
                         // TODO: Other sources also? Actually this condition does not quite belong here.
-                        emit speedChanged(this, GROUNDSPEED_BY_GPS, vel, time);
+                        emit gpsSpeedChanged(this, vel, time);
                     }
                     else
                     {

src/uas/UAS.h

@@ -852,10 +852,9 @@ signals:
     void distToWaypointChanged(double val,QString name);
     void bearingToWaypointChanged(double val,QString name);
 
-    void altitudeChanged(UASInterface*, AltitudeMeasurementSource source, double altitude, quint64 usec);
-    //void speedChanged(UASInterface*, SpeedMeasurementSource source, double speed,  quint64 usec);
-    //void climbRateChanged(UASInterface*, AltitudeMeasurementSource source, double speed,  quint64 usec);
-    void velocityChanged(UASInterface*, MAV_FRAME, double vx, double vy, double vz, quint64 usec);
+    //void primaryAltitudeChanged(UASInterface*, double altitude, quint64 usec);
+    //void gpsAltitudeChanged(UASInterface*, double altitude, quint64 usec);
+    //void velocityChanged_NED(UASInterface*, double vx, double vy, double vz, quint64 usec);
 protected:
     /** @brief Get the UNIX timestamp in milliseconds, enter microseconds */
     quint64 getUnixTime(quint64 time=0);

src/uas/UASInterface.h

@@ -61,6 +61,7 @@ enum BatteryType
     AGZN = 5
 }; ///< The type of battery used
 
+/*
 enum SpeedMeasurementSource
 {
     PRIMARY_SPEED = 0,          // ArduPlane: Measured airspeed or estimated airspeed. ArduCopter: Ground (XY) speed.
@@ -84,6 +85,7 @@ enum AltitudeMeasurementFrame
     ABSOLUTE = 0,               // Altitude is pressure altitude
     ABOVE_HOME_POSITION = 1
 }; ///< For altitude data, a reference frame
+*/
 
 /**
  * @brief Interface for all robots.
@@ -520,16 +522,18 @@ signals:
     void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec);
     void localPositionChanged(UASInterface*, int component, double x, double y, double z, quint64 usec);
     void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec);
-    void altitudeChanged(UASInterface*, AltitudeMeasurementSource source, double altitude, quint64 usec);
+    void primaryAltitudeChanged(UASInterface*, double altitude, quint64 usec);
+    void gpsAltitudeChanged(UASInterface*, double altitude, 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*, SpeedMeasurementSource source, double speed, quint64 usec);
+    void primarySpeedChanged(UASInterface*, double speed, quint64 usec);
+    void gpsSpeedChanged(UASInterface*, double speed, quint64 usec);
     // The vertical speed (a scalar)
-    void climbRateChanged(UASInterface*, AltitudeMeasurementSource source, double speed, quint64 usec);
+    void climbRateChanged(UASInterface*, double climb, quint64 usec);
     // Consider adding a MAV_FRAME parameter to this; could help specifying what the 3 scalars are.
-    void velocityChanged(UASInterface*, MAV_FRAME frame, double vx, double vy, double vz, quint64 usec);
+    void velocityChanged_NED(UASInterface*, double vx, double vy, double vz, quint64 usec);
 
     void navigationControllerErrorsChanged(UASInterface*, double altitudeError, double speedError, double xtrackError);
 

src/ui/HSIDisplay.cc

@@ -867,7 +867,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
         disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
         disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
-        disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
+        disconnect(this->uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
 
         disconnect(this->uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool)));
@@ -898,7 +898,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
     connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
     connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
     connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
-    connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
+    connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
 
     connect(uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool)));

src/ui/HUD.cc

@@ -270,7 +270,7 @@ void HUD::setActiveUAS(UASInterface* uas)
 
         disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
-        disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
+        disconnect(this->uas, SIGNAL(velocityChanged_NEDspeedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int)));
 
         // Try to disconnect the image link
@@ -293,7 +293,7 @@ void HUD::setActiveUAS(UASInterface* uas)
 
         connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
         connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
-        connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
+        connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
         connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int)));
 
         // Try to connect the image link

src/ui/PrimaryFlightDisplay.h

@@ -58,6 +58,9 @@
 #define COMPASS_DISK_MARKERWIDTH 0.2
 #define COMPASS_DISK_MARKERHEIGHT 0.133
 
+#define CROSSTRACK_MAX 1000
+#define CROSSTRACK_RADIUS 0.6
+
 #define TAPE_GAUGES_TICKWIDTH_MAJOR 0.25
 #define TAPE_GAUGES_TICKWIDTH_MINOR 0.15
 
@@ -107,9 +110,13 @@ public slots:
     /** @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, SpeedMeasurementSource source, double speed, quint64 timstamp);
-    void updateClimbRate(UASInterface* uas, AltitudeMeasurementSource source, double altitude, quint64 timestamp);
-    void updateAltitude(UASInterface* uas, AltitudeMeasurementSource source, double altitude, quint64 timestamp);
+    void updatePrimarySpeed(UASInterface* uas, double speed, quint64 timstamp);
+    void updateGPSSpeed(UASInterface* uas, double speed, quint64 timstamp);
+    void updateClimbRate(UASInterface* uas, double altitude, quint64 timestamp);
+    void updatePrimaryAltitude(UASInterface* uas, double altitude, quint64 timestamp);
+    void updateGPSAltitude(UASInterface* uas, double altitude, quint64 timestamp);
+    void updateNavigationControllerErrors(UASInterface* uas, double altitudeError, double speedError, double xtrackError);
+
     /** @brief Set the currently monitored UAS */
     virtual void setActiveUAS(UASInterface* uas);
 
@@ -168,6 +175,7 @@ private:
      * - Airplane show absolute altutude in altimeter, copter shows relative to home
      */
     bool isAirplane();
+    bool shouldDisplayNavigationData();
 
     void drawTextCenter(QPainter& painter, QString text, float fontSize, float x, float y);
     void drawTextLeftCenter(QPainter& painter, QString text, float fontSize, float x, float y);
@@ -222,6 +230,11 @@ private:
     float groundspeed;
     float verticalVelocity;
 
+    float navigationAltitudeError;
+    float navigationSpeedError;
+    float navigationCrosstrackError;
+    float navigationTargetBearing;
+
     Layout layout;      // The display layout.
     Style style;        // The AI style (tapes translusent or opague)
 

src/ui/SlugsDataSensorView.cc

@@ -38,7 +38,7 @@ void SlugsDataSensorView::addUAS(UASInterface* uas)
 
         //connect standar messages
         connect(slugsMav, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugLocalPositionChanged(UASInterface*,double,double,double,quint64)));
-        connect(slugsMav, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64)));
+        connect(slugsMav, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64)));
         connect(slugsMav, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugAttitudeChanged(UASInterface*,double,double,double,quint64)));
         connect(slugsMav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugsGlobalPositionChanged(UASInterface*,double,double,double,quint64)));
         connect(slugsMav,SIGNAL(slugsGPSCogSog(int,double,double)),this,SLOT(slugsGPSCogSog(int,double,double)));

src/ui/uas/UASControlParameters.cpp

@@ -93,7 +93,7 @@ void UASControlParameters::activeUasSet(UASInterface *uas)
 {
     if(uas) {
         connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
-        connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(speedChanged(UASInterface*,double,double,double,quint64)));
+        connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(speedChanged(UASInterface*,double,double,double,quint64)));
         connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
         connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
         connect(uas, SIGNAL(thrustChanged(UASInterface*,double)), this, SLOT(thrustChanged(UASInterface*,double)) );

src/ui/uas/UASView.cc

@@ -91,7 +91,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
     connect(uas, SIGNAL(thrustChanged(UASInterface*, double)), this, SLOT(updateThrust(UASInterface*, double)));
     connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
-    connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
+    connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
     connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString,QString)));
     connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
     connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double)));