zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #230 from Susurrus/gl_wp_hsi 

The HSIDisplay widget now displays all types of waypoints.
QGC4.4
Lorenz Meier 12 years ago
parent
4b005dcbb5 f6bf41e10c
commit
7a110c64f8
5 changed files with 54 additions and 42 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      src/uas/UAS.cc
  2. 2
      src/uas/UASWaypointManager.h
  3. 71
      src/ui/HSIDisplay.cc
  4. 2
      src/ui/HSIDisplay.h
  5. 11
      src/ui/QGCParamWidget.cc

10
src/uas/UAS.cc
Unescape View File

@ -887,6 +887,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// qDebug() << "RECEIVED PARAM:" << param; // qDebug() << "RECEIVED PARAM:" << param;
} }
break; 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: case MAV_PARAM_TYPE_UINT32:
{ {
// Variant // Variant

2
src/uas/UASWaypointManager.h
Unescape View File

@ -57,7 +57,7 @@ private:
WP_IDLE = 0, ///< Waiting for commands WP_IDLE = 0, ///< Waiting for commands
WP_SENDLIST, ///< Initial state for sending waypoints to the MAV WP_SENDLIST, ///< Initial state for sending waypoints to the MAV
WP_SENDLIST_SENDWPS,///< Sending waypoints 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_GETLIST_GETWPS, ///< Receiving waypoints
WP_CLEARLIST, ///< Clearing waypoint list on the MAV WP_CLEARLIST, ///< Clearing waypoint list on the MAV
WP_SETCURRENT ///< Setting new current waypoint on the MAV WP_SETCURRENT ///< Setting new current waypoint on the MAV

71
src/ui/HSIDisplay.cc
Unescape View File

@ -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<Waypoint*>& 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); 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())); poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f)); 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 // Draw background
pen.setColor(Qt::black); pen.setColor(Qt::black);
painter.setPen(pen); 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); drawPolygon(poly, &painter);
drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 3.0, Qt::black, &painter); drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 3.0, Qt::black, &painter);
// Draw foreground // Draw foreground
pen.setColor(color); pen.setColor(color);
painter.setPen(pen); 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); drawPolygon(poly, &painter);
drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter); drawCircle(p.x(), p.y(), metricToRef(acceptRadius), 1.0, Qt::green, &painter);
} }
@ -1227,51 +1228,65 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
{ {
if (uas) if (uas)
{ {
// Grab all waypoints.
const QVector<Waypoint*>& list = uas->getWaypointManager()->getWaypointEditableList(); const QVector<Waypoint*>& list = uas->getWaypointManager()->getWaypointEditableList();
const int numWaypoints = list.size();
// Do not work on empty lists // 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); painter.setBrush(Qt::NoBrush);
// XXX Ugly hacks, needs rewrite
QPointF lastWaypoint; QPointF lastWaypoint;
QPointF currentWaypoint; for (int i = 0; i < numWaypoints; i++)
int currentIndex = 0;
for (int i = 0; i < list.size(); i++)
{ {
const Waypoint *w = list.at(i);
QPointF in; 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(w->getX(), w->getY());
in = QPointF(list.at(i)->getX(), list.at(i)->getY()); }
} else { else if (frameRef == MAV_FRAME_LOCAL_ENU)
// Transform to local coordinates first {
double x = list.at(i)->getX(); in = QPointF(w->getY(), w->getX());
double y = list.at(i)->getY(); }
in = QPointF(x, y); // 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 // Transform from world to body coordinates
in = metricWorldToBody(in); in = metricWorldToBody(in);
// Scale from metric to screen reference coordinates // Scale from metric to screen reference coordinates
QPointF p = metricBodyToRef(in); QPointF p = metricBodyToRef(in);
// Select color based on if this is the current waypoint // Select color based on if this is the current waypoint.
if (list.at(i)->getCurrent()) if (w->getCurrent())
{ {
currentIndex = i; drawWaypoint(painter, QGC::colorYellow, refLineWidthToPen(0.8f), w, p);
currentWaypoint = p;
} }
else else
{ {
drawWaypoint(painter, QGC::colorCyan, refLineWidthToPen(0.4f), list, i, p); drawWaypoint(painter, QGC::colorCyan, refLineWidthToPen(0.4f), w, p);
} }
// DRAW CONNECTING LINE // Draw connecting line from last waypoint to this one.
// Draw line from last waypoint to this one
if (!lastWaypoint.isNull()) if (!lastWaypoint.isNull())
{ {
drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f*2.0f), Qt::black, &painter); 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; lastWaypoint = p;
} }
drawWaypoint(painter, QGC::colorYellow, refLineWidthToPen(0.8f), list, currentIndex, currentWaypoint);
} }
} }

2
src/ui/HSIDisplay.h
Unescape View File

@ -199,7 +199,7 @@ protected slots:
/** @brief Draw waypoints of this system */ /** @brief Draw waypoints of this system */
void drawWaypoints(QPainter& painter); void drawWaypoints(QPainter& painter);
/** @brief Draw one waypoint */ /** @brief Draw one waypoint */
void drawWaypoint(QPainter& painter, const QColor& color, float width, const QVector<Waypoint*>& 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 */ /** @brief Draw the limiting safety area */
void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter); void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter);
/** @brief Receive mouse clicks */ /** @brief Receive mouse clicks */

11
src/ui/QGCParamWidget.cc
Unescape View File

@ -168,17 +168,6 @@ void QGCParamWidget::loadParameterInfoCSV(const QString& autopilot, const QStrin
Q_UNUSED(airframe); 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";
qDebug() << "ATTEMPTING TO LOAD CSV";
QDir appDir = QApplication::applicationDirPath(); QDir appDir = QApplication::applicationDirPath();
appDir.cd("files"); appDir.cd("files");

Write Preview
Loading…
Cancel
Save