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

AC_PosControl: replace safe_sqrt with pythagorous2

This commit is contained in:
Randy Mackay 2014-04-01 20:40:08 +09:00
parent 15da01cf3a
commit 90e205c905
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -394,7 +394,7 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
float kP = _p_pos_xy.kP();
// calculate current velocity
float vel_total = safe_sqrt(curr_vel.x*curr_vel.x + curr_vel.y*curr_vel.y);
float vel_total = pythagorous2(curr_vel.x, curr_vel.y);
// avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
if (vel_total < 10.0f || kP <= 0.0f || _accel_cms <= 0.0f) {
@ -502,7 +502,7 @@ void AC_PosControl::desired_vel_to_pos(float nav_dt)
}
// constrain and scale the desired velocity
vel_desired_total = safe_sqrt(_vel_desired.x*_vel_desired.x + _vel_desired.y*_vel_desired.y);
vel_desired_total = pythagorous2(_vel_desired.x, _vel_desired.y);
if (vel_desired_total > _speed_cms && vel_desired_total > 0.0f) {
_vel_desired.x = _speed_cms * _vel_desired.x/vel_desired_total;
_vel_desired.y = _speed_cms * _vel_desired.y/vel_desired_total;
@ -534,7 +534,7 @@ void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt)
_pos_error.y = _pos_target.y - curr_pos.y;
// constrain target position to within reasonable distance of current location
_distance_to_target = safe_sqrt(_pos_error.x*_pos_error.x + _pos_error.y*_pos_error.y);
_distance_to_target = pythagorous2(_pos_error.x, _pos_error.y);
if (_distance_to_target > _leash && _distance_to_target > 0.0f) {
_pos_target.x = curr_pos.x + _leash * _pos_error.x/_distance_to_target;
_pos_target.y = curr_pos.y + _leash * _pos_error.y/_distance_to_target;
@ -569,7 +569,7 @@ void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt)
}
// scale velocity to stays within limits
float vel_total = safe_sqrt(_vel_target.x*_vel_target.x + _vel_target.y*_vel_target.y);
float vel_total = pythagorous2(_vel_target.x, _vel_target.y);
if (vel_total > vel_max_from_pos_error) {
_vel_target.x = vel_max_from_pos_error * _vel_target.x/vel_total;
_vel_target.y = vel_max_from_pos_error * _vel_target.y/vel_total;
@ -618,7 +618,7 @@ void AC_PosControl::rate_to_accel_xy(float dt)
// scale desired acceleration if it's beyond acceptable limit
// To-Do: move this check down to the accel_to_lean_angle method?
accel_total = safe_sqrt(_accel_target.x*_accel_target.x + _accel_target.y*_accel_target.y);
accel_total = pythagorous2(_accel_target.x, _accel_target.y);
if (accel_total > POSCONTROL_ACCEL_XY_MAX) {
_accel_target.x = POSCONTROL_ACCEL_XY_MAX * _accel_target.x/accel_total;
_accel_target.y = POSCONTROL_ACCEL_XY_MAX * _accel_target.y/accel_total;