Commit 8fb8b1fa authored by pixhawk's avatar pixhawk

Implemented setpoint limitation, fixed user interaction initialization for HSI

parent 40698e44
...@@ -96,6 +96,26 @@ bool UASManager::setHomePosition(double lat, double lon, double alt) ...@@ -96,6 +96,26 @@ bool UASManager::setHomePosition(double lat, double lon, double alt)
return changed; 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) void UASManager::initReference(const double & latitude, const double & longitude, const double & altitude)
{ {
...@@ -198,6 +218,7 @@ UASManager::UASManager() : ...@@ -198,6 +218,7 @@ UASManager::UASManager() :
{ {
start(QThread::LowPriority); start(QThread::LowPriority);
loadSettings(); loadSettings();
setLocalNEDSafetyBorders(1, -1, 0, -1, 1, -1);
} }
UASManager::~UASManager() UASManager::~UASManager()
......
...@@ -92,6 +92,37 @@ public: ...@@ -92,6 +92,37 @@ public:
/** @brief Convert WGS84 lat/lon coordinates to carthesian coordinates with home position as origin */ /** @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 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); // void wgs84ToNed(const double& lat, const double& lon, const double& alt, double* north, double* east, double* down);
...@@ -188,6 +219,9 @@ public slots: ...@@ -188,6 +219,9 @@ public slots:
/** @brief Set the current home position on all UAVs*/ /** @brief Set the current home position on all UAVs*/
bool setHomePosition(double lat, double lon, double alt); 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 */ /** @brief Update home position based on the position from one of the UAVs */
void uavChangedHomePosition(int uav, double lat, double lon, double alt); void uavChangedHomePosition(int uav, double lat, double lon, double alt);
...@@ -207,6 +241,8 @@ protected: ...@@ -207,6 +241,8 @@ protected:
double homeAlt; double homeAlt;
Eigen::Quaterniond ecef_ref_orientation_; Eigen::Quaterniond ecef_ref_orientation_;
Eigen::Vector3d ecef_ref_point_; Eigen::Vector3d ecef_ref_point_;
Eigen::Vector3d nedSafetyLimitPosition1;
Eigen::Vector3d nedSafetyLimitPosition2;
void initReference(const double & latitude, const double & longitude, const double & altitude); void initReference(const double & latitude, const double & longitude, const double & altitude);
......
...@@ -125,6 +125,9 @@ HSIDisplay::HSIDisplay(QWidget *parent) : ...@@ -125,6 +125,9 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
// Set tooltip // Set tooltip
setToolTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget.")); 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.")); 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() void HSIDisplay::resetMAVState()
...@@ -272,7 +275,7 @@ void HSIDisplay::renderOverlay() ...@@ -272,7 +275,7 @@ void HSIDisplay::renderOverlay()
float setPointDist = sqrt(xSpDiff*xSpDiff + ySpDiff*ySpDiff + zSpDiff*zSpDiff); 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); QColor spColor(150, 150, 150);
drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, spColor, painter); drawSetpointXYZYaw(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet, spColor, painter);
...@@ -335,8 +338,13 @@ void HSIDisplay::renderOverlay() ...@@ -335,8 +338,13 @@ void HSIDisplay::renderOverlay()
paintText(str, Qt::white, 2.6f, 10, vheight - 4.0f, &painter); paintText(str, Qt::white, 2.6f, 10, vheight - 4.0f, &painter);
} }
// Draw Field of view to bottom right // Draw Safety
//paintText(tr("FOV %1 m").arg(metricWidth, 5, 'f', 1, ' '), QGC::colorCyan, 2.6f, 55, vheight- 5.0f, &painter); 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) 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, ...@@ -653,6 +661,7 @@ void HSIDisplay::updateSpeed(UASInterface* uas, double vx, double vy, double vz,
void HSIDisplay::setBodySetpointCoordinateXY(double x, double y) void HSIDisplay::setBodySetpointCoordinateXY(double x, double y)
{ {
userSetPointSet = true; userSetPointSet = true;
userXYSetPointSet = true;
// Set coordinates and send them out to MAV // Set coordinates and send them out to MAV
QPointF sp(x, y); QPointF sp(x, y);
...@@ -680,6 +689,19 @@ void HSIDisplay::setBodySetpointCoordinateZ(double z) ...@@ -680,6 +689,19 @@ void HSIDisplay::setBodySetpointCoordinateZ(double z)
void HSIDisplay::setBodySetpointCoordinateYaw(double yaw) 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; userSetPointSet = true;
// Set coordinates and send them out to MAV // Set coordinates and send them out to MAV
uiYawSet = atan2(sin(yaw), cos(yaw)); uiYawSet = atan2(sin(yaw), cos(yaw));
...@@ -690,7 +712,21 @@ void HSIDisplay::setBodySetpointCoordinateYaw(double yaw) ...@@ -690,7 +712,21 @@ void HSIDisplay::setBodySetpointCoordinateYaw(double yaw)
void HSIDisplay::sendBodySetPointCoordinates() void HSIDisplay::sendBodySetPointCoordinates()
{ {
// Send the coordinates to the MAV // 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) 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 ...@@ -723,6 +759,14 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir
mavInitialized = true; mavInitialized = true;
setPointKnown = true; setPointKnown = true;
positionSetPointKnown = 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) 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 ...@@ -932,6 +976,7 @@ void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRig
QPen pen(color); QPen pen(color);
pen.setWidthF(refLineWidthToPen(0.1f)); pen.setWidthF(refLineWidthToPen(0.1f));
pen.setColor(color); pen.setColor(color);
pen.setBrush(Qt::NoBrush);
painter.setPen(pen); painter.setPen(pen);
painter.drawRect(QRectF(metricBodyToScreen(metricWorldToBody(topLeft)), metricBodyToScreen(metricWorldToBody(bottomRight)))); painter.drawRect(QRectF(metricBodyToScreen(metricWorldToBody(topLeft)), metricBodyToScreen(metricWorldToBody(bottomRight))));
} }
...@@ -1100,7 +1145,7 @@ void HSIDisplay::wheelEvent(QWheelEvent* event) ...@@ -1100,7 +1145,7 @@ void HSIDisplay::wheelEvent(QWheelEvent* event)
// Increase width -> Zoom out // Increase width -> Zoom out
metricWidth -= event->delta() * zoomScale; metricWidth -= event->delta() * zoomScale;
} }
metricWidth = qBound(0.1, metricWidth, 9999.0); metricWidth = qBound(0.5, metricWidth, 9999.0);
emit metricWidthChanged(metricWidth); emit metricWidthChanged(metricWidth);
} }
......
...@@ -85,6 +85,11 @@ public slots: ...@@ -85,6 +85,11 @@ public slots:
void pressKey(int key); void pressKey(int key);
/** @brief Reset the state of the view */ /** @brief Reset the state of the view */
void resetMAVState(); void resetMAVState();
/** @brief Clear the status message */
void clearStatusMessage()
{
statusMessage = "";
}
signals: signals:
void metricWidthChanged(double width); void metricWidthChanged(double width);
...@@ -120,6 +125,12 @@ protected slots: ...@@ -120,6 +125,12 @@ protected slots:
void wheelEvent(QWheelEvent* event); void wheelEvent(QWheelEvent* event);
/** @brief Ignore context menu event */ /** @brief Ignore context menu event */
void contextMenuEvent (QContextMenuEvent* event); void contextMenuEvent (QContextMenuEvent* event);
/** @brief Set status message on screen */
void setStatusMessage(const QString& message)
{
statusMessage = message;
statusClearTimer.start();
}
protected: protected:
...@@ -150,6 +161,8 @@ protected: ...@@ -150,6 +161,8 @@ protected:
QMap<int, float> objectDistances; QMap<int, float> objectDistances;
bool dragStarted; bool dragStarted;
float startX; float startX;
QTimer statusClearTimer;
QString statusMessage;
/** /**
* @brief Private data container class to be used within the HSI widget * @brief Private data container class to be used within the HSI widget
...@@ -266,7 +279,8 @@ protected: ...@@ -266,7 +279,8 @@ protected:
// Data indicators // Data indicators
bool setPointKnown; ///< Controller setpoint known status flag bool setPointKnown; ///< Controller setpoint known status flag
bool positionSetPointKnown; ///< Position 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: private:
}; };
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment