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