fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 21:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

geofence: can select gps instead of global position

Browse Source
This commit is contained in:
Thomas Gubler
2014-08-24 11:14:15 +02:00
parent 3c10b78e20
commit 8262739b62
5 changed files with 89 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+22 -1
View File
@@ -63,7 +63,8 @@ Geofence::Geofence() :
_altitude_max(0),
_verticesCount(0),
_param_geofence_on(this, "ON"),
_param_altitude_mode(this, "ALTMODE")
_param_altitude_mode(this, "ALTMODE"),
_param_source(this, "SOURCE")
{
/* Load initial params */
updateParams();
@@ -85,6 +86,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f
return inside(global_position.lat, global_position.lon, baro_altitude_amsl);
}
bool Geofence::inside(const struct vehicle_global_position_s &global_position,
const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) {
if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
return inside(global_position);
} else {
return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
(double)gps_position.alt * 1.0e-3);
}
} else {
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
return inside(global_position, baro_altitude_amsl);
} else {
return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
baro_altitude_amsl);
}
}
}
bool Geofence::inside(double lat, double lon, float altitude)
{
/* Return true if geofence is disabled */