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 444fe958f59d361f45ddf7991e370ccace9d2584..dceae05f6953b4d1352675ee470b5878b52a440d 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -125,6 +125,9 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
// 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(5000);
}
void HSIDisplay::resetMAVState()
@@ -272,7 +275,7 @@ void HSIDisplay::renderOverlay()
float setPointDist = sqrt(xSpDiff*xSpDiff + ySpDiff*ySpDiff + zSpDiff*zSpDiff);
- if (userSetPointSet && setPointDist > 0.1f || dragStarted)
+ if (userSetPointSet && setPointDist > 0.05f || dragStarted)
{
QColor spColor(150, 150, 150);
drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, spColor, painter);
@@ -335,8 +338,13 @@ void HSIDisplay::renderOverlay()
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)
@@ -653,6 +661,7 @@ 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);
@@ -680,6 +689,19 @@ void HSIDisplay::setBodySetpointCoordinateZ(double z)
void HSIDisplay::setBodySetpointCoordinateYaw(double yaw)
{
+ if (!userXYSetPointSet && setPointKnown)
+ {
+ uiXSetCoordinate = bodyXSetCoordinate;
+ uiYSetCoordinate = bodyYSetCoordinate;
+ uiZSetCoordinate = -0.65f;
+ }
+ else if (!userXYSetPointSet && mavInitialized)
+ {
+ QPointF coord = metricBodyToWorld(QPointF(0.0, 0.0));
+ uiXSetCoordinate = coord.x();
+ uiYSetCoordinate = coord.y();
+ uiZSetCoordinate = -0.65f;
+ }
userSetPointSet = true;
// Set coordinates and send them out to MAV
uiYawSet = atan2(sin(yaw), cos(yaw));
@@ -690,7 +712,21 @@ void HSIDisplay::setBodySetpointCoordinateYaw(double yaw)
void HSIDisplay::sendBodySetPointCoordinates()
{
// Send the coordinates to the MAV
- if (uas && mavInitialized) uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
+ 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)
@@ -723,6 +759,14 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir
mavInitialized = true;
setPointKnown = true;
positionSetPointKnown = true;
+
+ if (!userSetPointSet && !dragStarted)
+ {
+ uiXSetCoordinate = bodyXSetCoordinate;
+ uiYSetCoordinate = bodyYSetCoordinate;
+ uiZSetCoordinate = bodyZSetCoordinate;
+ uiYawSet= bodyYawSet;
+ }
}
void HSIDisplay::updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec)
@@ -932,6 +976,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))));
}
@@ -1100,7 +1145,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 25d15ad259c42e00c5c5ef96f31cd5852b0d3edf..fe535eb07632c82d8e3affdb640c2ad4577cfd70 100644
--- a/src/ui/HSIDisplay.h
+++ b/src/ui/HSIDisplay.h
@@ -85,6 +85,11 @@ public slots:
void pressKey(int key);
/** @brief Reset the state of the view */
void resetMAVState();
+ /** @brief Clear the status message */
+ void clearStatusMessage()
+ {
+ statusMessage = "";
+ }
signals:
void metricWidthChanged(double width);
@@ -120,6 +125,12 @@ protected slots:
void wheelEvent(QWheelEvent* 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:
@@ -150,6 +161,8 @@ protected:
QMap<int, float> objectDistances;
bool dragStarted;
float startX;
+ QTimer statusClearTimer;
+ QString statusMessage;
/**
* @brief Private data container class to be used within the HSI widget
@@ -266,7 +279,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:
};