GPS updates

src/uas/PxQuadMAV.cc

@@ -86,6 +86,19 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
             positionLock = true;
         }
         break;
+    case MAVLINK_MSG_ID_AUX_STATUS:
+        {
+            mavlink_aux_status_t status;
+            mavlink_msg_aux_status_decode(&message, &status);
+            emit loadChanged(this, status.load/10.0f);
+            emit errCountChanged(uasId, "IMU", "I2C0", status.i2c0_err_count);
+            emit errCountChanged(uasId, "IMU", "I2C1", status.i2c1_err_count);
+            emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count);
+            emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count);
+            emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count);
+            emit valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow());
+        }
+        break;
     default:
         // Do nothing
         break;

src/uas/UAS.cc

@@ -249,19 +249,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 }
             }
             break;
-        case MAVLINK_MSG_ID_AUX_STATUS:
-            {
-                mavlink_aux_status_t status;
-                mavlink_msg_aux_status_decode(&message, &status);
-                emit loadChanged(this, status.load/10.0f);
-                emit errCountChanged(uasId, "IMU", "I2C0", status.i2c0_err_count);
-                emit errCountChanged(uasId, "IMU", "I2C1", status.i2c1_err_count);
-                emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count);
-                emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count);
-                emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count);
-                emit valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow());
-            }
-            break;
         case MAVLINK_MSG_ID_RAW_IMU:
             {
                 mavlink_raw_imu_t raw;
@@ -350,13 +337,21 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "lat", pos.lat, time);
                 emit valueChanged(uasId, "lon", pos.lon, time);
                 emit valueChanged(uasId, "alt", pos.alt, time);
-                emit valueChanged(uasId, "g-vx", pos.vx, time);
-                emit valueChanged(uasId, "g-vy", pos.vy, time);
-                emit valueChanged(uasId, "g-vz", pos.vz, time);
+                emit valueChanged(uasId, "speed", pos.v, time);
                 qDebug() << "GOT GPS RAW";
                 emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
             }
             break;
+        case MAVLINK_MSG_ID_GPS_STATUS:
+            {
+                mavlink_gps_status_t pos;
+                mavlink_msg_gps_status_decode(&message, &pos);
+                for(int i = 0; i < pos.satellites_visible && i < sizeof(pos.satellite_used); i++)
+                {
+                    emit gpsSatelliteStatusChanged(uasId, i, pos.satellite_azimuth[i], pos.satellite_direction[i], pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
+                }
+            }
+            break;
         case MAVLINK_MSG_ID_PARAM_VALUE:
             {
                 mavlink_param_value_t value;

src/uas/UASInterface.h

@@ -301,6 +301,8 @@ signals:
     void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
     void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec);
     void globalPositionChanged(UASInterface*, double lat, double lon, double alt, 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);
     void speedChanged(UASInterface*, double x, double y, double z, quint64 usec);
     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

@@ -39,11 +39,22 @@ This file is part of the PIXHAWK project
 #include <QDebug>
 
 HSIDisplay::HSIDisplay(QWidget *parent) :
-        HDDisplay(NULL, parent)
+        HDDisplay(NULL, parent),
+        gpsSatellites()
 {
     
 }
 
+void HSIDisplay::paintEvent(QPaintEvent * event)
+{
+    Q_UNUSED(event);
+    //paintGL();
+    static quint64 interval = 0;
+    qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
+    interval = MG::TIME::getGroundTimeNow();
+    paintDisplay();
+}
+
 void HSIDisplay::paintDisplay()
 {
     quint64 refreshInterval = 100;
@@ -69,7 +80,20 @@ void HSIDisplay::paintDisplay()
     const int columns = 3;
     const float spacing = 0.4f; // 40% of width
     const float gaugeWidth = vwidth / (((float)columns) + (((float)columns+1) * spacing + spacing * 0.1f));
-    const QColor ringColor = QColor(200, 200, 200);
+    const QColor ringColor = QColor(200, 250, 200);
+
+    const int ringCount = 2;
+    const float margin = 0.1f;  // 10% margin of total width on each side
+
+    for (int i = 0; i < ringCount; i++)
+    {
+        float radius = (vwidth - vwidth * 2.0f * margin) / (2.0f * i+1) / 2.0f;
+        drawCircle(vwidth/2.0f, vheight/2.0f, radius, 0.1f, ringColor, &painter);
+    }
+
+    drawGPS();
+
+
     //drawSystemIndicator(10.0f-gaugeWidth/2.0f, 20.0f, 10.0f, 40.0f, 15.0f, &painter);
     //drawGauge(15.0f, 15.0f, gaugeWidth/2.0f, 0, 1.0f, "thrust", values.value("thrust", 0.0f), gaugeColor, &painter, qMakePair(0.45f, 0.8f), qMakePair(0.8f, 1.0f), true);
     //drawGauge(15.0f+gaugeWidth*1.7f, 15.0f, gaugeWidth/2.0f, 0, 10.0f, "altitude", values.value("altitude", 0.0f), gaugeColor, &painter, qMakePair(1.0f, 2.5f), qMakePair(0.0f, 0.5f), true);
@@ -119,9 +143,64 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
     //}
 }
 
+void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used)
+{
+    Q_UNUSED(uasid);
+    // If slot is empty, insert object
+    if (gpsSatellites.at(satid) == NULL)
+    {
+        gpsSatellites.insert(satid, new GPSSatellite(satid, azimuth, direction, snr, used));
+    }
+    else
+    {
+        // Satellite exists, update it
+        gpsSatellites.at(satid)->update(satid, azimuth, direction, snr, used);
+    }
+}
+
+QColor HSIDisplay::getColorForSNR(float snr)
+{
+    return QColor(200, 250, 200);
+}
+
 void HSIDisplay::drawGPS()
 {
-	
+    QPainter painter(this);
+    painter.setRenderHint(QPainter::Antialiasing, true);
+    painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
+
+    // Max satellite circle radius
+
+    const float margin = 0.2f;  // 20% margin of total width on each side
+    float radius = (vwidth - vwidth * 2.0f * margin) / 2.0f;
+
+    for (int i = 0; i < gpsSatellites.size(); i++)
+    {
+        GPSSatellite* sat = gpsSatellites.at(i);
+        if (sat)
+        {
+            // Draw satellite
+            QBrush brush;
+            QColor color = getColorForSNR(sat->snr);
+            brush.setColor(color);
+            if (sat->used)
+            {
+                brush.setStyle(Qt::SolidPattern);
+            }
+            else
+            {
+                brush.setStyle(Qt::NoBrush);
+            }
+            painter.setPen(Qt::SolidLine);
+            painter.setPen(color);
+            painter.setBrush(brush);
+
+            float xPos = sin(sat->direction) * sat->azimuth * radius;
+            float yPos = cos(sat->direction) * sat->azimuth * radius;
+
+            drawCircle(xPos, yPos, vwidth/10.0f, 1.0f, color, &painter);
+        }
+    }
 }
 
 void HSIDisplay::drawObjects()

src/ui/HSIDisplay.h

@@ -45,19 +45,58 @@ class HSIDisplay : public HDDisplay {
     Q_OBJECT
 public:
     HSIDisplay(QWidget *parent = 0);
-   // ~HSIDisplay();
+    // ~HSIDisplay();
 
 public slots:
     void setActiveUAS(UASInterface* uas);
+    void paintEvent(QPaintEvent * event);
 
 protected slots:
     void paintDisplay();
-	void drawGPS();
-	void drawObjects();
-	void drawBaseLines(float xRef, float yRef, float radius, float yaw, const QColor& color, QPainter* painter, bool solid);
-	
+    void drawGPS();
+    void drawObjects();
+    void drawBaseLines(float xRef, float yRef, float radius, float yaw, const QColor& color, QPainter* painter, bool solid);
+
 
 protected:
+    void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
+    static QColor getColorForSNR(float snr);
+
+    /**
+     * @brief Private data container class to be used within the HSI widget
+     */
+    class GPSSatellite
+    {
+    public:
+        GPSSatellite(int id, float azimuth, float direction, float snr, bool used) :
+            id(id),
+            azimuth(azimuth),
+            direction(direction),
+            snr(snr),
+            used(used)
+        {
+
+        }
+
+        void update(int id, float azimuth, float direction, float snr, bool used)
+        {
+            this->id = id;
+            this->azimuth = azimuth;
+            this->direction = direction;
+            this->snr = snr;
+            this->used = used;
+        }
+
+        int id;
+        float azimuth;
+        float direction;
+        float snr;
+        bool used;
+
+        friend class HSIDisplay;
+    };
+
+    QVector<GPSSatellite*> gpsSatellites;
 
 private:
 };

src/ui/MainWindow.cc

@@ -356,6 +356,7 @@ void MainWindow::clearView()
     linechart->setActive(false);
     headDown1->stop();
     headDown2->stop();
+    hsi->stop();
 
     // Remove all dock widgets
     QList<QObject*> list = this->children();
@@ -426,9 +427,10 @@ void MainWindow::loadOperatorView()
     container5->setWidget(waypoints);
     addDockWidget(Qt::BottomDockWidgetArea, container5);
 
-    // DEBUG CONSOLE
-    QDockWidget* container7 = new QDockWidget(tr("Communication Console"), this);
-    container7->setWidget(debugConsole);
+    // HORIZONTAL SITUATION INDICATOR
+    QDockWidget* container7 = new QDockWidget(tr("Horizontal Situation Indicator"), this);
+    container7->setWidget(hsi);
+    hsi->start();
     addDockWidget(Qt::BottomDockWidgetArea, container7);
 
     // OBJECT DETECTION