From 047dc94eb7de3a863b01bd977c6b3e6fb47299f2 Mon Sep 17 00:00:00 2001 From: LM Date: Tue, 23 Aug 2011 13:35:11 +0200 Subject: [PATCH] Added detected face function --- src/uas/UAS.cc | 24 ++++++++---------------- src/uas/UASInterface.h | 3 +++ src/ui/HSIDisplay.cc | 19 +++++++++++++++++++ src/ui/HSIDisplay.h | 6 ++++++ 4 files changed, 36 insertions(+), 16 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 0e93047..0f0df08 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1017,6 +1017,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, str+".z", "raw", vect.z, time); } break; + case MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT: + { + mavlink_object_detection_event_t event; + mavlink_msg_object_detection_event_decode(&message, &event); + QString str(event.name); + emit objectDetected(event.time, event.object_id, event.type, str, event.quality, event.bearing, event.distance); + } + break; // WILL BE ENABLED ONCE MESSAGE IS IN COMMON MESSAGE SET // case MAVLINK_MSG_ID_MEMORY_VECT: // { @@ -1066,22 +1074,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // } // } // break; - //#ifdef MAVLINK_ENABLED_PIXHAWK - // case MAVLINK_MSG_ID_POINT_OF_INTEREST: - // { - // mavlink_point_of_interest_t poi; - // mavlink_msg_point_of_interest_decode(&message, &poi); - // emit poiFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN), poi.x, poi.y, poi.z); - // } - // break; - // case MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION: - // { - // mavlink_point_of_interest_connection_t poi; - // mavlink_msg_point_of_interest_connection_decode(&message, &poi); - // emit poiConnectionFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN), poi.x1, poi.y1, poi.z1, poi.x2, poi.y2, poi.z2); - // } - // break; - //#endif #ifdef MAVLINK_ENABLED_UALBERTA case MAVLINK_MSG_ID_NAV_FILTER_BIAS: { diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 56ac538..0789d7c 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -472,6 +472,9 @@ signals: /** @brief Core specifications have changed */ void systemSpecsChanged(int uasId); + /** @brief Object detected */ + void objectDetected(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance); + // HOME POSITION / ORIGIN CHANGES void homePositionChanged(int uas, double lat, double lon, double alt); diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index bda28e2..924c66a 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -453,6 +453,23 @@ void HSIDisplay::updatePositionZControllerEnabled(bool enabled) zControlKnown = true; } +void HSIDisplay::updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance) +{ + // FIXME add multi-object support + QPainter painter(this); + QColor color(Qt::yellow); + float radius = vwidth / 20.0f; + QPen pen(color); + pen.setWidthF(refLineWidthToPen(0.4f)); + pen.setColor(color); + painter.setPen(pen); + painter.setBrush(Qt::NoBrush); + QPointF in(cos(bearing)-sin(bearing)*distance, sin(bearing)+cos(bearing)*distance); + // Scale from metric to screen reference coordinates + QPointF p = metricBodyToRef(in); + drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter); +} + QPointF HSIDisplay::metricWorldToBody(QPointF world) { // First translate to body-centered coordinates @@ -554,6 +571,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) disconnect(this->uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int))); disconnect(this->uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int))); disconnect(this->uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int))); + disconnect(this->uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float))); } connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool))); @@ -573,6 +591,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int))); connect(uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int))); connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int))); + connect(uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float))); this->uas = uas; diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 5730860..3a89992 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -65,6 +65,7 @@ public slots: void updateAttitudeControllerEnabled(bool enabled); void updatePositionXYControllerEnabled(bool enabled); void updatePositionZControllerEnabled(bool enabled); + void updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance); /** @brief Heading control enabled/disabled */ void updatePositionYawControllerEnabled(bool enabled); @@ -132,6 +133,11 @@ protected: QPointF metricBodyToRef(QPointF &metric); /** @brief Metric body coordinates to screen coordinates */ QPointF metricBodyToScreen(QPointF metric); + QMap objectNames; + QMap objectTypes; + QMap objectQualities; + QMap objectBearings; + QMap objectDistances; /** * @brief Private data container class to be used within the HSI widget