diff --git a/src/QGC.cc b/src/QGC.cc index 3237fadf3082d28f1f3864b15e75f8c5e3211b51..12dbcb688bbaf10f743a81312d5e61f1c8f1e0e3 100644 --- a/src/QGC.cc +++ b/src/QGC.cc @@ -22,7 +22,6 @@ This file is part of the QGROUNDCONTROL project ======================================================================*/ #include "QGC.h" - #include #include diff --git a/src/QGC.h b/src/QGC.h index 31d0936b4842f6461bdc58f9a9f5621f4fe14b13..bf36900d332a8dd1365ead9e14eacd0d1ebbb35f 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -69,7 +69,7 @@ const QColor colorRed(154, 20, 20); const QColor colorGreen(20, 200, 20); const QColor colorYellow(255, 255, 0); const QColor colorOrange(255, 140, 0); -const QColor colorMagenta(255, 0, 65); +const QColor colorMagenta(255, 0, 55); const QColor colorDarkWhite(240, 240, 240); const QColor colorDarkYellow(180, 180, 0); const QColor colorBackground("#050508"); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index a2fc2a8b01250d2d8c53c0104759ba607c88a70c..fc633971a742e153af4dc3f963e6095f1d247345 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -909,13 +909,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(&message, &wpc); - if (wpc.target_system == mavlink->getSystemId()) + if(wpc.target_system == mavlink->getSystemId() || wpc.target_system == 0) { waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count); } else { - qDebug() << "Got waypoint message, but was not for me"; + qDebug() << "Got waypoint message, but was wrong system id" << wpc.target_system; } } break; @@ -925,13 +925,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(&message, &wp); //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z; - if(wp.target_system == mavlink->getSystemId()) + if(wp.target_system == mavlink->getSystemId() || wp.target_system == 0) { waypointManager.handleWaypoint(message.sysid, message.compid, &wp); } else { - qDebug() << "Got waypoint message, but was not for me"; + qDebug() << "Got waypoint message, but was wrong system id" << wp.target_system; } } break; @@ -940,7 +940,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(&message, &wpa); - if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId()) + if((wpa.target_system == mavlink->getSystemId() || wpa.target_system == 0) && + (wpa.target_component == mavlink->getComponentId() || wpa.target_component == 0)) { waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa); } @@ -951,13 +952,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(&message, &wpr); - if(wpr.target_system == mavlink->getSystemId()) + if(wpr.target_system == mavlink->getSystemId() || wpr.target_system == 0) { waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr); } else { - qDebug() << "Got waypoint message, but was not for me"; + qDebug() << "Got waypoint message, but was wrong system id" << wpr.target_system; } } break; diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 2a241a28c3a3a4b2d0b800229df308e4d06fcc80..c2dce3575400af09711791cf19da30d7cb5f3d15 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -1116,18 +1116,67 @@ void HSIDisplay::drawSetpointXYZYaw(float x, float y, float z, float yaw, const } } +void HSIDisplay::drawWaypoint(QPainter& painter, const QColor& color, float width, const QVector& list, int i, const QPointF& p) +{ + painter.setBrush(Qt::NoBrush); + + // Setup pen for background + QPen penBg(color); + penBg.setWidthF(width*2.0f); + + // Setup pen for foreground + QPen pen(color); + pen.setWidthF(width); + + // DRAW WAYPOINT + float waypointSize = vwidth / 20.0f * 2.0f; + QPolygonF poly(4); + // Top point + poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f)); + // Right point + poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y())); + // Bottom point + poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f)); + poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y())); + + float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f)); + float acceptRadius = list.at(i)->getAcceptanceRadius(); + + // Draw background + pen.setColor(Qt::black); + painter.setPen(penBg); + drawLine(p.x(), p.y(), p.x()+sin(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, p.y()-cos(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, refLineWidthToPen(0.4f), color, &painter); + drawPolygon(poly, &painter); + drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::black, &painter); + + // Draw foreground + pen.setColor(color); + painter.setPen(pen); + drawLine(p.x(), p.y(), p.x()+sin(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, p.y()-cos(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, refLineWidthToPen(0.4f), color, &painter); + drawPolygon(poly, &painter); + drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter); +} + void HSIDisplay::drawWaypoints(QPainter& painter) { if (uas) { const QVector& list = uas->getWaypointManager()->getWaypointEditableList(); + // Do not work on empty lists + if (list.size() == 0) return; + QColor color; painter.setBrush(Qt::NoBrush); + // XXX Ugly hacks, needs rewrite + QPointF lastWaypoint; + QPointF currentWaypoint; + int currentIndex = 0; - for (int i = 0; i < list.size(); i++) { + for (int i = 0; i < list.size(); i++) + { QPointF in; if (list.at(i)->getFrame() == MAV_FRAME_LOCAL_NED) { @@ -1144,42 +1193,22 @@ void HSIDisplay::drawWaypoints(QPainter& painter) // Scale from metric to screen reference coordinates QPointF p = metricBodyToRef(in); - // Setup pen - QPen pen(color); - painter.setBrush(Qt::NoBrush); - - // DRAW WAYPOINT - float waypointSize = vwidth / 20.0f * 2.0f; - QPolygonF poly(4); - // Top point - poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f)); - // Right point - poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y())); - // Bottom point - poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f)); - poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y())); - // Select color based on if this is the current waypoint - if (list.at(i)->getCurrent()) { - color = QGC::colorYellow;//uas->getColor(); - pen.setWidthF(refLineWidthToPen(0.8f)); - } else { - color = QGC::colorCyan; - pen.setWidthF(refLineWidthToPen(0.4f)); + if (list.at(i)->getCurrent()) + { + currentIndex = i; + currentWaypoint = p; + } + else + { + drawWaypoint(painter, QGC::colorCyan, refLineWidthToPen(0.4f), list, i, p); } - - pen.setColor(color); - painter.setPen(pen); - float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f)); - drawLine(p.x(), p.y(), p.x()+sin(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, p.y()-cos(list.at(i)->getYaw()/180.0*M_PI-yaw) * radius, refLineWidthToPen(0.4f), color, &painter); - drawPolygon(poly, &painter); - float acceptRadius = list.at(i)->getAcceptanceRadius(); - drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter); // DRAW CONNECTING LINE // Draw line from last waypoint to this one if (!lastWaypoint.isNull()) { + QPen pen(QGC::colorCyan); pen.setWidthF(refLineWidthToPen(0.4f)); painter.setPen(pen); color = QGC::colorCyan; @@ -1187,6 +1216,8 @@ void HSIDisplay::drawWaypoints(QPainter& painter) } lastWaypoint = p; } + + drawWaypoint(painter, QGC::colorYellow, refLineWidthToPen(0.8f), list, currentIndex, currentWaypoint); } } diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index b23075e61f96e6061475d35d451931d2ddb42507..a4e7e689bc5cb28ab1efc54f33b214fe1783710c 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -120,6 +120,8 @@ protected slots: void drawSetpointXYZYaw(float x, float y, float z, float yaw, const QColor &color, QPainter &painter); /** @brief Draw waypoints of this system */ void drawWaypoints(QPainter& painter); + /** @brief Draw one waypoint */ + void drawWaypoint(QPainter& painter, const QColor& color, float width, const QVector& list, int i, const QPointF& p); /** @brief Draw the limiting safety area */ void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter); /** @brief Receive mouse clicks */ diff --git a/src/ui/MAVLinkDecoder.cc b/src/ui/MAVLinkDecoder.cc index 0453b47120955fdbd3bcd24a4b7d839cd1458570..cf98bc78f169a739e931249e3128b3599ef6e6ff 100644 --- a/src/ui/MAVLinkDecoder.cc +++ b/src/ui/MAVLinkDecoder.cc @@ -41,6 +41,7 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) : textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG_VECT, false); textMessageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, false); textMessageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_INT, false); +// textMessageFilter.insert(MAVLINK_MSG_ID_HIGHRES_IMU, false); connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t))); } @@ -239,6 +240,13 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 name = name.arg(debug.name).arg(fieldName); time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms); } +// else if (msgid == MAVLINK_MSG_ID_HIGHRES_IMU) +// { +// mavlink_highres_imu_t d; +// mavlink_msg_highres_imu_decode(msg, &d); +// name = name.arg(debug.name).arg(fieldName); +// time = getUnixTimeFromMs(msg->sysid, debug.time_boot_ms); +// } else { name = name.arg(messageInfo[msgid].name, fieldName);