aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

geofence: enable return to old control mode via FENCE_CHANNEL

Browse Source 
if you disable fencing with the fence channel switch while still in
GUIDED mode to the return point, then reset the control mode back to
the transmitter control mode
This commit is contained in:
Andrew Tridgell 2011-12-17 15:15:41 +11:00
parent 04150823e2
commit ce035fc324
1 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduPlane/geofence.pde
View File

@ -22,6 +22,7 @@ static struct geofence_state {
uint16_t breach_count;
uint8_t breach_type;
uint32_t breach_time;
byte old_switch_position;
/* point 0 is the return point */
Vector2l boundary[MAX_FENCEPOINTS];
} *geofence_state;
@ -173,6 +174,20 @@ static bool geofence_check_maxalt(void)
static void geofence_check(bool altitude_check_only)
{
if (!geofence_enabled()) {
// switch back to the chosen control mode if still in
// GUIDED to the return point
if (geofence_state != NULL &&
g.fence_action == FENCE_ACTION_GUIDED &&
g.fence_channel != 0 &&
control_mode == GUIDED &&
g.fence_total >= 5 &&
geofence_state->boundary_uptodate &&
geofence_state->old_switch_position == oldSwitchPosition &&
guided_WP.lat == geofence_state->boundary[0].x &&
guided_WP.lng == geofence_state->boundary[0].y) {
geofence_state->old_switch_position = 0;
reset_control_switch();
}
return;
}
@ -254,6 +269,8 @@ static void geofence_check(bool altitude_check_only)
guided_WP.lat = geofence_state->boundary[0].x;
guided_WP.lng = geofence_state->boundary[0].y;
geofence_state->old_switch_position = oldSwitchPosition;
if (control_mode == MANUAL && g.auto_trim) {
// make sure we don't auto trim the surfaces on this change
control_mode = STABILIZE;