diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 0121425..b741117 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -887,6 +887,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // qDebug() << "RECEIVED PARAM:" << param; } break; + case MAV_PARAM_TYPE_UINT8: + { + // Variant + QVariant param(val.param_uint8); + parameters.value(component)->insert(parameterName, param); + // Emit change + emit parameterChanged(uasId, message.compid, parameterName, param); + emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); + //qDebug() << "RECEIVED PARAM:" << param; + } case MAV_PARAM_TYPE_UINT32: { // Variant diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 207d11e..1ae6d75 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -57,7 +57,7 @@ private: WP_IDLE = 0, ///< Waiting for commands WP_SENDLIST, ///< Initial state for sending waypoints to the MAV WP_SENDLIST_SENDWPS,///< Sending waypoints - WP_GETLIST, ///< Initial state for retrieving wayppoints from the MAV + WP_GETLIST, ///< Initial state for retrieving waypoints from the MAV WP_GETLIST_GETWPS, ///< Receiving waypoints WP_CLEARLIST, ///< Clearing waypoint list on the MAV WP_SETCURRENT ///< Setting new current waypoint on the MAV diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 284780b..7129f26 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -1186,7 +1186,7 @@ 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) +void HSIDisplay::drawWaypoint(QPainter& painter, const QColor& color, float width, const Waypoint *w, const QPointF& p) { painter.setBrush(Qt::NoBrush); @@ -1206,19 +1206,20 @@ void HSIDisplay::drawWaypoint(QPainter& painter, const QColor& color, float widt 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(); + float acceptRadius = w->getAcceptanceRadius(); + double yawDiff = w->getYaw()/180.0*M_PI-yaw; // Draw background pen.setColor(Qt::black); 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*3.0f), Qt::black, &painter); + drawLine(p.x(), p.y(), p.x()+sin(yawDiff) * radius, p.y()-cos(yawDiff) * radius, refLineWidthToPen(0.4f*3.0f), Qt::black, &painter); drawPolygon(poly, &painter); drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 3.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); + drawLine(p.x(), p.y(), p.x()+sin(yawDiff) * radius, p.y()-cos(yawDiff) * radius, refLineWidthToPen(0.4f), color, &painter); drawPolygon(poly, &painter); drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter); } @@ -1227,51 +1228,65 @@ void HSIDisplay::drawWaypoints(QPainter& painter) { if (uas) { + // Grab all waypoints. const QVector& list = uas->getWaypointManager()->getWaypointEditableList(); + const int numWaypoints = list.size(); // Do not work on empty lists - if (list.size() == 0) return; + if (list.size() == 0) + { + return; + } - QColor color; + // Make sure any drawn shapes are not filled-in. 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 < numWaypoints; i++) { + const Waypoint *w = list.at(i); QPointF in; - if (list.at(i)->getFrame() == MAV_FRAME_LOCAL_NED) + // Use local coordinates as-is. + int frameRef = w->getFrame(); + if (frameRef == MAV_FRAME_LOCAL_NED) { - // Do not transform - in = QPointF(list.at(i)->getX(), list.at(i)->getY()); - } else { - // Transform to local coordinates first - double x = list.at(i)->getX(); - double y = list.at(i)->getY(); - in = QPointF(x, y); + in = QPointF(w->getX(), w->getY()); + } + else if (frameRef == MAV_FRAME_LOCAL_ENU) + { + in = QPointF(w->getY(), w->getX()); + } + // Convert global coordinates into the local ENU frame, then display them. + else if (frameRef == MAV_FRAME_GLOBAL || frameRef == MAV_FRAME_GLOBAL_RELATIVE_ALT) { + // Get the position of the GPS origin for the MAV. + + // Transform the lat/lon for this waypoint into the local frame + double e, n, u; + UASManager::instance()->wgs84ToEnu(w->getX(), w->getY(),w->getZ(), &e, &n, &u); + in = QPointF(n, e); + } + // Otherwise we don't process this waypoint. + // FIXME: This code will probably fail if the last waypoint found is not a valid one. + else { + continue; } + // Transform from world to body coordinates in = metricWorldToBody(in); // Scale from metric to screen reference coordinates QPointF p = metricBodyToRef(in); - // Select color based on if this is the current waypoint - if (list.at(i)->getCurrent()) + // Select color based on if this is the current waypoint. + if (w->getCurrent()) { - currentIndex = i; - currentWaypoint = p; + drawWaypoint(painter, QGC::colorYellow, refLineWidthToPen(0.8f), w, p); } else { - drawWaypoint(painter, QGC::colorCyan, refLineWidthToPen(0.4f), list, i, p); + drawWaypoint(painter, QGC::colorCyan, refLineWidthToPen(0.4f), w, p); } - // DRAW CONNECTING LINE - // Draw line from last waypoint to this one + // Draw connecting line from last waypoint to this one. if (!lastWaypoint.isNull()) { drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f*2.0f), Qt::black, &painter); @@ -1279,8 +1294,6 @@ 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 c91f24d..6ebfeb7 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -199,7 +199,7 @@ protected slots: /** @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); + void drawWaypoint(QPainter& painter, const QColor& color, float width, const Waypoint *w, 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/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index d31793a..9658249 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -168,17 +168,6 @@ void QGCParamWidget::loadParameterInfoCSV(const QString& autopilot, const QStrin Q_UNUSED(airframe); qDebug() << "ATTEMPTING TO LOAD CSV"; - qDebug() << "ATTEMPTING TO LOAD CSV"; - qDebug() << "ATTEMPTING TO LOAD CSV"; - qDebug() << "ATTEMPTING TO LOAD CSV"; - qDebug() << "ATTEMPTING TO LOAD CSV"; - qDebug() << "ATTEMPTING TO LOAD CSV"; - qDebug() << "ATTEMPTING TO LOAD CSV"; - qDebug() << "ATTEMPTING TO LOAD CSV"; - qDebug() << "ATTEMPTING TO LOAD CSV"; - qDebug() << "ATTEMPTING TO LOAD CSV"; - qDebug() << "ATTEMPTING TO LOAD CSV"; - qDebug() << "ATTEMPTING TO LOAD CSV"; QDir appDir = QApplication::applicationDirPath(); appDir.cd("files");