diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 425dc700dee4d03a241e5d3e96c6b61d25c50cfd..c975182b0a5a950be412b97d1a0b4ade5b577597 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -207,6 +207,15 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // ORDER MATTERS HERE! // If the matching UAS object does not yet exist, it has to be created // before emitting the packetReceived signal + + // BIG NASTY HACK + //TODO + //BUG + //BAD + //FIXME + if (message.sysid == 35) + message.sysid = 42; + UASInterface* uas = UASManager::instance()->getUASForId(message.sysid); // Check and (if necessary) create UAS object diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index eaf1798e9df1e5de1c26075e6598f33166b15242..050ef7a4bddbec6f10f94df97088bd444fb24112 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -379,7 +379,8 @@ void MAVLinkSimulationLink::mainloop() // Send back new setpoint mavlink_message_t ret; - mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, MAV_FRAME_LOCAL_NED, spX, spY, spZ, spYaw); + mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, MAV_FRAME_LOCAL_NED, spX, spY, spZ, spYaw); // spYaw/180.0*M_PI); + mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, spX, spY, spZ, bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 5379a345e982965821e67c3186cf227e5cafd458..bbc31e710826209178fd7569c472259cd0f3fdb3 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -72,7 +72,7 @@ pitch(0.0), yaw(0.0), statusTimeout(new QTimer(this)), paramsOnceRequested(false), -airframe(0), +airframe(QGC_AIRFRAME_EASYSTAR), attitudeKnown(false), paramManager(NULL), attitudeStamped(false), @@ -957,7 +957,7 @@ void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) { #ifdef MAVLINK_ENABLED_PIXHAWK mavlink_message_t msg; - mavlink_msg_position_control_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, 0, x, y, z, yaw); + mavlink_msg_local_position_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw/M_PI*180.0); sendMessage(msg); #else Q_UNUSED(x); diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index a105b8675c70d2313e39c96ab9b2b2c0942f8fcb..64a83a445baa9cbb14ae1bf6a391405f2f2c8b94 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -96,6 +96,26 @@ bool UASManager::setHomePosition(double lat, double lon, double alt) return changed; } +/** + * @param x1 Point 1 coordinate in x dimension + * @param y1 Point 1 coordinate in y dimension + * @param z1 Point 1 coordinate in z dimension + * + * @param x2 Point 2 coordinate in x dimension + * @param y2 Point 2 coordinate in y dimension + * @param z2 Point 2 coordinate in z dimension + */ +void UASManager::setLocalNEDSafetyBorders(double x1, double y1, double z1, double x2, double y2, double z2) +{ + nedSafetyLimitPosition1.x() = x1; + nedSafetyLimitPosition1.y() = y1; + nedSafetyLimitPosition1.z() = z1; + + nedSafetyLimitPosition2.x() = x2; + nedSafetyLimitPosition2.y() = y2; + nedSafetyLimitPosition2.z() = z2; +} + void UASManager::initReference(const double & latitude, const double & longitude, const double & altitude) { @@ -198,6 +218,7 @@ UASManager::UASManager() : { start(QThread::LowPriority); loadSettings(); + setLocalNEDSafetyBorders(1, -1, 0, -1, 1, -1); } UASManager::~UASManager() diff --git a/src/uas/UASManager.h b/src/uas/UASManager.h index a421b2199568aa9814ab4a2eba28f6f83c8064d7..c19732767a820d1b0a0f00067a61cf55453d2ccc 100644 --- a/src/uas/UASManager.h +++ b/src/uas/UASManager.h @@ -92,6 +92,37 @@ public: /** @brief Convert WGS84 lat/lon coordinates to carthesian coordinates with home position as origin */ void wgs84ToEnu(const double& lat, const double& lon, const double& alt, double* east, double* north, double* up); + void getLocalNEDSafetyLimits(double* x1, double* y1, double* z1, double* x2, double* y2, double* z2) + { + *x1 = nedSafetyLimitPosition1.x(); + *y1 = nedSafetyLimitPosition1.y(); + *z1 = nedSafetyLimitPosition1.z(); + + *x2 = nedSafetyLimitPosition2.x(); + *y2 = nedSafetyLimitPosition2.y(); + *z2 = nedSafetyLimitPosition2.z(); + } + + /** @brief Check if a position is in the local NED safety limits */ + bool isInLocalNEDSafetyLimits(double x, double y, double z) + { + if (x < nedSafetyLimitPosition1.x() && + y > nedSafetyLimitPosition1.y() && + z < nedSafetyLimitPosition1.z() && + x > nedSafetyLimitPosition2.x() && + y < nedSafetyLimitPosition2.y() && + z > nedSafetyLimitPosition2.z()) + { + // Within limits + return true; + } + else + { + // Not within limits + return false; + } + } + // void wgs84ToNed(const double& lat, const double& lon, const double& alt, double* north, double* east, double* down); @@ -188,6 +219,9 @@ public slots: /** @brief Set the current home position on all UAVs*/ bool setHomePosition(double lat, double lon, double alt); + /** @brief Set the safety limits in local position frame */ + void setLocalNEDSafetyBorders(double x1, double y1, double z1, double x2, double y2, double z2); + /** @brief Update home position based on the position from one of the UAVs */ void uavChangedHomePosition(int uav, double lat, double lon, double alt); @@ -207,6 +241,8 @@ protected: double homeAlt; Eigen::Quaterniond ecef_ref_orientation_; Eigen::Vector3d ecef_ref_point_; + Eigen::Vector3d nedSafetyLimitPosition1; + Eigen::Vector3d nedSafetyLimitPosition2; void initReference(const double & latitude, const double & longitude, const double & altitude); diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 1f3f28fca9838ef284f4e1781f8f2f16dfe30eaf..d397b51f2607ffa6926b43397ade03a6da58edce 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -85,7 +85,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) : bodyYawSet(0.0f), uiXSetCoordinate(0.0f), uiYSetCoordinate(0.0f), - uiZSetCoordinate(0.0f), + uiZSetCoordinate(-0.51f), uiYawSet(0.0f), metricWidth(4.0), positionLock(false), @@ -100,7 +100,11 @@ HSIDisplay::HSIDisplay(QWidget *parent) : mavInitialized(false), bottomMargin(10.0f), topMargin(12.0f), - userSetPointSet(false) + userSetPointSet(false), + dragStarted(false), + leftDragStarted(false), + mouseHasMoved(false), + actionPending(false) { refreshTimer->setInterval(updateInterval); @@ -121,6 +125,14 @@ HSIDisplay::HSIDisplay(QWidget *parent) : // Do first update setMetricWidth(metricWidth); + // Set tooltip + setToolTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget.")); + setStatusTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget.")); + + connect(&statusClearTimer, SIGNAL(timeout()), this, SLOT(clearStatusMessage())); + statusClearTimer.start(3000); + + setFocusPolicy(Qt::StrongFocus); } void HSIDisplay::resetMAVState() @@ -200,8 +212,9 @@ void HSIDisplay::renderOverlay() painter.setPen(pen); const int ringCount = 2; for (int i = 0; i < ringCount; i++) { - float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.3f * i+1) / 2.0f - bottomMargin / 2.0f; + 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); } // Draw orientation labels @@ -217,6 +230,15 @@ void HSIDisplay::renderOverlay() painter.rotate(+yawRotate); 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); + + // Draw center indicator // QPolygonF p(3); // p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f)); @@ -252,28 +274,41 @@ void HSIDisplay::renderOverlay() // Draw position setpoints in body coordinates - if (userSetPointSet) { + float xSpDiff = uiXSetCoordinate - bodyXSetCoordinate; + float ySpDiff = uiYSetCoordinate - bodyYSetCoordinate; + float zSpDiff = uiZSetCoordinate - bodyZSetCoordinate; + + float setPointDist = sqrt(xSpDiff*xSpDiff + ySpDiff*ySpDiff + zSpDiff*zSpDiff); + + float angleDiff = uiYawSet - bodyYawSet; + + float normAngleDiff = fabs(atan2(sin(angleDiff), cos(angleDiff))); + + if (userSetPointSet && setPointDist > 0.08f || normAngleDiff > 0.05f || dragStarted) + { QColor spColor(150, 150, 150); - drawSetpointXY(uiXSetCoordinate, uiYSetCoordinate, uiYawSet, spColor, painter); + drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, spColor, painter); } - if (positionSetPointKnown) { + // Labels on outer part and bottom + + // Draw waypoints + drawWaypoints(painter); + + // Draw setpoint over waypoints + if (positionSetPointKnown) + { // Draw setpoint - drawSetpointXY(bodyXSetCoordinate, bodyYSetCoordinate, bodyYawSet, QGC::colorCyan, painter); + 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, QGC::colorCyan, &painter); + drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, uas->getColor(), &painter); } - // Labels on outer part and bottom - - // Draw waypoints - drawWaypoints(painter); - // Draw status flags drawStatusFlag(2, 1, tr("ATT"), attControlEnabled, attControlKnown, painter); drawStatusFlag(22, 1, tr("PXY"), xyControlEnabled, xyControlKnown, painter); @@ -300,20 +335,25 @@ void HSIDisplay::renderOverlay() // Position QString str; str.sprintf("%05.2f %05.2f %05.2f m", x, y, z); - paintText(tr("POS"), QGC::colorCyan, 2.6f, 2, vheight- 5.0f, &painter); - paintText(str, Qt::white, 2.6f, 10, vheight - 5.0f, &painter); + paintText(tr("POS"), QGC::colorCyan, 2.6f, 2, vheight- 4.0f, &painter); + paintText(str, Qt::white, 2.6f, 10, vheight - 4.0f, &painter); } 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- 5.0f, &painter); - paintText(str, Qt::white, 2.6f, 10, vheight - 5.0f, &painter); + paintText(tr("GPS"), QGC::colorCyan, 2.6f, 2, vheight- 4.0f, &painter); + paintText(str, Qt::white, 2.6f, 10, vheight - 4.0f, &painter); } - // Draw Field of view to bottom right - paintText(tr("FOV %1 m").arg(metricWidth, 5, 'f', 1, ' '), QGC::colorCyan, 2.6f, 55, vheight- 5.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); + + // Draw status message + paintText(statusMessage, QGC::colorOrange, 2.4f, 8, 15, &painter); } void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter) @@ -333,7 +373,8 @@ void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bo painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), indicatorWidth, indicatorHeight)); paintText((status) ? tr("ON") : tr("OFF"), statusColor, 2.6f, x+7.9f, y+0.8f, &painter); // Cross out instrument if state unknown - if (!known) { + if (!known) + { QPen pen(Qt::yellow); pen.setWidth(2); painter.setPen(pen); @@ -466,13 +507,13 @@ QPointF HSIDisplay::metricBodyToWorld(QPointF body) { // First rotate into world coordinates // then translate to world position - QPointF result((cos(yaw) * body.x()) + (sin(yaw) * body.y()) + x, (-sin(yaw) * body.x()) + (cos(yaw) * body.y()) + y); + QPointF result((cos(-yaw) * body.x()) + (sin(-yaw) * body.y()) + x, (-sin(-yaw) * body.x()) + (cos(-yaw) * body.y()) + y); return result; } QPointF HSIDisplay::screenToMetricBody(QPointF ref) { - return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth - x, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth - y); + return QPointF(-((screenToRefY(ref.y()) - yCenterPos)/ vwidth) * metricWidth, ((screenToRefX(ref.x()) - xCenterPos) / vwidth) * metricWidth); } QPointF HSIDisplay::refToMetricBody(QPointF &ref) @@ -488,6 +529,16 @@ QPointF HSIDisplay::metricBodyToRef(QPointF &metric) return QPointF(((metric.y())/ metricWidth) * vwidth + xCenterPos, ((-metric.x()) / metricWidth) * vwidth + yCenterPos); } +double HSIDisplay::metricToRef(double metric) +{ + return (metric / metricWidth) * vwidth; +} + +double HSIDisplay::refToMetric(double ref) +{ + return (ref/vwidth) * metricWidth; +} + QPointF HSIDisplay::metricBodyToScreen(QPointF metric) { QPointF ref = metricBodyToRef(metric); @@ -498,30 +549,93 @@ QPointF HSIDisplay::metricBodyToScreen(QPointF metric) void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event) { - static bool dragStarted; - static float startX; - - if (event->MouseButtonDblClick) { - //setBodySetpointCoordinateXY(-refToMetric(screenToRefY(event->y()) - yCenterPos), refToMetric(screenToRefX(event->x()) - xCenterPos)); - + if (event->type() == QMouseEvent::MouseButtonDblClick) + { QPointF p = screenToMetricBody(event->posF()); setBodySetpointCoordinateXY(p.x(), p.y()); - qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos; - } else if (event->MouseButtonPress) { - startX = event->globalX(); - if (event->button() == Qt::RightButton) { +// qDebug() << "Double click at x: " << screenToRefX(event->x()) - xCenterPos << "y:" << screenToRefY(event->y()) - yCenterPos; + } +} + +void HSIDisplay::mouseReleaseEvent(QMouseEvent * event) +{ + if (mouseHasMoved) + { + if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::RightButton) + { + if (dragStarted) + { + setBodySetpointCoordinateYaw(uiYawSet); + dragStarted = false; + } + } + if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::LeftButton) + { + if (leftDragStarted) + { + setBodySetpointCoordinateZ(uiZSetCoordinate); + leftDragStarted = false; + } + } + } + mouseHasMoved = false; +} + +void HSIDisplay::mousePressEvent(QMouseEvent * event) +{ + if (event->type() == QMouseEvent::MouseButtonPress) + { + if (event->button() == Qt::RightButton) + { + startX = event->x(); // Start tracking mouse move dragStarted = true; - } else if (event->button() == Qt::LeftButton) { + } + else if (event->button() == Qt::LeftButton) + { + startY = event->y(); + leftDragStarted = true; + } + } + mouseHasMoved = false; +} +void HSIDisplay::mouseMoveEvent(QMouseEvent * event) +{ + if (event->type() == QMouseEvent::MouseMove) + { + if (dragStarted) uiYawSet -= 0.06f*(startX - event->x()) / this->frameSize().width(); + + if (leftDragStarted) + { +// uiZSetCoordinate -= 0.06f*(startY - event->y()) / this->frameSize().height(); +// setBodySetpointCoordinateZ(uiZSetCoordinate); } - } else if (event->MouseButtonRelease) { - dragStarted = false; - } else if (event->MouseMove) { - if (dragStarted) uiYawSet += (startX - event->globalX()) / this->frameSize().width(); + + if (leftDragStarted || dragStarted) mouseHasMoved = true; } } +void HSIDisplay::keyPressEvent(QKeyEvent* event) +{ + if ((event->key() == Qt::Key_Enter || event->key() == Qt::Key_Return) && actionPending) + { + actionPending = false; + statusMessage = "SETPOINT SENT"; + statusClearTimer.start(); + sendBodySetPointCoordinates(); + } + else + { + HDDisplay::keyPressEvent(event); + } +} + +void HSIDisplay::contextMenuEvent (QContextMenuEvent* event) +{ + event->ignore(); +} + void HSIDisplay::setMetricWidth(double width) { if (width != metricWidth) { @@ -593,6 +707,8 @@ 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 QPointF sp(x, y); @@ -602,21 +718,66 @@ void HSIDisplay::setBodySetpointCoordinateXY(double x, double y) qDebug() << "Attempting to set new setpoint at x: " << x << "metric y:" << y; - if (uas && mavInitialized) { - uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet); - qDebug() << "Setting new setpoint at x: " << x << "metric y:" << y; + if (uas && mavInitialized) + { + //sendBodySetPointCoordinates(); + statusMessage = "POSITION SET, PRESS TO SEND"; + actionPending = true; + statusClearTimer.start(); } } 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(); + //sendBodySetPointCoordinates(); +} + +void HSIDisplay::setBodySetpointCoordinateYaw(double yaw) +{ + 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), 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) + { + double dx = uiXSetCoordinate - uas->getLocalX(); + double dy = uiYSetCoordinate - uas->getLocalY(); + double dz = uiZSetCoordinate - uas->getLocalZ(); + bool valid = (sqrt(dx*dx + dy*dy + dz*dz) < 1.0);//UASManager::instance()->isInLocalNEDSafetyLimits(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate); + if (valid) + { + uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet); + } + else + { + setStatusMessage("REJECTED NEW SETPOINT: OUT OF BOUNDS"); + } + } } void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec) @@ -650,11 +811,13 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir setPointKnown = true; positionSetPointKnown = true; - // qDebug() << "Received setpoint at x: " << x << "metric y:" << y; - // posXSet = xDesired; - // posYSet = yDesired; - // posZSet = zDesired; - // posYawSet = yawDesired; + if (!userSetPointSet && !dragStarted) + { + uiXSetCoordinate = bodyXSetCoordinate; + uiYSetCoordinate = bodyYSetCoordinate; +// uiZSetCoordinate = bodyZSetCoordinate; + uiYawSet= bodyYawSet; + } } void HSIDisplay::updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec) @@ -748,10 +911,10 @@ QColor HSIDisplay::getColorForSNR(float snr) return color; } -void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color, QPainter &painter) +void HSIDisplay::drawSetpointXYZYaw(float x, float y, float z, float yaw, const QColor &color, QPainter &painter) { - if (setPointKnown) { - float radius = vwidth / 20.0f; + if (setPointKnown && uas) { + float radius = vwidth / 18.0f; QPen pen(color); pen.setWidthF(refLineWidthToPen(0.4f)); pen.setColor(color); @@ -762,9 +925,25 @@ void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color in = metricWorldToBody(in); // Scale from metric to screen reference coordinates QPointF p = metricBodyToRef(in); - drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter); + + QPolygonF poly(4); + // Top point + poly.replace(0, QPointF(p.x()-radius, p.y()-radius)); + // Right point + poly.replace(1, QPointF(p.x()+radius, p.y()-radius)); + // Bottom point + poly.replace(2, QPointF(p.x()+radius, p.y()+radius)); + 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); + + drawPolygon(poly, &painter); + radius *= 0.8f; - drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * 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.4f), color, &painter); + + // Draw center dot painter.setBrush(color); drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter); } @@ -772,20 +951,9 @@ void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color void HSIDisplay::drawWaypoints(QPainter& painter) { - if (uas) { + if (uas) + { const QVector& list = uas->getWaypointManager()->getWaypointList(); - // for (int i = 0; i < list.size(); i++) - // { - // QPointF in(list.at(i)->getX(), list.at(i)->getY()); - // // Transform from world to body coordinates - // in = metricWorldToBody(in); - // // Scale from metric to screen reference coordinates - // QPointF p = metricBodyToRef(in); - // Waypoint2DIcon* wp = new Waypoint2DIcon(); - // wp->setLocalPosition(list.at(i)->getX(), list.at(i)->getY()); - // wp->setPos(0, 0); - // scene()->addItem(wp); - // } QColor color; painter.setBrush(Qt::NoBrush); @@ -814,7 +982,6 @@ void HSIDisplay::drawWaypoints(QPainter& painter) painter.setBrush(Qt::NoBrush); // DRAW WAYPOINT - //drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter); float waypointSize = vwidth / 20.0f * 2.0f; QPolygonF poly(4); // Top point @@ -827,25 +994,28 @@ void HSIDisplay::drawWaypoints(QPainter& painter) // Select color based on if this is the current waypoint if (list.at(i)->getCurrent()) { - color = QGC::colorCyan;//uas->getColor(); + color = QGC::colorYellow;//uas->getColor(); pen.setWidthF(refLineWidthToPen(0.8f)); } else { - color = uas->getColor(); + color = QGC::colorCyan; pen.setWidthF(refLineWidthToPen(0.4f)); } 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()+yaw) * radius, p.y()-cos(list.at(i)->getYaw()+yaw) * radius, refLineWidthToPen(0.4f), color, &painter); + 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()) { + if (!lastWaypoint.isNull()) + { pen.setWidthF(refLineWidthToPen(0.4f)); painter.setPen(pen); - color = uas->getColor(); + color = QGC::colorCyan; drawLine(lastWaypoint.x(), lastWaypoint.y(), p.x(), p.y(), refLineWidthToPen(0.4f), color, &painter); } lastWaypoint = p; @@ -858,6 +1028,7 @@ void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRig QPen pen(color); pen.setWidthF(refLineWidthToPen(0.1f)); pen.setColor(color); + pen.setBrush(Qt::NoBrush); painter.setPen(pen); painter.drawRect(QRectF(metricBodyToScreen(metricWorldToBody(topLeft)), metricBodyToScreen(metricWorldToBody(bottomRight)))); } @@ -1026,7 +1197,7 @@ void HSIDisplay::wheelEvent(QWheelEvent* event) // Increase width -> Zoom out metricWidth -= event->delta() * zoomScale; } - metricWidth = qBound(0.1, metricWidth, 9999.0); + metricWidth = qBound(0.5, metricWidth, 9999.0); emit metricWidthChanged(metricWidth); } diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 3a899926254ec2d615420a5121d8f7e3203a86f4..82ab7821352baaf43737f3b3a9ca9ea57b0c8e8c 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -85,6 +85,14 @@ public slots: void pressKey(int key); /** @brief Reset the state of the view */ void resetMAVState(); + /** @brief Clear the status message */ + void clearStatusMessage() + { + statusMessage = ""; + if (actionPending) statusMessage = "TIMED OUT, NO ACTION"; + statusClearTimer.start(); + actionPending = false; + } signals: void metricWidthChanged(double width); @@ -101,19 +109,33 @@ protected slots: /** @brief Draw a position lock indicator */ void drawPositionLock(float x, float y, QString label, int status, bool known, QPainter& painter); void setBodySetpointCoordinateXY(double x, double y); + void setBodySetpointCoordinateYaw(double yaw); void setBodySetpointCoordinateZ(double z); /** @brief Send the current ui setpoint coordinates as new setpoint to the MAV */ void sendBodySetPointCoordinates(); /** @brief Draw one setpoint */ - void drawSetpointXY(float x, float y, float yaw, const QColor &color, QPainter &painter); + 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 the limiting safety area */ void drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRight, const QColor &color, QPainter &painter); /** @brief Receive mouse clicks */ void mouseDoubleClickEvent(QMouseEvent* event); + void mousePressEvent(QMouseEvent * event); + void mouseReleaseEvent(QMouseEvent * event); + void mouseMoveEvent(QMouseEvent * event); /** @brief Receive mouse wheel events */ void wheelEvent(QWheelEvent* event); + /** @brief Read out send keys */ + void keyPressEvent(QKeyEvent* event); + /** @brief Ignore context menu event */ + void contextMenuEvent (QContextMenuEvent* event); + /** @brief Set status message on screen */ + void setStatusMessage(const QString& message) + { + statusMessage = message; + statusClearTimer.start(); + } protected: @@ -131,6 +153,10 @@ protected: QPointF refToMetricBody(QPointF &ref); /** @brief Metric coordinates to reference coordinates */ QPointF metricBodyToRef(QPointF &metric); + /** @brief Metric length to reference coordinates */ + double metricToRef(double metric); + /** @bried Reference coordinates to metric length */ + double refToMetric(double ref); /** @brief Metric body coordinates to screen coordinates */ QPointF metricBodyToScreen(QPointF metric); QMap objectNames; @@ -138,6 +164,14 @@ protected: QMap objectQualities; QMap objectBearings; QMap objectDistances; + bool dragStarted; + bool leftDragStarted; + bool mouseHasMoved; + float startX; + float startY; + QTimer statusClearTimer; + QString statusMessage; + bool actionPending; /** * @brief Private data container class to be used within the HSI widget @@ -254,7 +288,8 @@ protected: // Data indicators bool setPointKnown; ///< Controller setpoint known status flag bool positionSetPointKnown; ///< Position setpoint known status flag - bool userSetPointSet; + bool userSetPointSet; ///< User set X, Y and Z + bool userXYSetPointSet; ///< User set the X/Y position already private: }; diff --git a/src/ui/Linechart.ui b/src/ui/Linechart.ui index b6e1df340ed20b2eab53f5713cd193a624827b38..227cef675495bbd295d6c29587c10d114b0eaec0 100644 --- a/src/ui/Linechart.ui +++ b/src/ui/Linechart.ui @@ -6,8 +6,8 @@ 0 0 - 662 - 381 + 833 + 585 @@ -25,23 +25,8 @@ Form - - - 3 - - - 2 - - - 0 - - - 2 - - - 2 - - + + @@ -114,8 +99,8 @@ 0 0 - 210 - 361 + 232 + 516 @@ -124,7 +109,7 @@ - + @@ -149,6 +134,33 @@ + + + + Short Names + + + + + + + Recolor + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc index f8a0baec8d03f7ed5d14badf59b14f76cd2d9216..2562d82017f402bd842bbc73c01773f13b509e6b 100644 --- a/src/ui/QGCToolBar.cc +++ b/src/ui/QGCToolBar.cc @@ -64,7 +64,7 @@ QGCToolBar::QGCToolBar(QWidget *parent) : toolBarBatteryBar->setMinimum(0); toolBarBatteryBar->setMaximum(100); toolBarBatteryBar->setMinimumWidth(20); - toolBarBatteryBar->setMaximumWidth(200); + toolBarBatteryBar->setMaximumWidth(100); toolBarBatteryVoltageLabel = new QLabel("xx.x V"); toolBarBatteryVoltageLabel->setStyleSheet(QString("QLabel { margin: 0px 0px 0px 4px; font: 14px; color: %1; }").arg(QColor(Qt::green).name())); symbolButton->setStyleSheet("QWidget { background-color: #050508; color: #DDDDDF; background-clip: border; } QToolButton { font-weight: bold; font-size: 12px; border: 0px solid #999999; border-radius: 5px; min-width:22px; max-width: 22px; min-height: 22px; max-height: 22px; padding: 0px; margin: 0px 0px 0px 20px; background-color: none; }"); diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index bd1d9e97cc7933d7974df18211090aba3aa17b81..8eef12ffeddbe4d19f363d78216ed365b107b637 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -210,7 +210,7 @@ void WaypointList::addCurrentPositonWaypoint() yawGlobal = last->getYaw(); } // Create global frame waypoint per default - wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), yawGlobal, acceptanceRadiusGlobal, holdTime, 0.0, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT); + wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT); uas->getWaypointManager()->addWaypoint(wp); updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint")); } diff --git a/src/ui/designer/QGCCommandButton.cc b/src/ui/designer/QGCCommandButton.cc index 6e262e3159843620ea0b5766f4d88f36dfc3a805..69989a013e395c8647381dd2e07bb487218cb16a 100644 --- a/src/ui/designer/QGCCommandButton.cc +++ b/src/ui/designer/QGCCommandButton.cc @@ -53,6 +53,7 @@ QGCCommandButton::QGCCommandButton(QWidget *parent) : ui->editCommandComboBox->addItem("CUSTOM 13", 13); ui->editCommandComboBox->addItem("CUSTOM 14", 14); ui->editCommandComboBox->addItem("CUSTOM 15", 15); + ui->editCommandComboBox->addItem("NAV_WAYPOINT", 16); ui->editCommandComboBox->setEditable(true); } @@ -75,6 +76,7 @@ void QGCCommandButton::sendCommand() int component = ui->editComponentSpinBox->value(); QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, component); + qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index; } else { qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING"; } diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index 8cdd19e350a509064f69cd16e4bd24e43f4b6717..7697cce358d6385b5375daedf262810956e105dc 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -127,6 +127,7 @@ LinechartPlot::LinechartPlot(QWidget *parent, int plotid, quint64 interval): Qwt //updateTimer->start(DEFAULT_REFRESH_RATE); connect(&timeoutTimer, SIGNAL(timeout()), this, SLOT(removeTimedOutCurves())); + //timeoutTimer.start(5000); } LinechartPlot::~LinechartPlot() @@ -208,7 +209,7 @@ void LinechartPlot::removeTimedOutCurves() foreach(QString key, lastUpdate.keys()) { quint64 time = lastUpdate.value(key); - if (QGC::groundTimeMilliseconds() - time > 20000) + if (QGC::groundTimeMilliseconds() - time > 10000) { // Remove this curve // Delete curves @@ -227,6 +228,7 @@ void LinechartPlot::removeTimedOutCurves() delete d; // Set the pointer null d = NULL; + emit curveRemoved(key); } } } diff --git a/src/ui/linechart/LinechartPlot.h b/src/ui/linechart/LinechartPlot.h index dd62e913e00cc731f1c1a8930be440fb6d90e35f..2cf4fc323c1dcd6eded2dfc3c082419907af32c9 100644 --- a/src/ui/linechart/LinechartPlot.h +++ b/src/ui/linechart/LinechartPlot.h @@ -269,6 +269,17 @@ public slots: void setAverageWindow(int windowSize); void removeTimedOutCurves(); + /** @brief Reset color map */ + void shuffleColors() + { + foreach (QwtPlotCurve* curve, curves) + { + QPen pen(curve->pen()); + pen.setColor(getNextColor()); + curve->setPen(pen); + } + } + public: QColor getColorForCurve(QString id); diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc index 023d00039df430e11018b06b3c292a9ac057f1b7..14a00b6c90ed2f96e14b95510728017ebc457809 100644 --- a/src/ui/linechart/LinechartWidget.cc +++ b/src/ui/linechart/LinechartWidget.cc @@ -76,7 +76,7 @@ LinechartWidget::LinechartWidget(int systemid, QWidget *parent) : QWidget(parent { // Add elements defined in Qt Designer ui.setupUi(this); - this->setMinimumSize(300, 200); + this->setMinimumSize(200, 150); // Add and customize curve list elements (left side) curvesWidget = new QWidget(ui.curveListWidget); @@ -104,7 +104,8 @@ LinechartWidget::LinechartWidget(int systemid, QWidget *parent) : QWidget(parent QLabel* mean; QLabel* variance; - //horizontalLayout->addWidget(checkBox); + connect(ui.recolorButton, SIGNAL(clicked()), this, SLOT(recolor())); + connect(ui.shortNameCheckBox, SIGNAL(clicked(bool)), this, SLOT(setShortNames(bool))); int labelRow = curvesWidgetLayout->rowCount(); @@ -170,6 +171,7 @@ void LinechartWidget::writeSettings() settings.beginGroup("LINECHART"); if (timeButton) settings.setValue("ENFORCE_GROUNDTIME", timeButton->isChecked()); if (unitsCheckBox) settings.setValue("SHOW_UNITS", unitsCheckBox->isChecked()); + if (ui.shortNameCheckBox) settings.setValue("SHORT_NAMES", ui.shortNameCheckBox->isChecked()); settings.endGroup(); settings.sync(); } @@ -183,7 +185,8 @@ void LinechartWidget::readSettings() timeButton->setChecked(settings.value("ENFORCE_GROUNDTIME", timeButton->isChecked()).toBool()); activePlot->enforceGroundTime(settings.value("ENFORCE_GROUNDTIME", timeButton->isChecked()).toBool()); } - if (unitsCheckBox) unitsCheckBox->setChecked(settings.value("SHOW_UNITS").toBool()); + if (unitsCheckBox) unitsCheckBox->setChecked(settings.value("SHOW_UNITS", unitsCheckBox->isChecked()).toBool()); + if (ui.shortNameCheckBox) ui.shortNameCheckBox->setChecked(settings.value("SHORT_NAMES", ui.shortNameCheckBox->isChecked()).toBool()); settings.endGroup(); } @@ -280,7 +283,8 @@ void LinechartWidget::createLayout() connect(this, SIGNAL(curveRemoved(QString)), activePlot, SLOT(hideCurve(QString))); // Update scrollbar when plot window changes (via translator method setPlotWindowPosition() - connect(activePlot, SIGNAL(windowPositionChanged(quint64)), this, SLOT(setPlotWindowPosition(quint64))); +// connect(activePlot, SIGNAL(windowPositionChanged(quint64)), this, SLOT(setPlotWindowPosition(quint64))); + connect(activePlot, SIGNAL(curveRemoved(QString)), this, SLOT(removeCurve(QString))); // Update plot when scrollbar is moved (via translator method setPlotWindowPosition() connect(this, SIGNAL(plotWindowPositionUpdated(quint64)), activePlot, SLOT(setWindowPosition(quint64))); @@ -614,6 +618,8 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit) QLabel* mean; QLabel* variance; + curveNames.insert(curve+unit, curve); + int labelRow = curvesWidgetLayout->rowCount(); checkBox = new QCheckBox(this); @@ -642,6 +648,9 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit) colorIcon->setStyleSheet(colorstyle); colorIcon->setAutoFillBackground(true); + // Label + curveNameLabels.insert(curve+unit, label); + // Value value = new QLabel(this); value->setNum(0.00); @@ -720,8 +729,96 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit) void LinechartWidget::removeCurve(QString curve) { Q_UNUSED(curve) - //TODO @todo Ensure that the button for a curve gets deleted when the original curve is deleted - // Remove name + + QWidget* widget = NULL; + widget = curveLabels->take(curve); + curvesWidgetLayout->removeWidget(widget); + widget->deleteLater(); + widget = curveMeans->take(curve); + curvesWidgetLayout->removeWidget(widget); + widget->deleteLater(); + widget = curveMedians->take(curve); + curvesWidgetLayout->removeWidget(widget); + widget->deleteLater(); + widget = curveVariances->take(curve); + curvesWidgetLayout->removeWidget(widget); + widget->deleteLater(); +// widget = colorIcons->take(curve); +// curvesWidgetLayout->removeWidget(colorIcons->take(curve)); + widget->deleteLater(); +// intData->remove(curve); +} + +void LinechartWidget::recolor() +{ + activePlot->shuffleColors(); + + foreach (QString key, colorIcons.keys()) + { + + // FIXME +// if (activePlot) + QString colorstyle; + QColor color = activePlot->getColorForCurve(key); + colorstyle = colorstyle.sprintf("QWidget { background-color: #%X%X%X; }", color.red(), color.green(), color.blue()); + QWidget* colorIcon = colorIcons.value(key, 0); + if (colorIcon) + { + colorIcon->setStyleSheet(colorstyle); + colorIcon->setAutoFillBackground(true); + } + } +} + +void LinechartWidget::setShortNames(bool enable) +{ + foreach (QString key, curveNames.keys()) + { + QString name; + if (enable) + { + QStringList parts = curveNames.value(key).split("."); + if (parts.length() > 1) + { + name = parts.at(1); + } + else + { + name = parts.at(0); + } + + const unsigned int sizeLimit = 10; + + // Replace known words with abbreviations + if (name.length() > sizeLimit) + { + name.replace("gyroscope", "gyro"); + name.replace("accelerometer", "acc"); + name.replace("magnetometer", "mag"); + name.replace("distance", "dist"); + name.replace("altitude", "alt"); + name.replace("waypoint", "wp"); + name.replace("error", "err"); + name.replace("message", "msg"); + name.replace("source", "src"); + } + + // Check if sub-part is still exceeding N chars + if (name.length() > sizeLimit) + { + name.replace("a", ""); + name.replace("e", ""); + name.replace("i", ""); + name.replace("o", ""); + name.replace("u", ""); + } + } + else + { + name = curveNames.value(key); + } + curveNameLabels.value(key)->setText(name); + } } void LinechartWidget::showEvent(QShowEvent* event) diff --git a/src/ui/linechart/LinechartWidget.h b/src/ui/linechart/LinechartWidget.h index 77b28afb28b944afbab3638dad049c21f3026909..f094eb1386a001874568278236dd49f02afe7924 100644 --- a/src/ui/linechart/LinechartWidget.h +++ b/src/ui/linechart/LinechartWidget.h @@ -73,6 +73,10 @@ public: public slots: void addCurve(const QString& curve, const QString& unit); void removeCurve(QString curve); + /** @brief Recolor all curves */ + void recolor(); + /** @brief Set short names for curves */ + void setShortNames(bool enable); /** @brief Append data without unit */ void appendData(int uasId, QString curve, double data, quint64 usec); /** @brief Append data with unit */ @@ -130,6 +134,8 @@ protected: int curveListCounter; ///< Counter of curves in curve list QList* listedCurves; ///< Curves listed QMap* curveLabels; ///< References to the curve labels + QMap curveNameLabels; ///< References to the curve labels + QMap curveNames; ///< Full curve names QMap* curveMeans; ///< References to the curve means QMap* curveMedians; ///< References to the curve medians QMap* curveVariances; ///< References to the curve variances diff --git a/src/ui/uas/UASListWidget.cc b/src/ui/uas/UASListWidget.cc index 882fa9601f3ae30936155c0515c3f75ebde8e8bf..faa136d3b0db284afd126956af5b3adb1257ef19 100644 --- a/src/ui/uas/UASListWidget.cc +++ b/src/ui/uas/UASListWidget.cc @@ -114,8 +114,8 @@ void UASListWidget::activeUAS(UASInterface* uas) void UASListWidget::removeUAS(UASInterface* uas) { - uasViews.remove(uas); - listLayout->removeWidget(uasViews.value(uas)); - uasViews.value(uas)->deleteLater(); +// uasViews.remove(uas); +// listLayout->removeWidget(uasViews.value(uas)); +// uasViews.value(uas)->deleteLater(); }