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; _vel_target.y = curr_vel.y;
const Vector3f &curr_accel = _ahrs.get_accel_ef_blended() * 100.0f; // Set desired accel to zero because raw acceleration is prone to noise
_accel_desired.xy() = curr_accel.xy(); _accel_desired.xy().zero();
_accel_desired.xy().limit_length(_accel_max_xy_cmss);
lean_angles_to_accel_xy(_accel_target.x, _accel_target.y); lean_angles_to_accel_xy(_accel_target.x, _accel_target.y);
// initialise I terms from lean angles // initialise I terms from lean angles
_pid_vel_xy.reset_filter(); _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 // initialise ekf xy reset handler
init_ekf_xy_reset(); init_ekf_xy_reset();