Implemented setpoint limitation, fixed user interaction initialization for HSI

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()

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);
 

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);
 }
 

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:
 };