mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
Plane: fixed previous_mode error in geofence
when we were in MANUAL we would switch back to STABILIZE
This commit is contained in:
parent
613fda4889
commit
14671d0bcd
@ -352,12 +352,13 @@ static void geofence_check(bool altitude_check_only)
|
||||
|
||||
set_guided_WP();
|
||||
|
||||
if (control_mode == MANUAL && g.auto_trim) {
|
||||
// make sure we don't auto trim the surfaces on this change
|
||||
control_mode = STABILIZE;
|
||||
}
|
||||
// make sure we don't auto trim the surfaces on this mode change
|
||||
int8_t saved_auto_trim = g.auto_trim;
|
||||
g.auto_trim.set(0);
|
||||
|
||||
set_mode(GUIDED);
|
||||
g.auto_trim.set(saved_auto_trim);
|
||||
|
||||
if (g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) {
|
||||
guided_throttle_passthru = true;
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user