mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
libraries: replace constrain() with constrain_float()
this makes the type much more obvious. Thanks to Tobias for the suggestion.
This commit is contained in:
parent
813e767efb
commit
ba83950fc4
libraries
AC_WPNav
APM_Control
AP_AHRS
AP_Compass
AP_Declination
AP_L1_Control
AP_Math
AP_RangeFinder
PID
@ -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
|
||||
|
@ -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) {
|
||||
|
@ -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);
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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 )
|
||||
|
@ -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;
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
@ -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;
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user