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

AC_PosControl: Fix slow target decay decay problem

This commit is contained in:
Leonard Hall 2022-09-01 21:48:05 +09:30 committed by Andrew Tridgell
parent 78d45a51d3
commit 649d3a3a6c
1 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -454,12 +454,12 @@ void AC_PosControl::init_xy_controller_stopping_point()
/// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
void AC_PosControl::relax_velocity_controller_xy()
{
init_xy_controller();
// decay resultant acceleration and therefore current attitude target to zero
// decay acceleration and therefore current attitude target to zero
// this will be reset by init_xy_controller() if !is_active_xy()
float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC);
_accel_target.xy() *= decay;
_pid_vel_xy.set_integrator(_accel_target - _accel_desired);
init_xy_controller();
}
/// reduce response for landing
@ -494,7 +494,9 @@ void AC_PosControl::init_xy_controller()
// 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);
if (!is_active_xy()) {
lean_angles_to_accel_xy(_accel_target.x, _accel_target.y);
}
// limit acceleration using maximum lean angles
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd());
@ -1024,9 +1026,9 @@ Vector3f AC_PosControl::lean_angles_to_accel(const Vector3f& att_target_euler) c
const float cos_yaw = cosf(att_target_euler.z);
return Vector3f{
(GRAVITY_MSS * 100) * (-cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
(GRAVITY_MSS * 100) * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
(GRAVITY_MSS * 100)
(GRAVITY_MSS * 100.0f) * (-cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
(GRAVITY_MSS * 100.0f) * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
(GRAVITY_MSS * 100.0f)
};
}