diff --git a/src/QGCCore.cc b/src/QGCCore.cc index 1b08a06..571c252 100644 --- a/src/QGCCore.cc +++ b/src/QGCCore.cc @@ -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) 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())); } diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 439f2b1..7706e5f 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -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) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 48c38a1..4d182bf 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -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()); diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 6cf3772..5a25f5d 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -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() 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() 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() 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() 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) 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) { 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) 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) 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) 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 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, 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 TO SEND"; actionPending = true; @@ -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 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 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 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 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 { uiXSetCoordinate = bodyXSetCoordinate; uiYSetCoordinate = bodyYSetCoordinate; -// uiZSetCoordinate = bodyZSetCoordinate; + // uiZSetCoordinate = bodyZSetCoordinate; uiYawSet= bodyYawSet; } } @@ -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 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);