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

Re-enabled double UDP port usage, substantially improved setpoint sending

QGC4.4
LM 13 years ago
parent
a347a65985
commit
aef10d0bd9
4 changed files with 224 additions and 126 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      src/QGCCore.cc
  2. 4
      src/comm/UDPLink.cc
  3. 4
      src/uas/UAS.cc
  4. 334
      src/ui/HSIDisplay.cc

8
src/QGCCore.cc
Unescape View File

@ -167,7 +167,8 @@ QGCCore::QGCCore(int &argc, char* argv[]) : QApplication(argc, argv) @@ -167,7 +167,8 @@ QGCCore::QGCCore(int &argc, char* argv[]) : QApplication(argc, argv)
if (upgraded) mainWindow->showInfoMessage(tr("Default Settings Loaded"), tr("QGroundControl has been upgraded from version %1 to version %2. Some of your user preferences have been reset to defaults for safety reasons. Please adjust them where needed.").arg(lastApplicationVersion).arg(QGC_APPLICATION_VERSION));
// Check if link could be connected
if (!udpLink->connect()) {
if (!udpLink->connect())
{
QMessageBox msgBox;
msgBox.setIcon(QMessageBox::Critical);
msgBox.setText("Could not connect UDP port. Is an instance of " + qAppName() + "already running?");
@ -177,10 +178,11 @@ QGCCore::QGCCore(int &argc, char* argv[]) : QApplication(argc, argv) @@ -177,10 +178,11 @@ QGCCore::QGCCore(int &argc, char* argv[]) : QApplication(argc, argv)
int ret = msgBox.exec();
// Close the message box shortly after the click to prevent accidental clicks
QTimer::singleShot(5000, &msgBox, SLOT(reject()));
QTimer::singleShot(15000, &msgBox, SLOT(reject()));
// Exit application
if (ret == QMessageBox::Yes) {
if (ret == QMessageBox::Yes)
{
//mainWindow->close();
QTimer::singleShot(200, mainWindow, SLOT(close()));
}

4
src/comm/UDPLink.cc
Unescape View File

@ -293,9 +293,9 @@ bool UDPLink::connect() @@ -293,9 +293,9 @@ bool UDPLink::connect()
this->quit();
this->wait();
}
this->hardwareConnect();
bool connected = this->hardwareConnect();
start(HighPriority);
return true;
return connected;
}
bool UDPLink::hardwareConnect(void)

4
src/uas/UAS.cc
Unescape View File

@ -816,6 +816,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -816,6 +816,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT:
{
if (multiComponentSourceDetected && wrongComponent)
{
break;
}
mavlink_local_position_setpoint_t p;
mavlink_msg_local_position_setpoint_decode(&message, &p);
emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs());

334
src/ui/HSIDisplay.cc
Unescape View File

@ -178,9 +178,9 @@ void HSIDisplay::paintEvent(QPaintEvent * event) @@ -178,9 +178,9 @@ void HSIDisplay::paintEvent(QPaintEvent * event)
{
Q_UNUSED(event);
//paintGL();
// static quint64 interval = 0;
// //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
// interval = MG::TIME::getGroundTimeNow();
// static quint64 interval = 0;
// //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
// interval = MG::TIME::getGroundTimeNow();
renderOverlay();
}
@ -219,7 +219,8 @@ void HSIDisplay::renderOverlay() @@ -219,7 +219,8 @@ void HSIDisplay::renderOverlay()
pen.setWidth(refLineWidthToPen(0.1f));
painter.setPen(pen);
const int ringCount = 2;
for (int i = 0; i < ringCount; i++) {
for (int i = 0; i < ringCount; i++)
{
float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.35f * i+1) / 2.0f - bottomMargin / 2.0f;
drawCircle(xCenterPos, yCenterPos, radius, 0.1f, ringColor, &painter);
paintText(tr("%1 m").arg(refToMetric(radius), 5, 'f', 1, ' '), QGC::colorCyan, 1.6f, vwidth/2-4, vheight/2+radius+2.2, &painter);
@ -239,22 +240,23 @@ void HSIDisplay::renderOverlay() @@ -239,22 +240,23 @@ void HSIDisplay::renderOverlay()
painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor);
// Draw trail
// QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
// // Transform from body to world coordinates
// m = metricWorldToBody(m);
// // Scale from metric body to screen reference units
// QPointF s = metricBodyToRef(m);
// drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, uas->getColor(), &painter);
// QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
// // Transform from body to world coordinates
// m = metricWorldToBody(m);
// // Scale from metric body to screen reference units
// QPointF s = metricBodyToRef(m);
// drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, uas->getColor(), &painter);
// Draw center indicator
// QPolygonF p(3);
// p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f));
// p.replace(1, QPointF(xCenterPos-4.0f, yCenterPos+3.5f));
// p.replace(2, QPointF(xCenterPos+4.0f, yCenterPos+3.5f));
// drawPolygon(p, &painter);
// QPolygonF p(3);
// p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f));
// p.replace(1, QPointF(xCenterPos-4.0f, yCenterPos+3.5f));
// p.replace(2, QPointF(xCenterPos+4.0f, yCenterPos+3.5f));
// drawPolygon(p, &painter);
if (uas) {
if (uas)
{
// Translate to center
painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor);
QColor uasColor = uas->getColor();
@ -292,31 +294,14 @@ void HSIDisplay::renderOverlay() @@ -292,31 +294,14 @@ void HSIDisplay::renderOverlay()
float normAngleDiff = fabs(atan2(sin(angleDiff), cos(angleDiff)));
if (userSetPointSet && setPointDist > 0.08f || normAngleDiff > 0.05f || dragStarted)
{
QColor spColor(150, 150, 150);
drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, spColor, painter);
}
//if (((userSetPointSet) || (normAngleDiff > 0.05f) || dragStarted) && !(setPointDist < 0.08f && mavInitialized))
// Labels on outer part and bottom
// Draw waypoints
drawWaypoints(painter);
// Draw setpoint over waypoints
if (positionSetPointKnown)
{
// Draw setpoint
drawSetpointXYZYaw(bodyXSetCoordinate, bodyYSetCoordinate, bodyZSetCoordinate, bodyYawSet, uas->getColor(), painter);
// Draw travel direction line
QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
// Transform from body to world coordinates
m = metricWorldToBody(m);
// Scale from metric body to screen reference units
QPointF s = metricBodyToRef(m);
drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, uas->getColor(), &painter);
}
// Draw status flags
drawStatusFlag(2, 1, tr("ATT"), attControlEnabled, attControlKnown, painter);
drawStatusFlag(22, 1, tr("PXY"), xyControlEnabled, xyControlKnown, painter);
@ -339,29 +324,52 @@ void HSIDisplay::renderOverlay() @@ -339,29 +324,52 @@ void HSIDisplay::renderOverlay()
paintText(tr("%1 m").arg(crossTrackError, 5, 'f', 2, '0'), Qt::white, 2.2f, 67, 11, &painter);
// Draw position to bottom left
if (localAvailable > 0 && globalAvailable == 0) {
if (localAvailable > 0)
{
// Position
QString str;
float offset = (globalAvailable > 0) ? -3.0f : 0.0f;
str.sprintf("%05.2f %05.2f %05.2f m", x, y, z);
paintText(tr("POS"), QGC::colorCyan, 2.6f, 2, vheight- 4.0f, &painter);
paintText(str, Qt::white, 2.6f, 10, vheight - 4.0f, &painter);
paintText(tr("POS"), QGC::colorCyan, 2.6f, 2, vheight - offset - 2.0f, &painter);
paintText(str, Qt::white, 2.6f, 10, vheight - offset - 2.0f, &painter);
}
if (globalAvailable > 0) {
if (globalAvailable > 0)
{
// Position
QString str;
str.sprintf("lat: %05.2f lon: %06.2f alt: %06.2f", lat, lon, alt);
paintText(tr("GPS"), QGC::colorCyan, 2.6f, 2, vheight- 4.0f, &painter);
paintText(str, Qt::white, 2.6f, 10, vheight - 4.0f, &painter);
paintText(tr("GPS"), QGC::colorCyan, 2.6f, 2, vheight- 2.0f, &painter);
paintText(str, Qt::white, 2.6f, 10, vheight - 2.0f, &painter);
}
// Draw Safety
double x1, y1, z1, x2, y2, z2;
UASManager::instance()->getLocalNEDSafetyLimits(&x1, &y1, &z1, &x2, &y2, &z2);
// drawSafetyArea(QPointF(x1, y1), QPointF(x2, y2), QGC::colorYellow, painter);
// drawSafetyArea(QPointF(x1, y1), QPointF(x2, y2), QGC::colorYellow, painter);
// Draw status message
paintText(statusMessage, QGC::colorOrange, 2.4f, 8, 15, &painter);
paintText(statusMessage, QGC::colorOrange, 2.8f, 8, 15, &painter);
// Draw setpoint over waypoints
if (positionSetPointKnown || setPointKnown)
{
// Draw setpoint
drawSetpointXYZYaw(bodyXSetCoordinate, bodyYSetCoordinate, bodyZSetCoordinate, bodyYawSet, QGC::colorYellow, painter);
// Draw travel direction line
QPointF m(bodyXSetCoordinate, bodyYSetCoordinate);
// Transform from body to world coordinates
m = metricWorldToBody(m);
// Scale from metric body to screen reference units
QPointF s = metricBodyToRef(m);
drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, QGC::colorYellow, &painter);
}
if ((userSetPointSet || dragStarted) && ((normAngleDiff > 0.05f) || !(setPointDist < 0.08f && mavInitialized)))
{
QColor spColor(150, 250, 150);
drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, spColor, painter);
}
}
void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter)
@ -487,11 +495,11 @@ void HSIDisplay::updatePositionZControllerEnabled(bool enabled) @@ -487,11 +495,11 @@ void HSIDisplay::updatePositionZControllerEnabled(bool enabled)
void HSIDisplay::updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance)
{
Q_UNUSED(quality);
Q_UNUSED(name);
Q_UNUSED(type);
Q_UNUSED(id);
Q_UNUSED(time);
Q_UNUSED(quality);
Q_UNUSED(name);
Q_UNUSED(type);
Q_UNUSED(id);
Q_UNUSED(time);
// FIXME add multi-object support
QPainter painter(this);
QColor color(Qt::yellow);
@ -566,7 +574,7 @@ void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event) @@ -566,7 +574,7 @@ void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event)
{
QPointF p = screenToMetricBody(event->posF());
setBodySetpointCoordinateXY(p.x(), p.y());
// qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos;
// qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos;
}
}
@ -621,8 +629,8 @@ void HSIDisplay::mouseMoveEvent(QMouseEvent * event) @@ -621,8 +629,8 @@ void HSIDisplay::mouseMoveEvent(QMouseEvent * event)
if (leftDragStarted)
{
// uiZSetCoordinate -= 0.06f*(startY - event->y()) / this->frameSize().height();
// setBodySetpointCoordinateZ(uiZSetCoordinate);
// uiZSetCoordinate -= 0.06f*(startY - event->y()) / this->frameSize().height();
// setBodySetpointCoordinateZ(uiZSetCoordinate);
}
if (leftDragStarted || dragStarted) mouseHasMoved = true;
@ -631,6 +639,8 @@ void HSIDisplay::mouseMoveEvent(QMouseEvent * event) @@ -631,6 +639,8 @@ void HSIDisplay::mouseMoveEvent(QMouseEvent * event)
void HSIDisplay::keyPressEvent(QKeyEvent* event)
{
QPointF bodySP = metricWorldToBody(QPointF(uiXSetCoordinate, uiYSetCoordinate));
if ((event->key() == Qt::Key_Enter || event->key() == Qt::Key_Return) && actionPending)
{
actionPending = false;
@ -638,55 +648,129 @@ void HSIDisplay::keyPressEvent(QKeyEvent* event) @@ -638,55 +648,129 @@ void HSIDisplay::keyPressEvent(QKeyEvent* event)
statusClearTimer.start();
sendBodySetPointCoordinates();
}
else if ((event->key() == Qt::Key_W))
if ((event->key() == Qt::Key_W))
{
if (!directSending)
{
setBodySetpointCoordinateZ(uas->getLocalZ());
setBodySetpointCoordinateYaw(uas->getYaw());
setBodySetpointCoordinateXY(1.0, 0);
}
else
{
setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()+0.2, +1.5), bodySP.y());
}
}
if ((event->key() == Qt::Key_S))
{
if (!directSending)
{
setBodySetpointCoordinateZ(uas->getLocalZ());
setBodySetpointCoordinateYaw(uas->getYaw());
setBodySetpointCoordinateXY(-1.0, 0);
}
else
{
setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()-0.2, +1.5), bodySP.y());
}
}
if ((event->key() == Qt::Key_A))
{
if (!directSending)
{
setBodySetpointCoordinateZ(uas->getLocalZ());
setBodySetpointCoordinateYaw(uas->getYaw());
setBodySetpointCoordinateXY(0, -1.0);
}
else
{
setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()-0.2, +1.5));
}
}
if ((event->key() == Qt::Key_D))
{
setBodySetpointCoordinateXY(1.0, 0);
setBodySetpointCoordinateZ(uas->getLocalZ());
setBodySetpointCoordinateYaw(uas->getYaw());
if (!directSending)
{
setBodySetpointCoordinateZ(uas->getLocalZ());
setBodySetpointCoordinateYaw(uas->getYaw());
setBodySetpointCoordinateXY(0, 1.0);
}
else
{
setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()+0.2, +1.5));
}
}
else if ((event->key() == Qt::Key_S))
if ((event->key() == Qt::Key_Up))
{
setBodySetpointCoordinateXY(-1.0, 0);
setBodySetpointCoordinateZ(uas->getLocalZ());
setBodySetpointCoordinateYaw(uas->getYaw());
if (!directSending)
{
setBodySetpointCoordinateXY(0, 0);
setBodySetpointCoordinateYaw(uas->getYaw());
}
setBodySetpointCoordinateZ(-0.5);
}
else if ((event->key() == Qt::Key_A))
if ((event->key() == Qt::Key_Down))
{
setBodySetpointCoordinateXY(0, -1.0);
setBodySetpointCoordinateZ(uas->getLocalZ());
setBodySetpointCoordinateYaw(uas->getYaw());
if (!directSending)
{
setBodySetpointCoordinateXY(0, 0);
setBodySetpointCoordinateYaw(uas->getYaw());
}
setBodySetpointCoordinateZ(+0.5);
}
else if ((event->key() == Qt::Key_D))
if ((event->key() == Qt::Key_Left))
{
setBodySetpointCoordinateXY(0, 1.0);
setBodySetpointCoordinateZ(uas->getLocalZ());
setBodySetpointCoordinateYaw(uas->getYaw());
if (!directSending)
{
setBodySetpointCoordinateXY(0, 0);
setBodySetpointCoordinateZ(uas->getLocalZ());
}
setBodySetpointCoordinateYaw(-0.2);
}
else if ((event->key() == Qt::Key_Up))
if ((event->key() == Qt::Key_Right))
{
setBodySetpointCoordinateXY(0, 0);
setBodySetpointCoordinateZ(-0.5+uas->getLocalZ());
setBodySetpointCoordinateYaw(uas->getYaw());
if (!directSending)
{
setBodySetpointCoordinateXY(0, 0);
setBodySetpointCoordinateZ(uas->getLocalZ());
}
setBodySetpointCoordinateYaw(0.2);
}
else if ((event->key() == Qt::Key_Down))
if ((event->key() == Qt::Key_Escape))
{
setBodySetpointCoordinateZ(+0.5+uas->getLocalZ());
setBodySetpointCoordinateXY(0, 0);
setBodySetpointCoordinateYaw(uas->getYaw());
setBodySetpointCoordinateZ(0);
setBodySetpointCoordinateYaw(0);
statusMessage = "CANCELLED, PRESS <ENTER> TO SEND";
}
else if ((event->key() == Qt::Key_Left))
if ((event->key() == Qt::Key_T))
{
setBodySetpointCoordinateXY(0, 0);
setBodySetpointCoordinateZ(uas->getLocalZ());
setBodySetpointCoordinateYaw(-0.2+uas->getYaw());
directSending = !directSending;
statusMessage = (directSending) ? "DIRECT SEND ON" : "DIRECT SEND OFF";
statusClearTimer.start();
}
else if ((event->key() == Qt::Key_Right))
if (actionPending && (directSending || (event->key() == Qt::Key_Escape)))
{
setBodySetpointCoordinateXY(0, 0);
setBodySetpointCoordinateZ(uas->getLocalZ());
setBodySetpointCoordinateYaw(0.2+uas->getYaw());
actionPending = false;
statusMessage = "SETPOINT SENT";
statusClearTimer.start();
sendBodySetPointCoordinates();
}
HDDisplay::keyPressEvent(event);
HDDisplay::keyPressEvent(event);
}
void HSIDisplay::contextMenuEvent (QContextMenuEvent* event)
@ -767,19 +851,20 @@ void HSIDisplay::updateSpeed(UASInterface* uas, double vx, double vy, double vz, @@ -767,19 +851,20 @@ void HSIDisplay::updateSpeed(UASInterface* uas, double vx, double vy, double vz,
void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
{
userSetPointSet = true;
userXYSetPointSet = true;
// Set coordinates and send them out to MAV
if (uas)
{
userSetPointSet = true;
userXYSetPointSet = true;
// Set coordinates and send them out to MAV
QPointF sp(x, y);
sp = metricBodyToWorld(sp);
uiXSetCoordinate = sp.x();
uiYSetCoordinate = sp.y();
QPointF sp(x, y);
sp = metricBodyToWorld(sp);
uiXSetCoordinate = sp.x();
uiYSetCoordinate = sp.y();
qDebug() << "Attempting to set new setpoint at x: " << x << "metric y:" << y;
qDebug() << "Attempting to set new setpoint at x: " << x << "metric y:" << y;
if (uas && mavInitialized)
{
//sendBodySetPointCoordinates();
statusMessage = "POSITION SET, PRESS <ENTER> TO SEND";
actionPending = true;
@ -789,46 +874,52 @@ void HSIDisplay::setBodySetpointCoordinateXY(double x, double y) @@ -789,46 +874,52 @@ void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
void HSIDisplay::setBodySetpointCoordinateZ(double z)
{
userSetPointSet = true;
// Set coordinates and send them out to MAV
uiZSetCoordinate = z;
statusMessage = "Z SET, PRESS <ENTER> TO SEND";
actionPending = true;
statusClearTimer.start();
if (uas)
{
userSetPointSet = true;
// Set coordinates and send them out to MAV
uiZSetCoordinate = z+uas->getLocalZ();
statusMessage = "Z SET, PRESS <ENTER> TO SEND";
actionPending = true;
statusClearTimer.start();
}
//sendBodySetPointCoordinates();
}
void HSIDisplay::setBodySetpointCoordinateYaw(double yaw)
{
if (!userXYSetPointSet && setPointKnown)
{
uiXSetCoordinate = bodyXSetCoordinate;
uiYSetCoordinate = bodyYSetCoordinate;
}
else if (!userXYSetPointSet && mavInitialized)
if (uas)
{
QPointF coord = metricBodyToWorld(QPointF(0.0, 0.0));
uiXSetCoordinate = coord.x();
uiYSetCoordinate = coord.y();
if (!userXYSetPointSet && setPointKnown)
{
uiXSetCoordinate = bodyXSetCoordinate;
uiYSetCoordinate = bodyYSetCoordinate;
}
else if (!userXYSetPointSet && mavInitialized)
{
QPointF coord = metricBodyToWorld(QPointF(0.0, 0.0));
uiXSetCoordinate = coord.x();
uiYSetCoordinate = coord.y();
}
userSetPointSet = true;
// Set coordinates and send them out to MAV
uiYawSet = atan2(sin(yaw+uas->getYaw()), cos(yaw+uas->getYaw()));
statusMessage = "YAW SET, PRESS <ENTER> TO SEND";
statusClearTimer.start();
actionPending = true;
}
userSetPointSet = true;
// Set coordinates and send them out to MAV
uiYawSet = atan2(sin(yaw), cos(yaw));
statusMessage = "YAW SET, PRESS <ENTER> TO SEND";
statusClearTimer.start();
actionPending = true;
//sendBodySetPointCoordinates();
}
void HSIDisplay::sendBodySetPointCoordinates()
{
// Send the coordinates to the MAV
if (uas && mavInitialized)
if (uas)
{
double dx = uiXSetCoordinate - uas->getLocalX();
double dy = uiYSetCoordinate - uas->getLocalY();
double dz = uiZSetCoordinate - uas->getLocalZ();
bool valid = (sqrt(dx*dx + dy*dy + dz*dz) < 3.0);//UASManager::instance()->isInLocalNEDSafetyLimits(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate);
bool valid = (sqrt(dx*dx + dy*dy + dz*dz) < 2.0);//UASManager::instance()->isInLocalNEDSafetyLimits(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate);
if (valid)
{
uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
@ -885,7 +976,7 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir @@ -885,7 +976,7 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir
{
uiXSetCoordinate = bodyXSetCoordinate;
uiYSetCoordinate = bodyYSetCoordinate;
// uiZSetCoordinate = bodyZSetCoordinate;
// uiZSetCoordinate = bodyZSetCoordinate;
uiYawSet= bodyYawSet;
}
}
@ -983,10 +1074,11 @@ QColor HSIDisplay::getColorForSNR(float snr) @@ -983,10 +1074,11 @@ QColor HSIDisplay::getColorForSNR(float snr)
void HSIDisplay::drawSetpointXYZYaw(float x, float y, float z, float yaw, const QColor &color, QPainter &painter)
{
if (setPointKnown && uas) {
if (uas)
{
float radius = vwidth / 18.0f;
QPen pen(color);
pen.setWidthF(refLineWidthToPen(0.4f));
pen.setWidthF(refLineWidthToPen(1.6f));
pen.setColor(color);
painter.setPen(pen);
painter.setBrush(Qt::NoBrush);
@ -1006,12 +1098,12 @@ void HSIDisplay::drawSetpointXYZYaw(float x, float y, float z, float yaw, const @@ -1006,12 +1098,12 @@ void HSIDisplay::drawSetpointXYZYaw(float x, float y, float z, float yaw, const
poly.replace(3, QPointF(p.x()-radius, p.y()+radius));
// Label
paintText(QString("z: %1 m").arg(z), color, 1.2f, p.x()-radius, p.y()-radius-2.0f, &painter);
paintText(QString("z: %1").arg(z), color, 2.1f, p.x()-radius, p.y()-radius-3.5f, &painter);
drawPolygon(poly, &painter);
radius *= 0.8f;
drawLine(p.x(), p.y(), p.x()+sin(-yaw + uas->getYaw() + M_PI) * radius, p.y()+cos(-yaw + uas->getYaw() + M_PI) * radius, refLineWidthToPen(0.4f), color, &painter);
drawLine(p.x(), p.y(), p.x()+sin(-yaw + uas->getYaw() + M_PI) * radius, p.y()+cos(-yaw + uas->getYaw() + M_PI) * radius, refLineWidthToPen(0.6f), color, &painter);
// Draw center dot
painter.setBrush(color);

Write Preview
Loading…
Cancel
Save