Merge pull request #1352 from Susurrus/crosstrack_error

Crosstrack error

src/uas/UAS.cc

@@ -1344,6 +1344,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             setDistToWaypoint(p.wp_dist);
             setBearingToWaypoint(p.nav_bearing);
             emit navigationControllerErrorsChanged(this, p.alt_error, p.aspd_error, p.xtrack_error);
+            emit NavigationControllerDataChanged(this, p.nav_roll, p.nav_pitch, p.nav_bearing, p.target_bearing, p.wp_dist);
         }
             break;
         // Messages to ignore

src/uas/UASInterface.h

@@ -544,6 +544,7 @@ signals:
     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);

src/ui/HSIDisplay.cc

@@ -94,6 +94,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
     uiZSetCoordinate(0.0f),
     uiYawSet(0.0f),
     metricWidth(4.0),
+    crosstrackError(std::numeric_limits<double>::quiet_NaN()),
     xCenterPos(0),
     yCenterPos(0),
     positionLock(false),
@@ -412,9 +413,13 @@ void HSIDisplay::renderOverlay()
     paintText(tr("%1 m/s").arg(speed, 5, 'f', 2, '0'), valueColor, 2.2f, 12, topMargin+2, &painter);
 
     // Draw crosstrack error to top right
-    float crossTrackError = 0;
     paintText(tr("XTRACK"), labelColor, 2.2f, 54, topMargin+2, &painter);
-    paintText(tr("%1 m").arg(crossTrackError, 5, 'f', 2, '0'), valueColor, 2.2f, 67, topMargin+2, &painter);
+    if (!isnan(crosstrackError)) {
+        paintText(tr("%1 m").arg(crosstrackError, 5, 'f', 2, '0'), valueColor, 2.2f, 67, topMargin+2, &painter);
+    } else {
+        paintText(tr("-- m"), valueColor, 2.2f, 67, topMargin+2, &painter);
+    }
+
 
     // Draw position to bottom left
     if (localAvailable > 0)
@@ -945,6 +950,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
         disconnect(this->uas, SIGNAL(laserStatusChanged(bool,bool,bool)), this, SLOT(updateLaserStatus(bool,bool,bool)));
         disconnect(this->uas, SIGNAL(groundTruthSensorStatusChanged(bool,bool,bool)), this, SLOT(updateGroundTruthSensorStatus(bool,bool,bool)));
         disconnect(this->uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)), this, SLOT(updateActuatorStatus(bool,bool,bool)));
+        disconnect(this->uas, &UASInterface::navigationControllerErrorsChanged,
+                   this, &HSIDisplay::UpdateNavErrors);
     }
 
     if (uas)
@@ -1003,6 +1010,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
                 this, SLOT(updateGroundTruthSensorStatus(bool,bool,bool)));
         connect(uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)),
                 this, SLOT(updateActuatorStatus(bool,bool,bool)));
+        connect(uas, &UASInterface::navigationControllerErrorsChanged,
+                this, &HSIDisplay::UpdateNavErrors);
         statusClearTimer.start(3000);
     }
     else
@@ -1159,6 +1168,15 @@ void HSIDisplay::updateLocalPosition(UASInterface*, double x, double y, double z
     localAvailable = usec;
 }
 
+void HSIDisplay::UpdateNavErrors(UASInterface *uas, double altitudeError, double airspeedError, double crosstrackError)
+{
+    Q_UNUSED(altitudeError);
+    Q_UNUSED(airspeedError);
+    if (this->uas == uas) {
+        this->crosstrackError = crosstrackError;
+    }
+}
+
 void HSIDisplay::updateGlobalPosition(UASInterface*, double lat, double lon, double altAMSL, double altWGS84, quint64 usec)
 {
     Q_UNUSED(altAMSL);

src/ui/HSIDisplay.h

@@ -62,6 +62,7 @@ public slots:
     void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec);
     void updateGlobalPosition(UASInterface*, double lat, double lon, double altAMSL, double altWGS84, quint64 usec);
     void updateSpeed(UASInterface* uas, double vx, double vy, double vz, quint64 time);
+    void UpdateNavErrors(UASInterface *uas, double altitudeError, double airspeedError, double crosstrackError);
     void updatePositionLock(UASInterface* uas, bool lock);
     void updateAttitudeControllerEnabled(bool enabled);
     void updatePositionXYControllerEnabled(bool enabled);
@@ -341,6 +342,9 @@ protected:
     float uiYawSet;            ///< Yaw Setpoint wanted by the UI
     double metricWidth;        ///< Width the instrument represents in meters (the width of the ground shown by the widget)
 
+    // Navigation parameters
+    double crosstrackError;   ///< The crosstrack error (m) reported by the UAS
+
     //
     float xCenterPos;         ///< X center of instrument in virtual coordinates
     float yCenterPos;         ///< Y center of instrument in virtual coordinates

src/ui/PrimaryFlightDisplay.cc

@@ -121,7 +121,7 @@ PrimaryFlightDisplay::PrimaryFlightDisplay(QWidget *parent) :
     airSpeed(std::numeric_limits<double>::quiet_NaN()),
     climbRate(std::numeric_limits<double>::quiet_NaN()),
 
-    navigationCrosstrackError(0),
+    navigationCrosstrackError(std::numeric_limits<double>::quiet_NaN()),
     navigationTargetBearing(std::numeric_limits<double>::quiet_NaN()),
 
     layout(COMPASS_INTEGRATED),
@@ -221,6 +221,7 @@ void PrimaryFlightDisplay::forgetUAS(UASInterface* uas)
         disconnect(this->uas, SIGNAL(speedChanged(UASInterface*, double, double, quint64)), this, SLOT(updateSpeed(UASInterface*, double, double, quint64)));
         disconnect(this->uas, SIGNAL(altitudeChanged(UASInterface*, double, double, double, double, quint64)), this, SLOT(updateAltitude(UASInterface*, double, double, double, quint64)));
         disconnect(this->uas, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)), this, SLOT(updateNavigationControllerErrors(UASInterface*, double, double, double)));
+        disconnect(this->uas, &UASInterface::NavigationControllerDataChanged, this, &PrimaryFlightDisplay::UpdateNavigationControllerData);
     }
 }
 
@@ -244,6 +245,7 @@ void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas)
         connect(uas, SIGNAL(speedChanged(UASInterface*, double, double, quint64)), this, SLOT(updateSpeed(UASInterface*, double, double, quint64)));
         connect(uas, SIGNAL(altitudeChanged(UASInterface*, double, double, double, double, quint64)), this, SLOT(updateAltitude(UASInterface*, double, double, double, double, quint64)));
         connect(uas, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)), this, SLOT(updateNavigationControllerErrors(UASInterface*, double, double, double)));
+        connect(uas, &UASInterface::NavigationControllerDataChanged, this, &PrimaryFlightDisplay::UpdateNavigationControllerData);
 
         // Set new UAS
         this->uas = uas;
@@ -346,6 +348,16 @@ void PrimaryFlightDisplay::updateAltitude(UASInterface* uas, double _altitudeAMS
     climbRate = _climbRate;
 }
 
+void PrimaryFlightDisplay::UpdateNavigationControllerData(UASInterface *uas, float navRoll, float navPitch, float navBearing, float targetBearing, float targetDistance) {
+    Q_UNUSED(navRoll);
+    Q_UNUSED(navPitch);
+    Q_UNUSED(navBearing);
+    Q_UNUSED(targetDistance);
+    if (this->uas == uas) {
+        this->navigationTargetBearing = targetBearing;
+    }
+}
+
 void PrimaryFlightDisplay::updateNavigationControllerErrors(UASInterface* uas, double altitudeError, double speedError, double xtrackError) {
     Q_UNUSED(uas);
     this->navigationAltitudeError = altitudeError;
@@ -862,7 +874,8 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo
     drawTextCenter(painter, s_digitalCompass, largeTextSize, 0, -radius*0.38-digitalCompassUpshift);
 
     // The CDI
-    if (shouldDisplayNavigationData() && !isnan(navigationTargetBearing) && !isinf(navigationCrosstrackError)) {
+    // We only display this navigation data if both the target bearing and crosstrack error are valid
+    if (shouldDisplayNavigationData() && !isnan(navigationTargetBearing) && !isnan(navigationCrosstrackError)) {
         painter.resetTransform();
         painter.translate(area.center());
         // TODO : Sign might be wrong?

src/ui/PrimaryFlightDisplay.h

@@ -21,6 +21,7 @@ public slots:
     void updateSpeed(UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp);
     void updateAltitude(UASInterface* uas, double _altitudeAMSL, double _altitudeWGS84, 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);
 
     /** @brief Set the currently monitored UAS */
     void forgetUAS(UASInterface* uas);