From a825303c6d0758742ad24bdb9360c2629da57894 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Fri, 15 Jul 2011 15:02:18 +0200 Subject: [PATCH 1/9] added vision speed estimate parsing (pixhawk specific) --- src/uas/PxQuadMAV.cc | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 1ad0fbf..80f4021 100644 --- a/src/uas/PxQuadMAV.cc +++ b/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; case MAVLINK_MSG_ID_AUX_STATUS: { From 0046adb64d85195e98eafca079ef02f078b12786 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Mon, 15 Aug 2011 09:44:43 +0200 Subject: [PATCH 2/9] readded vision speed estimate display in realtime graph view, pixhawk specific --- src/uas/UAS.cc | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 4340baa..1b86477 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -421,6 +421,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit gpsLocalizationChanged(this, status.gps_fix); } 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", static_cast(speed.x), time); + emit valueChanged(uasId, "vis.speed y", "m/s", static_cast(speed.y), time); + emit valueChanged(uasId, "vis.speed z", "m/s", static_cast(speed.z), time); + } + break; #endif // PIXHAWK case MAVLINK_MSG_ID_RAW_IMU: { From 32ab8e46443048c771aee0db286e630d33e513f5 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Thu, 18 Aug 2011 18:31:59 +0200 Subject: [PATCH 3/9] wrongly named field in roll/pitch/yaw/thrust setpoint message --- src/uas/UAS.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1b86477..17c3de6 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -838,7 +838,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_roll_pitch_yaw_thrust_setpoint_t out; mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&message, &out); - quint64 time = getUnixTime(out.time_ms*1000); + quint64 time = getUnixTime(out.time_us*1000); emit attitudeThrustSetPointChanged(this, out.roll, out.pitch, out.yaw, out.thrust, time); emit valueChanged(uasId, "att control roll", "rad", out.roll, time); emit valueChanged(uasId, "att control pitch", "rad", out.pitch, time); From 2d1d33f76e316df93813d59d91c87ce4cc8819d5 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Fri, 19 Aug 2011 14:16:11 +0200 Subject: [PATCH 4/9] added optical flow message display to realtime plot --- src/uas/UAS.cc | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 17c3de6..2b4292b 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -932,6 +932,18 @@ 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); + } + break; case MAVLINK_MSG_ID_STATUSTEXT: { QByteArray b; From 80bae56d4361fa409a3200e03c8316efb835a01e Mon Sep 17 00:00:00 2001 From: pixhawk Date: Fri, 19 Aug 2011 14:31:38 +0200 Subject: [PATCH 5/9] added distance to optical flow --- src/uas/UAS.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 2b4292b..4a58641 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -942,6 +942,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) 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: From d72a46936ff7510a77d5c65657da4b41c50f5824 Mon Sep 17 00:00:00 2001 From: LM Date: Mon, 22 Aug 2011 13:49:32 +0200 Subject: [PATCH 6/9] Fixed long-standing issues with HSI display and load logging --- src/uas/UAS.cc | 2 +- src/ui/HDDisplay.ui | 3 +++ src/ui/HSIDisplay.cc | 4 ++-- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 4a58641..9601467 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -270,7 +270,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } emit loadChanged(this,state.load/10.0f); - emit valueChanged(uasId, "Load", "%", ((float)state.load)/10.0f, getUnixTime()); + //emit valueChanged(uasId, "Load", "%", ((float)state.load)/10.0f, getUnixTime()); if (this->mode != static_cast(state.mode)) { diff --git a/src/ui/HDDisplay.ui b/src/ui/HDDisplay.ui index 2cc9828..72273ec 100644 --- a/src/ui/HDDisplay.ui +++ b/src/ui/HDDisplay.ui @@ -14,6 +14,9 @@ Form + + 0 + diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 27f40b8..bda28e2 100644 --- a/src/ui/HSIDisplay.cc +++ b/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); From b5abc015cf368ea31426b0e7cd29686ce2dbfc74 Mon Sep 17 00:00:00 2001 From: LM Date: Mon, 22 Aug 2011 13:54:57 +0200 Subject: [PATCH 7/9] Fixed timing steps --- src/uas/UAS.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 9601467..60105e0 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -528,8 +528,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: From bd8ee7f3839e6dda073599d65312e7fc5a055190 Mon Sep 17 00:00:00 2001 From: LM Date: Mon, 22 Aug 2011 15:58:16 +0200 Subject: [PATCH 8/9] Minor cleanups --- src/uas/UAS.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 60105e0..0e93047 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -467,8 +467,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); From 047dc94eb7de3a863b01bd977c6b3e6fb47299f2 Mon Sep 17 00:00:00 2001 From: LM Date: Tue, 23 Aug 2011 13:35:11 +0200 Subject: [PATCH 9/9] 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