Browse Source

AC_PosControl_Sub: do not reset accel_z integrator when relaxing

Doing so caused the ROV to dive and slowly go back to the altitude
setpoint
c415-sdk
Willian Galvani 5 years ago committed by Jacob Walser
parent
commit
678fd8de1d
  1. 1
      libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp

1
libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp

@ -100,7 +100,6 @@ void AC_PosControl_Sub::relax_alt_hold_controllers()
_accel_desired.z = 0.0f; _accel_desired.z = 0.0f;
_accel_last_z_cms = 0.0f; _accel_last_z_cms = 0.0f;
_flags.reset_rate_to_accel_z = true; _flags.reset_rate_to_accel_z = true;
_pid_accel_z.set_integrator(-_motors.get_throttle_hover() * 1000.0f);
_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
_pid_accel_z.reset_filter(); _pid_accel_z.reset_filter();
} }

Loading…
Cancel
Save