diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 71348cee4c5a553bd59b5376bcc3892ae80ec648..539b6e6cfae5347ef121721564cb0ef1d0e2a225 100644 --- a/src/uas/PxQuadMAV.cc +++ b/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; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index bb4ef71551383de5ec13de099fa6df58702df277..8d7fe46291567333e8a68936624a0f1c21b5bc3e 100644 --- a/src/uas/UAS.cc +++ b/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(pos.satellite_used[i])); + } + } + break; case MAVLINK_MSG_ID_PARAM_VALUE: { mavlink_param_value_t value; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index bbf0106dadf0c208b4c7f924660be8a97b4ad06a..0a963a426cae119c9d132aea7836a0128088317e 100644 --- a/src/uas/UASInterface.h +++ b/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); diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 88af81f977525a118ec51ab5ca57b214696aede2..eb4ba2d1c1f998e4fe67ba36c93bc65493e28652 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -39,11 +39,22 @@ This file is part of the PIXHAWK project #include 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() diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 69c257a632b86ec8b1c09c883d512aa01ea18309..838013daed55df642414caaf8c3327fec81f10df 100644 --- a/src/ui/HSIDisplay.h +++ b/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 gpsSatellites; private: }; diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 11d49940fec25ced58a692d5d6261dbdb07a33c1..fcfab3cf514f1334224be302697f5db1eabbe21f 100644 --- a/src/ui/MainWindow.cc +++ b/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 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