Merged

src/uas/PxQuadMAV.cc

@@ -120,7 +120,16 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
             emit valueChanged(uasId, "vicon x", "m", pos.x, time);
             emit valueChanged(uasId, "vicon y", "m", pos.y, time);
             emit valueChanged(uasId, "vicon z", "m", pos.z, time);
-            emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
+            //emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
+        }
+        break;
+        case MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE: {
+            mavlink_vision_speed_estimate_t speed;
+            mavlink_msg_vision_speed_estimate_decode(&message, &speed);
+            quint64 time = getUnixTime(speed.usec);
+            emit valueChanged(uasId, "vis. speed x", "m/s", speed.x, time);
+            emit valueChanged(uasId, "vis. speed y", "m/s", speed.y, time);
+            emit valueChanged(uasId, "vis. speed z", "m/s", speed.z, time);
         }
         break;
         default:

src/uas/UAS.cc

@@ -259,8 +259,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 modechanged = true;
                 this->mode = static_cast<int>(state.base_mode);
-
-
                 shortModeText = getShortModeTextFor(this->mode);
 
                 emit modeChanged(this->getUASID(), shortModeText, "");
@@ -345,7 +343,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit dropRateChanged(this->getUASID(), state.drop_rate_comm/10000.0f);
             }
             break;
-
         case MAVLINK_MSG_ID_RAW_IMU:
             {
                 mavlink_raw_imu_t raw;
@@ -381,8 +378,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             }
             break;
         case MAVLINK_MSG_ID_ATTITUDE:
-            //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;
             {
                 mavlink_attitude_t attitude;
                 mavlink_msg_attitude_decode(&message, &attitude);
@@ -449,8 +444,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
                 emit altitudeChanged(uasId, hud.alt);
                 //yaw = (hud.heading-180.0f/360.0f)*M_PI;
-                //emit attitudeChanged(this, roll, pitch, yaw, getUnixTime());
-                emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, getUnixTime());
+                emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, time);
             }
             break;
         case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
@@ -833,6 +827,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "servo #8", "us", servos.servo8_raw, time);
             }
             break;
+
+        case MAVLINK_MSG_ID_OPTICAL_FLOW:
+            {
+                mavlink_optical_flow_t flow;
+                mavlink_msg_optical_flow_decode(&message, &flow);
+                quint64 time = getUnixTime(flow.time);
+
+                emit valueChanged(uasId, QString("opt_flow_%1.x").arg(flow.sensor_id), "Pixel", flow.flow_x, time);
+                emit valueChanged(uasId, QString("opt_flow_%1.y").arg(flow.sensor_id), "Pixel", flow.flow_y, time);
+                emit valueChanged(uasId, QString("opt_flow_%1.qual").arg(flow.sensor_id), "0-255", flow.quality, time);
+                emit valueChanged(uasId, QString("opt_flow_%1.dist").arg(flow.sensor_id), "m", flow.ground_distance, time);
+            }
+            break;
         case MAVLINK_MSG_ID_STATUSTEXT:
             {
                 QByteArray b;
@@ -908,6 +915,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:
 //        {
@@ -957,22 +972,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

@@ -476,6 +476,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/HDDisplay.ui

@@ -14,6 +14,9 @@
    <string>Form</string>
   </property>
   <layout class="QHBoxLayout" name="horizontalLayout">
+   <property name="margin">
+    <number>0</number>
+   </property>
    <item>
     <widget class="QGraphicsView" name="view"/>
    </item>

src/ui/HSIDisplay.cc

@@ -119,8 +119,8 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
 
     // Add interaction elements
     QHBoxLayout* layout = new QHBoxLayout(this);
-    layout->setMargin(2);
-    layout->setSpacing(0);
+    layout->setMargin(0);
+    layout->setSpacing(12);
     QDoubleSpinBox* spinBox = new QDoubleSpinBox(this);
     spinBox->setMinimum(0.1);
     spinBox->setMaximum(9999);
@@ -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

src/ui/QGCMAVLinkInspector.cc

@@ -13,7 +13,7 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *par
     ui->setupUi(this);
 
     /* Insert system */
-    //ui->systemComboBox();
+    ui->systemComboBox->addItem(tr("All Systems"), -1);
 
     mavlink_message_info_t msg[256] = MAVLINK_MESSAGE_INFO;
     memcpy(messageInfo, msg, sizeof(mavlink_message_info_t)*256);