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

AC_PosControl: Fix slow target decay decay problem

Browse Source
This commit is contained in:
Leonard Hall 2023-03-13 20:09:37 +09:00 committed by Andrew Tridgell
parent f104d35d57
commit bd04635b08
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -472,14 +472,13 @@ 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. /// 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() void AC_PosControl::relax_velocity_controller_xy()
{ {
init_xy();
// decay resultant acceleration and therefore current attitude target to zero // decay resultant acceleration and therefore current attitude target to zero
float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC); float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC);
_accel_target.x *= decay; _accel_target.x *= decay;
_accel_target.y *= decay; _accel_target.y *= decay;
_pid_vel_xy.set_integrator(_accel_target - _accel_desired);
init_xy();
} }
/// init_xy - initialise the position controller to the current position, velocity and acceleration. /// init_xy - initialise the position controller to the current position, velocity and acceleration.
@ -507,7 +506,9 @@ void AC_PosControl::init_xy()
// Set desired accel to zero because raw acceleration is prone to noise // Set desired accel to zero because raw acceleration is prone to noise
_accel_desired.xy().zero(); _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);
}
// initialise I terms from lean angles // initialise I terms from lean angles
_pid_vel_xy.reset_filter(); _pid_vel_xy.reset_filter();
@ -1061,9 +1062,9 @@ Vector3f AC_PosControl::lean_angles_to_accel(const Vector3f& att_target_euler) c
const float cos_yaw = cosf(att_target_euler.z); const float cos_yaw = cosf(att_target_euler.z);
return Vector3f{ 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.0f) * (-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.0f) * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
(GRAVITY_MSS * 100) (GRAVITY_MSS * 100.0f)
}; };
} }