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

AC_AttitudeControl: AC_PosControl: Init desired accel to zero

This commit is contained in:
Leonard Hall 2022-01-24 12:43:22 +10:30 committed by Randy Mackay
parent 0d035891fe
commit 550a4a9c1a
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -509,15 +509,16 @@ void AC_PosControl::init_xy()
_vel_target.y = curr_vel.y;
const Vector3f &curr_accel = _ahrs.get_accel_ef_blended() * 100.0f;
_accel_desired.xy() = curr_accel.xy();
_accel_desired.xy().limit_length(_accel_max_xy_cmss);
// Set desired accel to zero because raw acceleration is prone to noise
_accel_desired.xy().zero();
lean_angles_to_accel_xy(_accel_target.x, _accel_target.y);
// initialise I terms from lean angles
_pid_vel_xy.reset_filter();
_pid_vel_xy.set_integrator(_accel_target - _accel_desired);
// initialise the I term to _accel_target - _accel_desired
// _accel_desired is zero and can be removed from the equation
_pid_vel_xy.set_integrator(_accel_target);
// initialise ekf xy reset handler
init_ekf_xy_reset();