From 8b3ba6594a725cca265057ce3eb2512b68fc84ee Mon Sep 17 00:00:00 2001
From: Bryant <susurrus@gmail.com>
Date: Wed, 11 Mar 2015 18:30:10 -0700
Subject: [PATCH] Display the crosstrack error in the HSI widget.

---
 src/ui/HSIDisplay.cc | 22 ++++++++++++++++++++--
 src/ui/HSIDisplay.h  |  4 ++++
 2 files changed, 24 insertions(+), 2 deletions(-)

diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index d865dcb0d..8b233963e 100644
--- a/src/ui/HSIDisplay.cc
+++ b/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);
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index 4185db79e..a99eab785 100644
--- a/src/ui/HSIDisplay.h
+++ b/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
-- 
2.22.0