aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeControl: AC_PosControl: Init desired accel to zero

Browse Source
This commit is contained in:
Leonard Hall 2022-01-24 12:43:22 +10:30 committed by Randy Mackay
parent 03cde13b55
commit 2cda59c09d
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

@ -486,9 +486,8 @@ void AC_PosControl::init_xy_controller()
_vel_desired.xy() = curr_vel;
_vel_target.xy() = curr_vel;
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);
@ -499,7 +498,9 @@ void AC_PosControl::init_xy_controller()
// 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();