From db75e47f81d21ff43b513c607dd80200913d57a3 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Thu, 3 Jun 2010 07:52:13 +0200 Subject: [PATCH 1/3] Working MAV tracking --- src/ui/MapWidget.cc | 60 ++++++++++++++++++++++++++++++++++++----------------- src/ui/MapWidget.h | 3 +++ 2 files changed, 44 insertions(+), 19 deletions(-) diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index c077bd3..db7777c 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -34,6 +34,8 @@ This file is part of the PIXHAWK project #include "UASInterface.h" #include "UASManager.h" +#include "MG.h" + MapWidget::MapWidget(QWidget *parent) : QWidget(parent), zoomLevel(0), @@ -48,6 +50,8 @@ MapWidget::MapWidget(QWidget *parent) : mc->showScale(true); mc->enablePersistentCache(); + uasIcons = QMap(); + //QSize(480,640) // ImageManager::instance()->setProxy("www-cache", 8080); @@ -74,7 +78,7 @@ MapWidget::MapWidget(QWidget *parent) : QPushButton* zoomout = new QPushButton(QIcon(":/images/actions/list-remove.svg"), "", this); followgps = new QPushButton(QIcon(":/images/actions/system-lock-screen.svg"), "", this); followgps->setCheckable(true); - // gpsposition = new QLabel(); + // gpsposition = new QLabel(); zoomin->setMaximumWidth(50); zoomout->setMaximumWidth(50); followgps->setMaximumWidth(50); @@ -120,27 +124,44 @@ void MapWidget::addUAS(UASInterface* uas) void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec) { Q_UNUSED(usec); + quint64 currTime = MG::TIME::getGroundTimeNow(); + if (currTime - lastUpdate > 100) + { + lastUpdate = currTime; // create a LineString - QList points; + //QList points; // Points with a circle // A QPen can be used to customize the QPen* pointpen = new QPen(uas->getColor()); - pointpen->setWidth(3); - points.append(new CirclePoint(lat, lon, alt, uas->getUASName(), Point::Middle, pointpen)); -// points.append(new CirclePoint(8.275145, 50.016992, 15, "Wiesbaden-Mainz-Kastel, Johannes-Goßner-Straße", Point::Middle, pointpen)); -// points.append(new CirclePoint(8.270476, 50.021426, 15, "Wiesbaden-Mainz-Kastel, Ruthof", Point::Middle, pointpen)); -// // "Blind" Points -// points.append(new Point(8.266445, 50.025913, "Wiesbaden-Mainz-Kastel, Mudra Kaserne")); -// points.append(new Point(8.260378, 50.030345, "Wiesbaden-Mainz-Amoneburg, Dyckerhoffstraße")); - - // A QPen also can use transparency - QPen* linepen = new QPen(QColor(0, 0, 255, 100)); - linepen->setWidth(5); - // Add the Points and the QPen to a LineString - LineString* ls = new LineString(points, "Path", linepen); - - // Add the LineString to the layer - osmLayer->addGeometry(ls); + //pointpen->setWidth(3); + //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen)); + + if (!uasIcons.contains(uas->getUASID())) + { + CirclePoint* p = new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen); + uasIcons.insert(uas->getUASID(), p); + osmLayer->addGeometry(p); + } + else + { + CirclePoint* p = uasIcons.value(uas->getUASID()); + p->setCoordinate(QPointF(lat, lon)); + } + + // points.append(new CirclePoint(8.275145, 50.016992, 15, "Wiesbaden-Mainz-Kastel, Johannes-Goßner-Straße", Point::Middle, pointpen)); + // points.append(new CirclePoint(8.270476, 50.021426, 15, "Wiesbaden-Mainz-Kastel, Ruthof", Point::Middle, pointpen)); + // // "Blind" Points + // points.append(new Point(8.266445, 50.025913, "Wiesbaden-Mainz-Kastel, Mudra Kaserne")); + // points.append(new Point(8.260378, 50.030345, "Wiesbaden-Mainz-Amoneburg, Dyckerhoffstraße")); + +// // A QPen also can use transparency +// QPen* linepen = new QPen(QColor(0, 0, 255, 100)); +// linepen->setWidth(5); +// // Add the Points and the QPen to a LineString +// LineString* ls = new LineString(points, "Path", linepen); +// +// // Add the LineString to the layer +// osmLayer->addGeometry(ls); // Connect click events of the layer to this object //connect(osmLayer, SIGNAL(geometryClicked(Geometry*, QPoint)), @@ -150,9 +171,10 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, //QList view; //view.append(QPointF(8.24764, 50.0319)); //view.append(QPointF(8.28412, 49.9998)); - // mc->setView(view); + // mc->setView(view); updatePosition(0, lat, lon); } +} void MapWidget::updatePosition(float time, double lat, double lon) diff --git a/src/ui/MapWidget.h b/src/ui/MapWidget.h index c4be93a..006bd0c 100644 --- a/src/ui/MapWidget.h +++ b/src/ui/MapWidget.h @@ -33,6 +33,7 @@ This file is part of the PIXHAWK project #define MAPWIDGET_H #include +#include #include "qmapcontrol.h" #include "UASInterface.h" @@ -71,7 +72,9 @@ protected: Layer* osmLayer; Layer* gSatLayer; + QMap uasIcons; UASInterface* mav; + quint64 lastUpdate; private: Ui::MapWidget *m_ui; From acd864237cea16500dc9da3efc477781dd0c53bb Mon Sep 17 00:00:00 2001 From: pixhawk Date: Fri, 4 Jun 2010 12:01:03 +0200 Subject: [PATCH 2/3] Improved GPS, added UAS trails, improved error reporting --- src/uas/UAS.cc | 5 +++ src/uas/UASInterface.h | 15 +++++++ src/ui/MapWidget.cc | 107 +++++++++++++++++++++++++------------------- src/ui/MapWidget.h | 1 + src/ui/uas/UASInfoWidget.cc | 17 ++++++- src/ui/uas/UASInfoWidget.h | 3 ++ 6 files changed, 100 insertions(+), 48 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index ff956db..c1b6ca9 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -253,6 +253,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) 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; diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 926e777..8422c44 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -230,6 +230,21 @@ signals: */ void textMessageReceived(int uasid, int severity, QString text); /** + * @brief Update the error count of a device + * + * The error count indicates how many errors occured during the use of a device. + * Usually a random error from time to time is acceptable, e.g. through electromagnetic + * interferences on device lines like I2C and SPI. A constantly and rapidly increasing + * error count however can help to identify broken cables or misbehaving drivers. + * + * @param uasid System ID + * @param component Name of the component, e.g. "IMU" + * @param device Name of the device, e.g. "SPI0" or "I2C1" + * @param count Errors occured since system startup + */ + void errCountChanged(int uasid, QString component, QString device, int count); + + /** * @brief Drop rate of communication link updated * * @param systemId id of the air system diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index db7777c..ac6d7e8 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -39,6 +39,8 @@ This file is part of the PIXHAWK project MapWidget::MapWidget(QWidget *parent) : QWidget(parent), zoomLevel(0), + uasIcons(), + uasTrails(), m_ui(new Ui::MapWidget) { m_ui->setupUi(this); @@ -50,7 +52,7 @@ MapWidget::MapWidget(QWidget *parent) : mc->showScale(true); mc->enablePersistentCache(); - uasIcons = QMap(); + //uasIcons = QMap(); //QSize(480,640) // ImageManager::instance()->setProxy("www-cache", 8080); @@ -125,55 +127,66 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, { Q_UNUSED(usec); quint64 currTime = MG::TIME::getGroundTimeNow(); - if (currTime - lastUpdate > 100) + if (currTime - lastUpdate > 90) { lastUpdate = currTime; - // create a LineString - //QList points; - // Points with a circle - // A QPen can be used to customize the - QPen* pointpen = new QPen(uas->getColor()); - //pointpen->setWidth(3); - //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen)); - - if (!uasIcons.contains(uas->getUASID())) - { - CirclePoint* p = new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen); - uasIcons.insert(uas->getUASID(), p); - osmLayer->addGeometry(p); - } - else - { - CirclePoint* p = uasIcons.value(uas->getUASID()); - p->setCoordinate(QPointF(lat, lon)); + // create a LineString + //QList points; + // Points with a circle + // A QPen can be used to customize the + //pointpen->setWidth(3); + //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen)); + + if (!uasIcons.contains(uas->getUASID())) + { + // Get the UAS color + QColor uasColor = uas->getColor(); + + // Icon + QPen* pointpen = new QPen(uasColor); + CirclePoint* p = new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen); + uasIcons.insert(uas->getUASID(), p); + osmLayer->addGeometry(p); + + // Line + // A QPen also can use transparency + + QList points; + points.append(new Point(lat, lon, QString("lat: %1 lon: %2").arg(lat, lon))); + QPen* linepen = new QPen(uasColor.darker()); + linepen->setWidth(2); + // Add the Points and the QPen to a LineString + LineString* ls = new LineString(points, uas->getUASName(), linepen); + uasTrails.insert(uas->getUASID(), ls); + + // Add the LineString to the layer + osmLayer->addGeometry(ls); + } + else + { + CirclePoint* p = uasIcons.value(uas->getUASID()); + p->setCoordinate(QPointF(lat, lon)); + // Extend trail + uasTrails.value(uas->getUASID())->addPoint(new Point(lat, lon, QString("lat: %1 lon: %2").arg(lat, lon))); + } + + // points.append(new CirclePoint(8.275145, 50.016992, 15, "Wiesbaden-Mainz-Kastel, Johannes-Goßner-Straße", Point::Middle, pointpen)); + // points.append(new CirclePoint(8.270476, 50.021426, 15, "Wiesbaden-Mainz-Kastel, Ruthof", Point::Middle, pointpen)); + // // "Blind" Points + // points.append(new Point(8.266445, 50.025913, "Wiesbaden-Mainz-Kastel, Mudra Kaserne")); + // points.append(new Point(8.260378, 50.030345, "Wiesbaden-Mainz-Amoneburg, Dyckerhoffstraße")); + + // Connect click events of the layer to this object + //connect(osmLayer, SIGNAL(geometryClicked(Geometry*, QPoint)), + // this, SLOT(geometryClicked(Geometry*, QPoint))); + + // Sets the view to the interesting area + //QList view; + //view.append(QPointF(8.24764, 50.0319)); + //view.append(QPointF(8.28412, 49.9998)); + // mc->setView(view); + updatePosition(0, lat, lon); } - - // points.append(new CirclePoint(8.275145, 50.016992, 15, "Wiesbaden-Mainz-Kastel, Johannes-Goßner-Straße", Point::Middle, pointpen)); - // points.append(new CirclePoint(8.270476, 50.021426, 15, "Wiesbaden-Mainz-Kastel, Ruthof", Point::Middle, pointpen)); - // // "Blind" Points - // points.append(new Point(8.266445, 50.025913, "Wiesbaden-Mainz-Kastel, Mudra Kaserne")); - // points.append(new Point(8.260378, 50.030345, "Wiesbaden-Mainz-Amoneburg, Dyckerhoffstraße")); - -// // A QPen also can use transparency -// QPen* linepen = new QPen(QColor(0, 0, 255, 100)); -// linepen->setWidth(5); -// // Add the Points and the QPen to a LineString -// LineString* ls = new LineString(points, "Path", linepen); -// -// // Add the LineString to the layer -// osmLayer->addGeometry(ls); - - // Connect click events of the layer to this object - //connect(osmLayer, SIGNAL(geometryClicked(Geometry*, QPoint)), - // this, SLOT(geometryClicked(Geometry*, QPoint))); - - // Sets the view to the interesting area - //QList view; - //view.append(QPointF(8.24764, 50.0319)); - //view.append(QPointF(8.28412, 49.9998)); - // mc->setView(view); - updatePosition(0, lat, lon); -} } diff --git a/src/ui/MapWidget.h b/src/ui/MapWidget.h index 006bd0c..f3675e7 100644 --- a/src/ui/MapWidget.h +++ b/src/ui/MapWidget.h @@ -73,6 +73,7 @@ protected: Layer* gSatLayer; QMap uasIcons; + QMap uasTrails; UASInterface* mav; quint64 lastUpdate; diff --git a/src/ui/uas/UASInfoWidget.cc b/src/ui/uas/UASInfoWidget.cc index 8a0535a..7dfe569 100644 --- a/src/ui/uas/UASInfoWidget.cc +++ b/src/ui/uas/UASInfoWidget.cc @@ -61,7 +61,7 @@ UASInfoWidget::UASInfoWidget(QWidget *parent, QString name) : QWidget(parent) // Set default values /** Set two voltage decimals and zero charge level decimals **/ - this->voltageDecimals = 2; + this->voltageDecimals = 2; this->loadDecimals = 2; this->voltage = 0; @@ -69,6 +69,7 @@ UASInfoWidget::UASInfoWidget(QWidget *parent, QString name) : QWidget(parent) this->load = 0; receiveLoss = 0; sendLoss = 0; + errors = QMap(); updateTimer = new QTimer(this); connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh())); @@ -88,6 +89,7 @@ void UASInfoWidget::addUAS(UASInterface* uas) connect(uas, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBattery(UASInterface*,double,double,int))); connect(uas, SIGNAL(dropRateChanged(int,float)), this, SLOT(updateReceiveLoss(int,float))); connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateCPULoad(UASInterface*,double))); + connect(uas, SIGNAL(errCountChanged(int,QString,QString,int)), this, SLOT(updateErrorCount(int,QString,QString,int))); // Set this UAS as active if it is the first one if (activeUAS == 0) activeUAS = uas; @@ -106,6 +108,16 @@ void UASInfoWidget::updateBattery(UASInterface* uas, double voltage, double perc setTimeRemaining(uas, seconds); } +void UASInfoWidget::updateErrorCount(int uasid, QString component, QString device, int count) +{ + qDebug() << __FILE__ << __LINE__ << activeUAS->getUASID() << "=" << uasid; + if (activeUAS->getUASID() == uasid) + { + errors.remove(component + ":" + device); + errors.insert(component + ":" + device, count); + } +} + /** * */ @@ -168,4 +180,7 @@ void UASInfoWidget::refresh() ui.sendLossBar->setValue(sendLoss); ui.sendLossLabel->setText(QString::number(sendLoss, 'f', 2)); + + QString errorString; + // ui. } diff --git a/src/ui/uas/UASInfoWidget.h b/src/ui/uas/UASInfoWidget.h index 065fa9b..b669987 100644 --- a/src/ui/uas/UASInfoWidget.h +++ b/src/ui/uas/UASInfoWidget.h @@ -60,6 +60,8 @@ public slots: void updateReceiveLoss(int uasId, float receiveLoss); /** @brief Set the loss rate of packets sent from the MAV */ void updateSendLoss(int uasId, float sendLoss); + /** @brief Update the error count */ + void updateErrorCount(int uasid, QString component, QString device, int count); void setVoltage(UASInterface* uas, double voltage); void setChargeLevel(UASInterface* uas, double chargeLevel); @@ -90,6 +92,7 @@ protected: QTimer* updateTimer; QString name; quint64 startTime; + QMap errors; // double lastRemainingTime; // double lastChargeLevel; // double startVoltage; From 09ebae051e22aabcecee623dfe3dc03aa94029b9 Mon Sep 17 00:00:00 2001 From: lm Date: Fri, 4 Jun 2010 16:13:35 +0200 Subject: [PATCH 3/3] Adapted to new MAVLink messages --- src/uas/PxQuadMAV.cc | 26 ++++++++++++++++++++++++++ src/uas/UAS.cc | 31 ------------------------------- 2 files changed, 26 insertions(+), 31 deletions(-) diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 72beac8..71348ce 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -1,4 +1,5 @@ #include "PxQuadMAV.h" +#include "GAudioOutput.h" PxQuadMAV::PxQuadMAV(MAVLinkProtocol* mavlink, int id) : UAS(mavlink, id) @@ -60,6 +61,31 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, str+".z", vect.z, MG::TIME::getGroundTimeNow()); } break; + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + { + mavlink_vision_position_estimate_t pos; + mavlink_msg_vision_position_estimate_decode(&message, &pos); + quint64 time = getUnixTime(pos.usec); + emit valueChanged(uasId, "vis. time", pos.usec, time); + emit valueChanged(uasId, "vis. roll", pos.roll, time); + emit valueChanged(uasId, "vis. pitch", pos.pitch, time); + emit valueChanged(uasId, "vis. yaw", pos.yaw, time); + emit valueChanged(uasId, "vis. x", pos.x, time); + emit valueChanged(uasId, "vis. y", pos.y, time); + emit valueChanged(uasId, "vis. z", pos.z, time); + emit valueChanged(uasId, "vis. vx", pos.vx, time); + emit valueChanged(uasId, "vis. vy", pos.vy, time); + emit valueChanged(uasId, "vis. vz", pos.vz, time); + emit valueChanged(uasId, "vis. vyaw", pos.vyaw, time); + // Set internal state + if (!positionLock) + { + // If position was not locked before, notify positive + GAudioOutput::instance()->notifyPositive(); + } + positionLock = true; + } + break; default: // Do nothing break; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index c1b6ca9..f48ce8f 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -306,37 +306,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit attitudeChanged(this, mavlink_msg_attitude_get_roll(&message), mavlink_msg_attitude_get_pitch(&message), mavlink_msg_attitude_get_yaw(&message), time); } break; - case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: - { - mavlink_vision_position_estimate_t pos; - mavlink_msg_vision_position_estimate_decode(&message, &pos); - quint64 time = getUnixTime(pos.usec); - emit valueChanged(uasId, "vis. time", pos.usec, time); - emit valueChanged(uasId, "vis. roll", pos.roll, time); - emit valueChanged(uasId, "vis. pitch", pos.pitch, time); - emit valueChanged(uasId, "vis. yaw", pos.yaw, time); - emit valueChanged(uasId, "vis. x", pos.x, time); - emit valueChanged(uasId, "vis. y", pos.y, time); - emit valueChanged(uasId, "vis. z", pos.z, time); - // FIXME Only for testing for now - emit valueChanged(uasId, "vis. rot r1", pos.r1, time); - emit valueChanged(uasId, "vis. rot r2", pos.r2, time); - emit valueChanged(uasId, "vis. rot r3", pos.r3, time); - emit valueChanged(uasId, "vis. rot r4", pos.r4, time); - emit valueChanged(uasId, "vis. rot r5", pos.r5, time); - emit valueChanged(uasId, "vis. rot r6", pos.r6, time); - emit valueChanged(uasId, "vis. rot r7", pos.r7, time); - emit valueChanged(uasId, "vis. rot r8", pos.r8, time); - emit valueChanged(uasId, "vis. rot r9", pos.r9, time); - // Set internal state - if (!positionLock) - { - // If position was not locked before, notify positive - GAudioOutput::instance()->notifyPositive(); - } - positionLock = true; - } - break; case MAVLINK_MSG_ID_LOCAL_POSITION: //std::cerr << std::endl; //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;