Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: Decay posiiton error during relax

This commit is contained in:
Leonard Hall 2022-01-31 00:45:30 +10:30 committed by Randy Mackay
parent 3eb741125c
commit c31116ebda
1 changed files with 4 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -458,7 +458,6 @@ void AC_PosControl::relax_velocity_controller_xy()
// decay resultant acceleration and therefore current attitude target to zero // decay resultant acceleration and therefore current attitude target to zero
float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC); float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC);
_accel_target.xy() *= decay; _accel_target.xy() *= decay;
_pid_vel_xy.set_integrator(_accel_target - _accel_desired); _pid_vel_xy.set_integrator(_accel_target - _accel_desired);
} }
@ -466,11 +465,11 @@ void AC_PosControl::relax_velocity_controller_xy()
/// reduce response for landing /// reduce response for landing
void AC_PosControl::soften_for_landing_xy() void AC_PosControl::soften_for_landing_xy()
{ {
// set target position to current position // decay position error to zero
_pos_target.xy() = _inav.get_position_xy_cm().topostype(); _pos_target.xy() += (_inav.get_position_xy_cm().topostype() - _pos_target.xy()) * (_dt / (_dt + POSCONTROL_RELAX_TC));
// also prevent I term build up in xy velocity controller. Note // Prevent I term build up in xy velocity controller.
// that this flag is reset on each loop, in update_xy_controller() // Note that this flag is reset on each loop in update_xy_controller()
set_externally_limited_xy(); set_externally_limited_xy();
} }