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

@@ -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_roll = constrain_float((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_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

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) {

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.x = constrain_float(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.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.x = constrain_float(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.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);

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.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
-        _omega_I_sum.y = constrain(_omega_I_sum.y, -change_limit, change_limit);
+        _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
-        _omega_I_sum.z = constrain(_omega_I_sum.z, -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);
 	}

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

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 )

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);
+    lat = constrain_float(lat, -90, 90);
-    lon = constrain(lon, -180, 180);
+    lon = constrain_float(lon, -180, 180);
 
     latmin = floorf(lat/5)*5;
     lonmin = floorf(lon/5)*5;

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 &center_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;
 }
 

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;

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);
 

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);
     

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;

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