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

AC_PosControl: use AP_Math control functions

This commit is contained in:
Randy Mackay 2021-01-20 10:37:19 +09:00 committed by Andrew Tridgell
parent 1129a3fd59
commit 6320338771
2 changed files with 4 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -568,7 +568,7 @@ void AC_PosControl::run_z_controller()
}
// calculate _vel_target.z using from _pos_error.z using sqrt controller
_vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt);
_vel_target.z = sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt);
// check speed limits
// To-Do: check these speed limits here or in the pos->rate controller
@ -1037,7 +1037,7 @@ void AC_PosControl::run_xy_controller(float dt)
_pos_target.y = curr_pos.y + _pos_error.y;
}
_vel_target = sqrt_controller(_pos_error, kP, _accel_cms);
_vel_target = sqrt_controller_3D(_pos_error, kP, _accel_cms);
}
// add velocity feed-forward
@ -1206,20 +1206,8 @@ void AC_PosControl::check_for_ekf_z_reset()
}
}
/// limit vector to a given length, returns true if vector was limited
bool AC_PosControl::limit_vector_length(float& vector_x, float& vector_y, float max_length)
{
float vector_length = norm(vector_x, vector_y);
if ((vector_length > max_length) && is_positive(vector_length)) {
vector_x *= (max_length / vector_length);
vector_y *= (max_length / vector_length);
return true;
}
return false;
}
/// Proportional controller with piecewise sqrt sections to constrain second derivative
Vector3f AC_PosControl::sqrt_controller(const Vector3f& error, float p, float second_ord_lim)
Vector3f AC_PosControl::sqrt_controller_3D(const Vector3f& error, float p, float second_ord_lim)
{
if (second_ord_lim < 0.0f || is_zero(second_ord_lim) || is_zero(p)) {
return Vector3f(error.x * p, error.y * p, error.z);

5
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -362,11 +362,8 @@ protected:
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float calc_leash_length(float speed_cms, float accel_cms, float kP) const;
/// limit vector to a given length, returns true if vector was limited
static bool limit_vector_length(float& vector_x, float& vector_y, float max_length);
/// Proportional controller with piecewise sqrt sections to constrain second derivative
static Vector3f sqrt_controller(const Vector3f& error, float p, float second_ord_lim);
static Vector3f sqrt_controller_3D(const Vector3f& error, float p, float second_ord_lim);
/// initialise and check for ekf position resets
void init_ekf_xy_reset();