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

AC_PosControl: remove unused sqrt_controller_3D

Browse Source
This commit is contained in:
Leonard Hall 2020-12-15 23:17:25 +10:30 committed by Randy Mackay
parent 49da872218
commit b73ad1a1bc
2 changed files with 0 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -1200,23 +1200,6 @@ void AC_PosControl::check_for_ekf_z_reset()
}
}
/// Proportional controller with piecewise sqrt sections to constrain second derivative
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);
}
float linear_dist = second_ord_lim / sq(p);
float error_length = norm(error.x, error.y);
if (error_length > linear_dist) {
float first_order_scale = safe_sqrt(2.0f * second_ord_lim * (error_length - (linear_dist * 0.5f))) / error_length;
return Vector3f(error.x * first_order_scale, error.y * first_order_scale, error.z);
} else {
return Vector3f(error.x * p, error.y * p, error.z);
}
}
bool AC_PosControl::pre_arm_checks(const char *param_prefix,
char *failure_msg,
const uint8_t failure_msg_len)

3
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -365,9 +365,6 @@ 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;
/// Proportional controller with piecewise sqrt sections to constrain second derivative
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();
void check_for_ekf_xy_reset();