From ba83950fc4cf58375c3b6b6628c298119f88a8f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 May 2013 10:25:40 +1000 Subject: [PATCH] libraries: replace constrain() with constrain_float() this makes the type much more obvious. Thanks to Tobias for the suggestion. --- libraries/AC_WPNav/AC_WPNav.cpp | 10 +++++----- libraries/APM_Control/AP_PitchController.cpp | 4 ++-- libraries/AP_AHRS/AP_AHRS.cpp | 10 +++++----- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 10 +++++----- libraries/AP_AHRS/AP_AHRS_MPU6000.cpp | 4 ++-- libraries/AP_Compass/Compass.cpp | 2 +- libraries/AP_Declination/AP_Declination.cpp | 4 ++-- libraries/AP_L1_Control/AP_L1_Control.cpp | 10 +++++----- libraries/AP_Math/AP_Math.cpp | 4 ++-- libraries/AP_Math/AP_Math.h | 2 +- .../AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp | 2 +- libraries/AP_RangeFinder/RangeFinder.cpp | 2 +- libraries/PID/PID.cpp | 2 +- 13 files changed, 33 insertions(+), 33 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 4791d1f055..663e0c9d9c 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -92,7 +92,7 @@ void AC_WPNav::project_stopping_point(const Vector3f& position, const Vector3f& linear_distance = MAX_LOITER_POS_ACCEL/(2*_pid_pos_lat->kP()*_pid_pos_lat->kP()); target_dist = linear_distance + (vel_total*vel_total)/(2*MAX_LOITER_POS_ACCEL); } - target_dist = constrain(target_dist, 0, MAX_LOITER_OVERSHOOT); + target_dist = constrain_float(target_dist, 0, MAX_LOITER_OVERSHOOT); target.x = position.x + (target_dist * velocity.x / vel_total); target.y = position.y + (target_dist * velocity.y / vel_total); @@ -275,7 +275,7 @@ void AC_WPNav::advance_target_along_track(float dt) track_desired_temp = track_desired_max; } // do not let desired point go past the end of the segment - track_desired_temp = constrain(track_desired_temp, 0, _track_length); + track_desired_temp = constrain_float(track_desired_temp, 0, _track_length); _track_desired = max(_track_desired, track_desired_temp); // recalculate the desired position @@ -354,7 +354,7 @@ void AC_WPNav::get_loiter_position_to_velocity(float dt) dist_error_total = safe_sqrt(dist_error.x*dist_error.x + dist_error.y*dist_error.y); if( dist_error_total > 2*linear_distance ) { - vel_sqrt = constrain(safe_sqrt(2*MAX_LOITER_POS_ACCEL*(dist_error_total-linear_distance)),0,1000); + vel_sqrt = constrain_float(safe_sqrt(2*MAX_LOITER_POS_ACCEL*(dist_error_total-linear_distance)),0,1000); desired_vel.x = vel_sqrt * dist_error.x/dist_error_total; desired_vel.y = vel_sqrt * dist_error.y/dist_error_total; }else{ @@ -428,8 +428,8 @@ void AC_WPNav::get_loiter_acceleration_to_lean_angles(float accel_lat, float acc accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw; // update angle targets that will be passed to stabilize controller - _desired_roll = constrain((accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max); - _desired_pitch = constrain((-accel_forward/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max); + _desired_roll = constrain_float((accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max); + _desired_pitch = constrain_float((-accel_forward/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max); } // get_bearing_cd - return bearing in centi-degrees between two positions diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index b6566f2c96..0a4cd51c7d 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -52,7 +52,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab (delta_time / (RC + delta_time)) * (rate - _last_rate); _last_rate = rate; - float roll_scaler = 1/constrain(cosf(_ahrs->roll),0.33f,1); + float roll_scaler = 1/constrain_float(cosf(_ahrs->roll),0.33f,1); int32_t desired_rate = angle_err * _kp_angle; @@ -76,7 +76,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab else if(roll_ff < 0) roll_ff = 0; - float out = constrain(((rate_error * _kp_rate) + (desired_rate * _kp_ff) + roll_ff) * scaler,-4500,4500); + float out = constrain_float(((rate_error * _kp_rate) + (desired_rate * _kp_ff) + roll_ff) * scaler,-4500,4500); //rate integrator if (!stabilize) { diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 04561e25c4..2a88409b0b 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -108,7 +108,7 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret) if (_wind_max > 0 && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D) { // constrain the airspeed by the ground speed // and AHRS_WIND_MAX - *airspeed_ret = constrain(*airspeed_ret, + *airspeed_ret = constrain_float(*airspeed_ret, _gps->ground_speed*0.01f - _wind_max, _gps->ground_speed*0.01f + _wind_max); } @@ -121,8 +121,8 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret) void AP_AHRS::set_trim(Vector3f new_trim) { Vector3f trim; - trim.x = constrain(new_trim.x, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)); - trim.y = constrain(new_trim.y, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)); + trim.x = constrain_float(new_trim.x, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)); + trim.y = constrain_float(new_trim.y, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)); _trim.set_and_save(trim); } @@ -132,8 +132,8 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_ Vector3f trim = _trim.get(); // add new trim - trim.x = constrain(trim.x + roll_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)); - trim.y = constrain(trim.y + pitch_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)); + trim.x = constrain_float(trim.x + roll_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)); + trim.y = constrain_float(trim.y + pitch_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)); // set new trim values _trim.set(trim); diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 7d86532d2d..b72cf38072 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -509,7 +509,7 @@ AP_AHRS_DCM::drift_correction(float deltat) Vector3f vdelta = (velocity - _last_velocity) * v_scale; // limit vertical acceleration correction to 0.5 gravities. The // barometer sometimes gives crazy acceleration changes. - vdelta.z = constrain(vdelta.z, -0.5f, 0.5f); + vdelta.z = constrain_float(vdelta.z, -0.5f, 0.5f); GA_e = Vector3f(0, 0, -1.0f) + vdelta; GA_e.normalize(); if (GA_e.is_inf()) { @@ -605,9 +605,9 @@ AP_AHRS_DCM::drift_correction(float deltat) // short term errors don't cause a buildup of omega_I // beyond the physical limits of the device float change_limit = _gyro_drift_limit * _omega_I_sum_time; - _omega_I_sum.x = constrain(_omega_I_sum.x, -change_limit, change_limit); - _omega_I_sum.y = constrain(_omega_I_sum.y, -change_limit, change_limit); - _omega_I_sum.z = constrain(_omega_I_sum.z, -change_limit, change_limit); + _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit); + _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit); + _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit); _omega_I += _omega_I_sum; _omega_I_sum.zero(); _omega_I_sum_time = 0; @@ -763,7 +763,7 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) if (ret && _wind_max > 0 && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D) { // constrain the airspeed by the ground speed // and AHRS_WIND_MAX - *airspeed_ret = constrain(*airspeed_ret, + *airspeed_ret = constrain_float(*airspeed_ret, _gps->ground_speed*0.01f - _wind_max, _gps->ground_speed*0.01f + _wind_max); } diff --git a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp index e8194b12ae..87a1708d4b 100644 --- a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp +++ b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp @@ -129,9 +129,9 @@ void AP_AHRS_MPU6000::drift_correction( float deltat ) // 0.65*0.04 / 0.005 = 5.2 float drift_limit = _mpu6000->get_gyro_drift_rate() * deltat / _gyro_bias_from_gravity_gain; - errorRollPitch[0] = constrain(errorRollPitch[0], + errorRollPitch[0] = constrain_float(errorRollPitch[0], -drift_limit, drift_limit); - errorRollPitch[1] = constrain(errorRollPitch[1], + errorRollPitch[1] = constrain_float(errorRollPitch[1], -drift_limit, drift_limit); // We correct gyroX and gyroY bias using the error vector diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index fce9e57292..4059c8c7aa 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -239,7 +239,7 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch; // magnetic heading // 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S. - heading = constrain(atan2f(-headY,headX), -3.15f, 3.15f); + heading = constrain_float(atan2f(-headY,headX), -3.15f, 3.15f); // Declination correction (if supplied) if( fabsf(_declination) > 0.0f ) diff --git a/libraries/AP_Declination/AP_Declination.cpp b/libraries/AP_Declination/AP_Declination.cpp index 8d83294f54..4507bad5b0 100644 --- a/libraries/AP_Declination/AP_Declination.cpp +++ b/libraries/AP_Declination/AP_Declination.cpp @@ -115,8 +115,8 @@ AP_Declination::get_declination(float lat, float lon) float decmin, decmax; // Constrain to valid inputs - lat = constrain(lat, -90, 90); - lon = constrain(lon, -180, 180); + lat = constrain_float(lat, -90, 90); + lon = constrain_float(lon, -180, 180); latmin = floorf(lat/5)*5; lonmin = floorf(lon/5)*5; diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index e8b9087ea5..01da83c9e2 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -39,7 +39,7 @@ int32_t AP_L1_Control::nav_roll_cd(void) { float ret; ret = degrees(atanf(_latAccDem * 0.101972f) * 100.0f); // 0.101972 = 1/9.81 - ret = constrain(ret, -9000, 9000); + ret = constrain_float(ret, -9000, 9000); return ret; } @@ -143,7 +143,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct float xtrackErr = _cross2D(A_air, AB_unit); float sine_Nu1 = xtrackErr/_maxf(_L1_dist , 0.1f); //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg - sine_Nu1 = constrain(sine_Nu1, -0.7854f, 0.7854f); + sine_Nu1 = constrain_float(sine_Nu1, -0.7854f, 0.7854f); float Nu1 = asinf(sine_Nu1); //Calculate Nu @@ -159,7 +159,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct } //Limit Nu to +-pi - Nu = constrain(Nu, -1.5708f, +1.5708f); + Nu = constrain_float(Nu, -1.5708f, +1.5708f); _latAccDem = (xtrackVel*dampingWeight*Kv + 2.0f*sinf(Nu))*VomegaA; // Waypoint capture status is always false during waypoint following @@ -210,7 +210,7 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius float xtrackVelCap = _cross2D(A_air_unit , _groundspeed_vector); // Velocity across line - perpendicular to radial inbound to WP float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP float Nu = atan2f(xtrackVelCap,ltrackVelCap); - Nu = constrain(Nu, -1.5708f, +1.5708f); //Limit Nu to +- Pi/2 + Nu = constrain_float(Nu, -1.5708f, +1.5708f); //Limit Nu to +- Pi/2 //Calculate lat accln demand to capture center_WP (use L1 guidance law) float latAccDemCap = VomegaA * (xtrackVelCap * Kv_L1 + 2.0f * sinf(Nu)); @@ -290,7 +290,7 @@ void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd) _bearing_error = Nu; // bearing error angle (radians), +ve to left of track // Limit Nu to +-pi - Nu = constrain(Nu, -1.5708f, +1.5708f); + Nu = constrain_float(Nu, -1.5708f, +1.5708f); _latAccDem = 2.0f*sinf(Nu)*VomegaA; } diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index 4d6f6655ff..2ceed26eab 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -70,11 +70,11 @@ enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *fou #endif // constrain a value -float constrain(float amt, float low, float high) +float constrain_float(float amt, float low, float high) { // the check for NaN as a float prevents propogation of // floating point errors through any function that uses - // constrain(). The normal float semantics already handle -Inf + // constrain_float(). The normal float semantics already handle -Inf // and +Inf if (isnan(amt)) { return (low+high)*0.5f; diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index ef66ce3e83..254eacf75e 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -97,7 +97,7 @@ int32_t wrap_180_cd(int32_t error); void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon); // constrain a value -float constrain(float amt, float low, float high); +float constrain_float(float amt, float low, float high); int16_t constrain_int16(int16_t amt, int16_t low, int16_t high); int32_t constrain_int32(int32_t amt, int32_t low, int32_t high); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp index 8a6b57ea9c..19150b13d3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp @@ -71,7 +71,7 @@ int AP_RangeFinder_MaxsonarI2CXL::read() } // ensure distance is within min and max - ret_value = constrain(ret_value, min_distance, max_distance); + ret_value = constrain_float(ret_value, min_distance, max_distance); ret_value = _mode_filter->apply(ret_value); diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index a83b4983dd..e72b5926f5 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -32,7 +32,7 @@ int RangeFinder::read() temp_dist = convert_raw_to_distance(raw_value); // ensure distance is within min and max - temp_dist = constrain(temp_dist, min_distance, max_distance); + temp_dist = constrain_float(temp_dist, min_distance, max_distance); distance = _mode_filter->apply(temp_dist); return distance; diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index 56dcf60488..86737ce293 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -91,7 +91,7 @@ float PID::get_pid(float error, float scaler) int16_t PID::get_pid_4500(float error, float scaler) { float v = get_pid(error, scaler); - return constrain(v, -4500, 4500); + return constrain_float(v, -4500, 4500); } void