Browse Source

Plane: Quadplane: always reset to QPOS_NONE on mode entry

apm_2208
Iampete1 3 years ago committed by Peter Barker
parent
commit
0cd1f605f6
  1. 3
      ArduPlane/mode_qrtl.cpp
  2. 1
      ArduPlane/quadplane.cpp

3
ArduPlane/mode_qrtl.cpp

@ -14,9 +14,6 @@ bool ModeQRTL::_enter() @@ -14,9 +14,6 @@ bool ModeQRTL::_enter()
submode = SubMode::RTL;
plane.prev_WP_loc = plane.current_loc;
// clear QPOS state
quadplane.poscontrol.set_state(QuadPlane::QPOS_NONE);
const int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL;
if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
// VTOL motors are active, either in VTOL flight or assisted flight

1
ArduPlane/quadplane.cpp

@ -4137,6 +4137,7 @@ void QuadPlane::mode_enter(void) @@ -4137,6 +4137,7 @@ void QuadPlane::mode_enter(void)
poscontrol.xy_correction.zero();
poscontrol.velocity_match.zero();
poscontrol.last_velocity_match_ms = 0;
poscontrol.set_state(QuadPlane::QPOS_NONE);
// clear guided takeoff wait on any mode change, but remember the
// state for special behaviour

Loading…
Cancel
Save