From 06a542bf0675810c57b2c3e2c957abab138a6d5e Mon Sep 17 00:00:00 2001
From: pixhawk <pixhawk@student.ethz.ch>
Date: Wed, 9 Jun 2010 08:07:15 +0200
Subject: [PATCH] Added initialization and safety checks to remove clutter from
 UAS view

---
 src/uas/UAS.cc        | 25 ++++++++++++++++++++++---
 src/ui/uas/UASView.cc | 44 +++++++++++++++++++++++++++++++++++++++++---
 src/ui/uas/UASView.h  |  1 +
 3 files changed, 64 insertions(+), 6 deletions(-)

diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 1d026b4..2608af3 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -323,14 +323,33 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_gps_raw_t pos;
                 mavlink_msg_gps_raw_decode(&message, &pos);
+
+                // SANITY CHECK
+                // only accept values in a realistic range
                // quint64 time = getUnixTime(pos.usec);
                 quint64 time = MG::TIME::getGroundTimeNow();
                 emit valueChanged(uasId, "lat", pos.lat, time);
                 emit valueChanged(uasId, "lon", pos.lon, time);
+                // Check for NaN
+                int alt = pos.alt;
+                if (alt != alt)
+                {
+                    alt = 0;
+                    emit textMessageReceived(uasId, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
+                }
                 emit valueChanged(uasId, "alt", pos.alt, time);
-                emit valueChanged(uasId, "speed", pos.v, time);
-                //qDebug() << "GOT GPS RAW";
-                emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
+                // Smaller than threshold and not NaN
+                if (pos.v < 1000000 && pos.v == pos.v)
+                {
+                    emit valueChanged(uasId, "speed", pos.v, time);
+                    //qDebug() << "GOT GPS RAW";
+                    emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
+                }
+                else
+                {
+                     emit textMessageReceived(uasId, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v));
+                }
+                emit globalPositionChanged(this, pos.lon, pos.lat, alt, time);
             }
             break;
         case MAVLINK_MSG_ID_GPS_STATUS:
diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc
index 10dbd9a..defc81c 100644
--- a/src/ui/uas/UASView.cc
+++ b/src/ui/uas/UASView.cc
@@ -40,16 +40,26 @@ This file is part of the PIXHAWK project
 
 UASView::UASView(UASInterface* uas, QWidget *parent) :
         QWidget(parent),
+        startTime(0),
         timeRemaining(0),
+        chargeLevel(0),
+        uas(uas),
+        load(0),
         state("UNKNOWN"),
         stateDesc(tr("Unknown system state")),
         mode("MAV_MODE_UNKNOWN"),
         thrust(0),
         isActive(false),
+        x(0),
+        y(0),
+        z(0),
+        totalSpeed(0),
+        lat(0),
+        lon(0),
+        alt(0),
+        groundDistance(0),
         m_ui(new Ui::UASView)
 {
-    this->uas = uas;
-
     m_ui->setupUi(this);
 
     // Setup communication
@@ -316,9 +326,37 @@ void UASView::refresh()
     position = position.sprintf("%02.2f %02.2f %02.2f m", x, y, z);
     m_ui->positionLabel->setText(position);
     QString globalPosition;
-    globalPosition = globalPosition.sprintf("%02.2f %02.2f %02.2f m", lon, lat, alt);
+    QString latIndicator;
+    if (lat > 0)
+    {
+        latIndicator = "N";
+    }
+    else
+    {
+        latIndicator = "S";
+    }
+    QString lonIndicator;
+    if (lon > 0)
+    {
+        lonIndicator = "E";
+    }
+    else
+    {
+        lonIndicator = "W";
+    }
+    globalPosition = globalPosition.sprintf("%02.2f%s %02.2f%s %02.2f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt);
     m_ui->gpsLabel->setText(globalPosition);
 
+    // Altitude
+    if (groundDistance == 0 && alt != 0)
+    {
+        m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt));
+    }
+    else
+    {
+        m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance));
+    }
+
     // Speed
     QString speed;
     speed = speed.sprintf("%02.2f m/s", totalSpeed);
diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h
index bcd91c1..928423e 100644
--- a/src/ui/uas/UASView.h
+++ b/src/ui/uas/UASView.h
@@ -93,6 +93,7 @@ protected:
     float lat;
     float lon;
     float alt;
+    float groundDistance;
 
 
     void mouseDoubleClickEvent (QMouseEvent * event);