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

AC_AttitudeControl: AC_PosControl: fix stopping point initialization

Browse Source
This commit is contained in:
Leonard Hall 2021-08-22 14:44:43 +09:30 committed by Andrew Tridgell
parent 4fe9a34308
commit 1c2655f565
1 changed files with 7 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -470,8 +470,8 @@ void AC_PosControl::init_xy_controller_stopping_point()
{
init_xy();
_vel_desired.xy().zero();
get_stopping_point_xy_cm(_pos_target.xy());
_vel_desired.xy().zero();
_accel_desired.xy().zero();
_pid_vel_xy.set_integrator(_accel_target);
@ -592,6 +592,7 @@ void AC_PosControl::stop_vel_xy_stabilisation()
const Vector3f &curr_vel = _inav.get_velocity();
_vel_desired.x = curr_vel.x;
_vel_desired.y = curr_vel.y;
// with zero position error _vel_target = _vel_desired
_vel_target.x = curr_vel.x;
_vel_target.y = curr_vel.y;
@ -764,7 +765,8 @@ void AC_PosControl::init_z_controller_stopping_point()
init_z_controller();
get_stopping_point_z_cm(_pos_target.z);
_vel_target.z = 0.0f;
_vel_desired.z = 0.0f;
_accel_desired.z = 0.0f;
}
// relax_z_controller - initialise the position controller to the current position and velocity with decaying acceleration.
@ -789,6 +791,7 @@ void AC_PosControl::init_z()
const Vector3f &curr_vel = _inav.get_velocity();
_vel_desired.z = curr_vel.z;
// with zero position error _vel_target = _vel_desired
_vel_target.z = curr_vel.z;
const Vector3f &curr_accel = _ahrs.get_accel_ef_blended();
@ -1026,11 +1029,6 @@ void AC_PosControl::get_stopping_point_z_cm(postype_t &stopping_point) const
const float curr_pos_z = _inav.get_position().z;
float curr_vel_z = _inav.get_velocity().z;
// if position controller is active remove the desired velocity component
if (is_active_z()) {
curr_vel_z -= _vel_desired.z;
}
// avoid divide by zero by using current position if kP is very low or acceleration is zero
if (!is_positive(_p_pos_z.kP()) || !is_positive(_accel_max_z_cmss)) {
stopping_point = curr_pos_z;
@ -1102,16 +1100,10 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
stopping_point = curr_pos.xy().topostype();
float kP = _p_pos_xy.kP();
Vector3f curr_vel = _inav.get_velocity();
// if position controller is active remove the desired velocity component
if (is_active_xy()) {
curr_vel.x -= _vel_desired.x;
curr_vel.y -= _vel_desired.y;
}
Vector2f curr_vel = _inav.get_velocity().xy();
// calculate current velocity
float vel_total = norm(curr_vel.x, curr_vel.y);
float vel_total = curr_vel.length();
if (!is_positive(vel_total)) {
return;