Added detected face function

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:
             {

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);

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;
 

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<int, QString> objectNames;
+    QMap<int, int> objectTypes;
+    QMap<int, float> objectQualities;
+    QMap<int, float> objectBearings;
+    QMap<int, float> objectDistances;
 
     /**
      * @brief Private data container class to be used within the HSI widget