Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: this fixes an issue with Z accel initialisation

Browse Source 
tailsitter quadplanes were climbing a lot on transition from fixed
wing to Q modes. Leonard tracked it down to a bug in the Z accel init
This commit is contained in:
Andrew Tridgell 2021-08-15 13:40:09 +10:00 committed by Randy Mackay
parent 03de8e752a
commit 70e041d611
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -749,10 +749,10 @@ void AC_PosControl::init_z_controller_no_descent()
init_z_controller();
// remove all descent if present
_vel_desired.z = MAX(0.0f, _vel_desired.z);
_vel_target.z = MAX(0.0f, _vel_target.z);
_accel_desired.z = MAX(GRAVITY_MSS * 100.0f, _accel_desired.z);
_accel_target.z = MAX(GRAVITY_MSS * 100.0f, _accel_target.z);
_vel_desired.z = MAX(0.0, _vel_desired.z);
_vel_target.z = MAX(0.0, _vel_target.z);
_accel_desired.z = MAX(0.0, _accel_desired.z);
_accel_target.z = MAX(0.0, _accel_target.z);
}
/// init_z_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
@ -800,7 +800,7 @@ void AC_PosControl::init_z()
_pid_vel_z.reset_filter();
_pid_vel_z.set_integrator(0.0f);
_accel_desired.z = -(curr_accel.z + GRAVITY_MSS) * 100.0f;
_accel_desired.z = constrain_float(-(curr_accel.z + GRAVITY_MSS) * 100.0f, -_accel_max_z_cmss, _accel_max_z_cmss);
_accel_target.z = -(curr_accel.z + GRAVITY_MSS) * 100.0f;
_pid_accel_z.reset_filter();