Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM: use AHRS position for geo-fence 

this allows it to take account of dead-reckoning
This commit is contained in:
Andrew Tridgell 2012-08-15 14:31:10 +10:00
parent 3f76a4cc61
commit c652538516
1 changed files with 7 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduPlane/geofence.pde
View File

@ -121,8 +121,9 @@ failed:
static bool geofence_enabled(void)
{
if (g.fence_action == FENCE_ACTION_NONE ||
g.fence_channel == 0 ||
APM_RC.InputCh(g.fence_channel-1) < FENCE_ENABLE_PWM) {
(g.fence_action != FENCE_ACTION_REPORT &&
(g.fence_channel == 0 ||
APM_RC.InputCh(g.fence_channel-1) < FENCE_ENABLE_PWM))) {
// geo-fencing is disabled
if (geofence_state != NULL) {
// re-arm for when the channel trigger is switched on
@ -131,11 +132,6 @@ static bool geofence_enabled(void)
return false;
}
if (!g_gps->fix) {
// we can't do much without a GPS fix
return false;
}
return true;
}
@ -203,6 +199,7 @@ static void geofence_check(bool altitude_check_only)
bool outside = false;
uint8_t breach_type = FENCE_BREACH_NONE;
struct Location loc;
if (geofence_check_minalt()) {
outside = true;
@ -210,10 +207,10 @@ static void geofence_check(bool altitude_check_only)
} else if (geofence_check_maxalt()) {
outside = true;
breach_type = FENCE_BREACH_MAXALT;
} else if (!altitude_check_only) {
} else if (!altitude_check_only && ahrs.get_position(&loc)) {
Vector2l location;
location.x = current_loc.lat;
location.y = current_loc.lng;
location.x = loc.lat;
location.y = loc.lng;
outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
if (outside) {
breach_type = FENCE_BREACH_BOUNDARY;