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_AttitudeControl: revert AP_Math class change

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-05 12:34:37 +10:00
parent 8395b92309
commit 326b0b33ea
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -434,7 +434,7 @@ void AC_AttitudeControl::frame_conversion_ef_to_bf(const Vector3f& ef_vector, Ve
bool AC_AttitudeControl::frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f& ef_vector) bool AC_AttitudeControl::frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f& ef_vector)
{ {
// avoid divide by zero // avoid divide by zero
if (AP_Math::is_zero(_ahrs.cos_pitch())) { if (is_zero(_ahrs.cos_pitch())) {
return false; return false;
} }
// convert earth frame angle or rates to body frame // convert earth frame angle or rates to body frame
@ -672,14 +672,14 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits)
{ {
if (enable_limits) { if (enable_limits) {
// if enabling limits, reload from eeprom or set to defaults // if enabling limits, reload from eeprom or set to defaults
if (AP_Math::is_zero(_accel_roll_max)) { if (is_zero(_accel_roll_max)) {
_accel_roll_max.load(); _accel_roll_max.load();
} }
// if enabling limits, reload from eeprom or set to defaults // if enabling limits, reload from eeprom or set to defaults
if (AP_Math::is_zero(_accel_pitch_max)) { if (is_zero(_accel_pitch_max)) {
_accel_pitch_max.load(); _accel_pitch_max.load();
} }
if (AP_Math::is_zero(_accel_yaw_max)) { if (is_zero(_accel_yaw_max)) {
_accel_yaw_max.load(); _accel_yaw_max.load();
} }
} else { } else {
@ -742,7 +742,7 @@ float AC_AttitudeControl::get_boosted_throttle(float throttle_in)
// sqrt_controller - response based on the sqrt of the error instead of the more common linear response // sqrt_controller - response based on the sqrt of the error instead of the more common linear response
float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim) float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim)
{ {
if (AP_Math::is_zero(second_ord_lim) || AP_Math::is_zero(p)) { if (is_zero(second_ord_lim) || is_zero(p)) {
return error*p; return error*p;
} }

2
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -505,7 +505,7 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
float vel_total = pythagorous2(curr_vel.x, 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 // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
if (kP <= 0.0f || _accel_cms <= 0.0f || AP_Math::is_zero(vel_total)) { if (kP <= 0.0f || _accel_cms <= 0.0f || is_zero(vel_total)) {
stopping_point.x = curr_pos.x; stopping_point.x = curr_pos.x;
stopping_point.y = curr_pos.y; stopping_point.y = curr_pos.y;
return; return;