From 1bca81eaed774e0cc2cf5c6525f0213d3c76c557 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Sat, 2 May 2015 02:34:54 -0700 Subject: [PATCH] AC_AttitudeControl: compiler warnings: apply is_zero(float) or is_equal(float) and float to doubles --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 11 +++++----- .../AC_AttitudeControl/AC_PosControl.cpp | 21 ++++++++++--------- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 2a9d9817f0..cfb87d63a7 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -2,6 +2,7 @@ #include "AC_AttitudeControl.h" #include +#include // table of user settable parameters const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = { @@ -433,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) { // avoid divide by zero - if (_ahrs.cos_pitch() == 0.0f) { + if (AP_Math::is_zero(_ahrs.cos_pitch())) { return false; } // convert earth frame angle or rates to body frame @@ -671,14 +672,14 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits) { if (enable_limits) { // if enabling limits, reload from eeprom or set to defaults - if (_accel_roll_max == 0.0f) { + if (AP_Math::is_zero(_accel_roll_max)) { _accel_roll_max.load(); } // if enabling limits, reload from eeprom or set to defaults - if (_accel_pitch_max == 0.0f) { + if (AP_Math::is_zero(_accel_pitch_max)) { _accel_pitch_max.load(); } - if (_accel_yaw_max == 0.0f) { + if (AP_Math::is_zero(_accel_yaw_max)) { _accel_yaw_max.load(); } } else { @@ -741,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 float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim) { - if (second_ord_lim == 0.0f || p == 0.0f) { + if (AP_Math::is_zero(second_ord_lim) || AP_Math::is_zero(p)) { return error*p; } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 2b664e8b49..6c41c0895a 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -1,6 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include #include +#include extern const AP_HAL::HAL& hal; @@ -99,9 +100,9 @@ void AC_PosControl::set_dt_xy(float dt_xy) void AC_PosControl::set_speed_z(float speed_down, float speed_up) { // ensure speed_down is always negative - speed_down = (float)-fabs(speed_down); + speed_down = -fabsf(speed_down); - if (((float)fabs(_speed_down_cms-speed_down) > 1.0f) || ((float)fabs(_speed_up_cms-speed_up) > 1.0f)) { + if ((fabsf(_speed_down_cms-speed_down) > 1.0f) || (fabsf(_speed_up_cms-speed_up) > 1.0f)) { _speed_down_cms = speed_down; _speed_up_cms = speed_up; _flags.recalc_leash_z = true; @@ -111,7 +112,7 @@ void AC_PosControl::set_speed_z(float speed_down, float speed_up) /// set_accel_z - set vertical acceleration in cm/s/s void AC_PosControl::set_accel_z(float accel_cmss) { - if ((float)fabs(_accel_z_cms-accel_cmss) > 1.0f) { + if (fabsf(_accel_z_cms-accel_cmss) > 1.0f) { _accel_z_cms = accel_cmss; _flags.recalc_leash_z = true; } @@ -146,7 +147,7 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d // jerk_z is calculated to reach full acceleration in 1000ms. float jerk_z = _accel_z_cms * POSCONTROL_JERK_RATIO; - float accel_z_max = min(_accel_z_cms, safe_sqrt(2.0f*fabs(_vel_desired.z - climb_rate_cms)*jerk_z)); + float accel_z_max = min(_accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z)); _accel_last_z_cms += jerk_z * dt; _accel_last_z_cms = min(accel_z_max, _accel_last_z_cms); @@ -224,7 +225,7 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const // calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function linear_velocity = _accel_z_cms/_p_pos_z.kP(); - if ((float)fabs(curr_vel_z) < linear_velocity) { + if (fabsf(curr_vel_z) < linear_velocity) { // if our current velocity is below the cross-over point we use a linear function stopping_point.z = curr_pos_z + curr_vel_z/_p_pos_z.kP(); } else { @@ -436,7 +437,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z) /// calc_leash_length_xy should be called afterwards void AC_PosControl::set_accel_xy(float accel_cmss) { - if ((float)fabs(_accel_cms-accel_cmss) > 1.0f) { + if (fabsf(_accel_cms-accel_cmss) > 1.0f) { _accel_cms = accel_cmss; _flags.recalc_leash_xy = true; } @@ -446,7 +447,7 @@ void AC_PosControl::set_accel_xy(float accel_cmss) /// calc_leash_length_xy should be called afterwards void AC_PosControl::set_speed_xy(float speed_cms) { - if ((float)fabs(_speed_cms-speed_cms) > 1.0f) { + if (fabsf(_speed_cms-speed_cms) > 1.0f) { _speed_cms = speed_cms; _flags.recalc_leash_xy = true; } @@ -504,7 +505,7 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const 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 (kP <= 0.0f || _accel_cms <= 0.0f || vel_total == 0.0f) { + if (kP <= 0.0f || _accel_cms <= 0.0f || AP_Math::is_zero(vel_total)) { stopping_point.x = curr_pos.x; stopping_point.y = curr_pos.y; return; @@ -875,9 +876,9 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler) accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw(); // update angle targets that will be passed to stabilize controller - _pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max); + _pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/(float)M_PI),-lean_angle_max, lean_angle_max); float cos_pitch_target = cosf(_pitch_target*(float)M_PI/18000); - _roll_target = constrain_float(fast_atan(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max); + _roll_target = constrain_float(fast_atan(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/(float)M_PI), -lean_angle_max, lean_angle_max); } // get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s