Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
8fb8b1fa
Commit
8fb8b1fa
authored
Sep 16, 2011
by
pixhawk
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Implemented setpoint limitation, fixed user interaction initialization for HSI
parent
40698e44
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
with
122 additions
and
6 deletions
+122
-6
UASManager.cc
src/uas/UASManager.cc
+21
-0
UASManager.h
src/uas/UASManager.h
+36
-0
HSIDisplay.cc
src/ui/HSIDisplay.cc
+50
-5
HSIDisplay.h
src/ui/HSIDisplay.h
+15
-1
No files found.
src/uas/UASManager.cc
View file @
8fb8b1fa
...
@@ -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
()
...
...
src/uas/UASManager.h
View file @
8fb8b1fa
...
@@ -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
);
...
...
src/ui/HSIDisplay.cc
View file @
8fb8b1fa
...
@@ -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.
1
f
||
dragStarted
)
if
(
userSetPointSet
&&
setPointDist
>
0.
05
f
||
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.6
f
,
10
,
vheight
-
4.0
f
,
&
painter
);
paintText
(
str
,
Qt
::
white
,
2.6
f
,
10
,
vheight
-
4.0
f
,
&
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.4
f
,
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.65
f
;
}
else
if
(
!
userXYSetPointSet
&&
mavInitialized
)
{
QPointF
coord
=
metricBodyToWorld
(
QPointF
(
0.0
,
0.0
));
uiXSetCoordinate
=
coord
.
x
();
uiYSetCoordinate
=
coord
.
y
();
uiZSetCoordinate
=
-
0.65
f
;
}
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.1
f
));
pen
.
setWidthF
(
refLineWidthToPen
(
0.1
f
));
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
);
}
}
...
...
src/ui/HSIDisplay.h
View file @
8fb8b1fa
...
@@ -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:
};
};
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment