diff --git a/APMrover2/test.cpp b/APMrover2/test.cpp index 77acd1c80b..bc095e8355 100644 --- a/APMrover2/test.cpp +++ b/APMrover2/test.cpp @@ -456,10 +456,10 @@ int8_t Rover::test_sonar(uint8_t argc, const Menu::arg *argv) sonar_dist_cm_min = dist_cm; voltage_min = voltage; } - sonar_dist_cm_max = max(sonar_dist_cm_max, dist_cm); - sonar_dist_cm_min = min(sonar_dist_cm_min, dist_cm); - voltage_min = min(voltage_min, voltage); - voltage_max = max(voltage_max, voltage); + sonar_dist_cm_max = MAX(sonar_dist_cm_max, dist_cm); + sonar_dist_cm_min = MIN(sonar_dist_cm_min, dist_cm); + voltage_min = MIN(voltage_min, voltage); + voltage_max = MAX(voltage_max, voltage); dist_cm = sonar.distance_cm(1); voltage = sonar.voltage_mv(1); @@ -467,10 +467,10 @@ int8_t Rover::test_sonar(uint8_t argc, const Menu::arg *argv) sonar2_dist_cm_min = dist_cm; voltage2_min = voltage; } - sonar2_dist_cm_max = max(sonar2_dist_cm_max, dist_cm); - sonar2_dist_cm_min = min(sonar2_dist_cm_min, dist_cm); - voltage2_min = min(voltage2_min, voltage); - voltage2_max = max(voltage2_max, voltage); + sonar2_dist_cm_max = MAX(sonar2_dist_cm_max, dist_cm); + sonar2_dist_cm_min = MIN(sonar2_dist_cm_min, dist_cm); + voltage2_min = MIN(voltage2_min, voltage); + voltage2_max = MAX(voltage2_max, voltage); if (now - last_print >= 200) { cliSerial->printf("sonar1 dist=%.1f:%.1fcm volt1=%.2f:%.2f sonar2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n", diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 8bcc8aa3b2..7e49485bf6 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -285,8 +285,8 @@ void Copter::do_takeoff(const AP_Mission::Mission_Command& cmd) { // Set wp navigation target to safe altitude above current position float takeoff_alt = cmd.content.location.alt; - takeoff_alt = max(takeoff_alt,current_loc.alt); - takeoff_alt = max(takeoff_alt,100.0f); + takeoff_alt = MAX(takeoff_alt,current_loc.alt); + takeoff_alt = MAX(takeoff_alt,100.0f); auto_takeoff_start(takeoff_alt); } @@ -750,7 +750,7 @@ void Copter::do_yaw(const AP_Mission::Mission_Command& cmd) bool Copter::verify_wait_delay() { - if (millis() - condition_start > (uint32_t)max(condition_value,0)) { + if (millis() - condition_start > (uint32_t)MAX(condition_value,0)) { condition_value = 0; return true; } @@ -767,7 +767,7 @@ bool Copter::verify_within_distance() { // update distance calculation calc_wp_distance(); - if (wp_distance < max(condition_value,0)) { + if (wp_distance < MAX(condition_value,0)) { condition_value = 0; return true; } diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index f4520db34a..c204b68e8b 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -217,8 +217,8 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan) } // record maximum throttle and current - throttle_pct_max = max(throttle_pct_max, throttle_pct); - current_amps_max = max(current_amps_max, battery.current_amps()); + throttle_pct_max = MAX(throttle_pct_max, throttle_pct); + current_amps_max = MAX(current_amps_max, battery.current_amps()); } if (AP_HAL::millis() - last_send_time > 500) { diff --git a/ArduCopter/control_acro.cpp b/ArduCopter/control_acro.cpp index 6e6a51cac7..c4d9e01889 100644 --- a/ArduCopter/control_acro.cpp +++ b/ArduCopter/control_acro.cpp @@ -133,7 +133,7 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in rate_bf_request.y += rate_bf_level.y; rate_bf_request.z += rate_bf_level.z; }else{ - float acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch(); + float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch(); // Scale leveling rates by stick input rate_bf_level = rate_bf_level*acro_level_mix; diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index b3073e3bc3..c2281d8b94 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -639,16 +639,16 @@ void Copter::autotune_attitude_control() autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); switch (autotune_state.axis) { case AUTOTUNE_AXIS_ROLL: - tune_roll_rd = max(g.autotune_min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF); - tune_roll_rp = max(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF); + tune_roll_rd = MAX(g.autotune_min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF); + tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF); break; case AUTOTUNE_AXIS_PITCH: - tune_pitch_rd = max(g.autotune_min_d, tune_pitch_rd * AUTOTUNE_RD_BACKOFF); - tune_pitch_rp = max(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF); + tune_pitch_rd = MAX(g.autotune_min_d, tune_pitch_rd * AUTOTUNE_RD_BACKOFF); + tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF); break; case AUTOTUNE_AXIS_YAW: - tune_yaw_rLPF = max(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF); - tune_yaw_rp = max(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF); + tune_yaw_rLPF = MAX(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF); + tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF); break; } break; @@ -656,13 +656,13 @@ void Copter::autotune_attitude_control() autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); switch (autotune_state.axis) { case AUTOTUNE_AXIS_ROLL: - tune_roll_rp = max(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF); + tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF); break; case AUTOTUNE_AXIS_PITCH: - tune_pitch_rp = max(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF); + tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF); break; case AUTOTUNE_AXIS_YAW: - tune_yaw_rp = max(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF); + tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF); break; } break; @@ -677,8 +677,8 @@ void Copter::autotune_attitude_control() bool autotune_complete = false; switch (autotune_state.axis) { case AUTOTUNE_AXIS_ROLL: - tune_roll_sp = max(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF); - tune_roll_accel = max(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); + tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF); + tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); if (autotune_pitch_enabled()) { autotune_state.axis = AUTOTUNE_AXIS_PITCH; } else if (autotune_yaw_enabled()) { @@ -688,8 +688,8 @@ void Copter::autotune_attitude_control() } break; case AUTOTUNE_AXIS_PITCH: - tune_pitch_sp = max(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF); - tune_pitch_accel = max(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); + tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF); + tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); if (autotune_yaw_enabled()) { autotune_state.axis = AUTOTUNE_AXIS_YAW; } else { @@ -697,8 +697,8 @@ void Copter::autotune_attitude_control() } break; case AUTOTUNE_AXIS_YAW: - tune_yaw_sp = max(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF); - tune_yaw_accel = max(AUTOTUNE_Y_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF); + tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF); + tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF); autotune_complete = true; break; } @@ -1062,7 +1062,7 @@ void Copter::autotune_twitching_test(float measurement, float target, float &mea if (measurement_max < target * 0.75f) { // the measurement not reached the 90% threshold yet autotune_step_stop_time = autotune_step_start_time + (millis() - autotune_step_start_time) * 3.0f; - autotune_step_stop_time = min(autotune_step_stop_time, autotune_step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); + autotune_step_stop_time = MIN(autotune_step_stop_time, autotune_step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); } if (measurement_max > target) { diff --git a/ArduCopter/control_drift.cpp b/ArduCopter/control_drift.cpp index a6cf6db04a..a6d9b0a8ce 100644 --- a/ArduCopter/control_drift.cpp +++ b/ArduCopter/control_drift.cpp @@ -68,7 +68,7 @@ void Copter::drift_run() float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel // gain sceduling for Yaw - float pitch_vel2 = min(fabsf(pitch_vel), 2000); + float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p; roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); @@ -87,7 +87,7 @@ void Copter::drift_run() if(is_zero(target_pitch)){ // .14/ (.03 * 100) = 4.6 seconds till full breaking breaker += .03f; - breaker = min(breaker, DRIFT_SPEEDGAIN); + breaker = MIN(breaker, DRIFT_SPEEDGAIN); target_pitch = pitch_vel * breaker; }else{ breaker = 0.0f; diff --git a/ArduCopter/control_flip.cpp b/ArduCopter/control_flip.cpp index 877392d8b5..c196380a50 100644 --- a/ArduCopter/control_flip.cpp +++ b/ArduCopter/control_flip.cpp @@ -144,7 +144,7 @@ void Copter::flip_run() attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0); // decrease throttle if (throttle_out >= g.throttle_min) { - throttle_out = max(throttle_out - FLIP_THR_DEC, g.throttle_min); + throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min); } // beyond -90deg move on to recovery @@ -158,7 +158,7 @@ void Copter::flip_run() attitude_control.rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0); // decrease throttle if (throttle_out >= g.throttle_min) { - throttle_out = max(throttle_out - FLIP_THR_DEC, g.throttle_min); + throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min); } // check roll for inversion @@ -172,7 +172,7 @@ void Copter::flip_run() attitude_control.rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0); // decrease throttle if (throttle_out >= g.throttle_min) { - throttle_out = max(throttle_out - FLIP_THR_DEC, g.throttle_min); + throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min); } // check roll for inversion diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 86cc749860..c4bcc1ba3e 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -459,7 +459,7 @@ void Copter::guided_angle_control_run() float roll_in = guided_angle_state.roll_cd; float pitch_in = guided_angle_state.pitch_cd; float total_in = pythagorous2(roll_in, pitch_in); - float angle_max = min(attitude_control.get_althold_lean_angle_max(), aparm.angle_max); + float angle_max = MIN(attitude_control.get_althold_lean_angle_max(), aparm.angle_max); if (total_in > angle_max) { float ratio = angle_max / total_in; roll_in *= ratio; diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index 309bd3de75..9a794eda4e 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -531,13 +531,13 @@ void Copter::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float & // lean_angle_raw must be pulling lean_angle_filtered towards zero, smooth the decrease if (lean_angle_filtered > 0) { // reduce the filtered lean angle at 5% or the brake rate (whichever is faster). - lean_angle_filtered -= max((float)lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, max(1, g.poshold_brake_rate/LOOP_RATE_FACTOR)); + lean_angle_filtered -= MAX((float)lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, MAX(1, g.poshold_brake_rate/LOOP_RATE_FACTOR)); // do not let the filtered angle fall below the pilot's input lean angle. // the above line pulls the filtered angle down and the below line acts as a catch - lean_angle_filtered = max(lean_angle_filtered, lean_angle_raw); + lean_angle_filtered = MAX(lean_angle_filtered, lean_angle_raw); }else{ - lean_angle_filtered += max(-(float)lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, max(1, g.poshold_brake_rate/LOOP_RATE_FACTOR)); - lean_angle_filtered = min(lean_angle_filtered, lean_angle_raw); + lean_angle_filtered += MAX(-(float)lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, MAX(1, g.poshold_brake_rate/LOOP_RATE_FACTOR)); + lean_angle_filtered = MIN(lean_angle_filtered, lean_angle_raw); } } } diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index bc7a27da09..6ebb4c7096 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -97,7 +97,7 @@ void Copter::rtl_climb_start() // rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt); rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home - rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home + rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home destination.z = pv_alt_above_origin(rally_point.alt); #else destination.z = pv_alt_above_origin(rtl_alt); @@ -122,7 +122,7 @@ void Copter::rtl_return_start() // rally_point will be the nearest rally point or home. uses absolute altitudes Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt); rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home - rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home + rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home Vector3f destination = pv_location_to_vector(rally_point); #else Vector3f destination = pv_location_to_vector(ahrs.get_home()); @@ -420,13 +420,13 @@ void Copter::rtl_land_run() float Copter::get_RTL_alt() { // maximum of current altitude + climb_min and rtl altitude - float ret = max(current_loc.alt + max(0, g.rtl_climb_min), g.rtl_altitude); - ret = max(ret, RTL_ALT_MIN); + float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), g.rtl_altitude); + ret = MAX(ret, RTL_ALT_MIN); #if AC_FENCE == ENABLED // ensure not above fence altitude if alt fence is enabled if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { - ret = min(ret, fence.get_safe_alt()*100.0f); + ret = MIN(ret, fence.get_safe_alt()*100.0f); } #endif diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 3f2a3455a9..1fc2b94e1f 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -129,7 +129,7 @@ uint8_t Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_s // set timeout motor_test_start_ms = AP_HAL::millis(); - motor_test_timeout_ms = min(timeout_sec * 1000, MOTOR_TEST_TIMEOUT_MS_MAX); + motor_test_timeout_ms = MIN(timeout_sec * 1000, MOTOR_TEST_TIMEOUT_MS_MAX); // store required output motor_test_seq = motor_seq; diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 0a7a13afe8..15d4fdd0c2 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -59,7 +59,7 @@ int16_t Copter::read_sonar(void) #if SONAR_TILT_CORRECTION == 1 // correct alt for angle of the sonar float temp = ahrs.cos_pitch() * ahrs.cos_roll(); - temp = max(temp, 0.707f); + temp = MAX(temp, 0.707f); temp_alt = (float)temp_alt * temp; #endif diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 18c3e6e099..e8297ddc72 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -315,7 +315,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) cmd.p1 = 0; cmd.content.location.lat = 0; cmd.content.location.lng = 0; - cmd.content.location.alt = max(current_loc.alt,100); + cmd.content.location.alt = MAX(current_loc.alt,100); // use the current altitude for the target alt for takeoff. // only altitude will matter to the AP mission script for takeoff. diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index ceb754b02c..716685a460 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -57,7 +57,7 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) void Copter::takeoff_timer_start(float alt_cm) { // calculate climb rate - float speed = min(wp_nav.get_speed_up(), max(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f)); + float speed = MIN(wp_nav.get_speed_up(), MAX(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f)); // sanity check speed and target if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) { @@ -92,9 +92,9 @@ void Copter::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_cli // acceleration of 50cm/s/s static const float takeoff_accel = 50.0f; - float takeoff_minspeed = min(50.0f,takeoff_state.max_speed); + float takeoff_minspeed = MIN(50.0f,takeoff_state.max_speed); float time_elapsed = (millis()-takeoff_state.start_ms)*1.0e-3f; - float speed = min(time_elapsed*takeoff_accel+takeoff_minspeed, takeoff_state.max_speed); + float speed = MIN(time_elapsed*takeoff_accel+takeoff_minspeed, takeoff_state.max_speed); float time_to_max_speed = (takeoff_state.max_speed-takeoff_minspeed)/takeoff_accel; float height_gained; diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 40ad515422..e57dec9c64 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -123,7 +123,7 @@ void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out) ch_inf = (float)channel->radio_in - (float)channel->radio_trim; ch_inf = fabsf(ch_inf); - ch_inf = min(ch_inf, 400.0f); + ch_inf = MIN(ch_inf, 400.0f); ch_inf = ((400.0f - ch_inf) / 400.0f); servo_out *= ch_inf; servo_out += channel->pwm_to_angle(); @@ -586,7 +586,7 @@ bool Plane::suppress_throttle(void) uint32_t launch_duration_ms = ((int32_t)g.takeoff_throttle_delay)*100 + 2000; if (is_flying() && - millis() - started_flying_ms > max(launch_duration_ms,5000) && // been flying >5s in any mode + millis() - started_flying_ms > MAX(launch_duration_ms,5000) && // been flying >5s in any mode adjusted_relative_altitude_cm() > 500 && // are >5m above AGL/home labs(ahrs.pitch_sensor) < 3000 && // not high pitch, which happens when held before launch gps_movement) { // definate gps movement diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 2e2e48694f..c795ab8b2e 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -692,7 +692,7 @@ bool Plane::verify_loiter_to_alt() bool Plane::verify_RTL() { update_loiter(); - if (auto_state.wp_distance <= (uint32_t)max(g.waypoint_radius,0) || + if (auto_state.wp_distance <= (uint32_t)MAX(g.waypoint_radius,0) || nav_controller->reached_loiter_target()) { gcs_send_text(MAV_SEVERITY_INFO,"Reached HOME"); return true; @@ -826,7 +826,7 @@ bool Plane::verify_change_alt() bool Plane::verify_within_distance() { - if (auto_state.wp_distance < max(condition_value,0)) { + if (auto_state.wp_distance < MAX(condition_value,0)) { condition_value = 0; return true; } diff --git a/ArduPlane/geofence.cpp b/ArduPlane/geofence.cpp index 4e32002a1c..5f97322304 100644 --- a/ArduPlane/geofence.cpp +++ b/ArduPlane/geofence.cpp @@ -49,7 +49,7 @@ static const StorageAccess fence_storage(StorageManager::StorageFence); */ uint8_t Plane::max_fencepoints(void) { - return min(255, fence_storage.size() / sizeof(Vector2l)); + return MIN(255, fence_storage.size() / sizeof(Vector2l)); } /* diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index f596eeaa2f..87beaf6e67 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -18,7 +18,7 @@ bool Plane::auto_takeoff_check(void) static bool launchTimerStarted; static uint32_t last_tkoff_arm_time; static uint32_t last_check_ms; - uint16_t wait_time_ms = min(uint16_t(g.takeoff_throttle_delay)*100,12700); + uint16_t wait_time_ms = MIN(uint16_t(g.takeoff_throttle_delay)*100,12700); // Reset states if process has been interrupted if (last_check_ms && (now - last_check_ms) > 200) { diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index ccb57bdc21..435c86dbe3 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -834,11 +834,11 @@ void Replay::log_check_solution(void) float vel_error = (velocity - check_state.velocity).length(); float pos_error = get_distance(check_state.pos, loc); - check_result.max_roll_error = max(check_result.max_roll_error, roll_error); - check_result.max_pitch_error = max(check_result.max_pitch_error, pitch_error); - check_result.max_yaw_error = max(check_result.max_yaw_error, yaw_error); - check_result.max_vel_error = max(check_result.max_vel_error, vel_error); - check_result.max_pos_error = max(check_result.max_pos_error, pos_error); + check_result.max_roll_error = MAX(check_result.max_roll_error, roll_error); + check_result.max_pitch_error = MAX(check_result.max_pitch_error, pitch_error); + check_result.max_yaw_error = MAX(check_result.max_yaw_error, yaw_error); + check_result.max_vel_error = MAX(check_result.max_vel_error, vel_error); + check_result.max_pos_error = MAX(check_result.max_pos_error, pos_error); } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 88db923304..8e77e49d3a 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -197,10 +197,10 @@ void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, floa // 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*fabsf(_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); + _accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms); float vel_change_limit = _accel_last_z_cms * dt; _vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit); @@ -915,7 +915,7 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler, bo // limit acceleration if necessary if (use_althold_lean_angle) { - accel_max = min(accel_max, GRAVITY_MSS * 100.0f * sinf(ToRad(constrain_float(_attitude_control.get_althold_lean_angle_max(),1000,8000)/100.0f))); + accel_max = MIN(accel_max, GRAVITY_MSS * 100.0f * sinf(ToRad(constrain_float(_attitude_control.get_althold_lean_angle_max(),1000,8000)/100.0f))); } // scale desired acceleration if it's beyond acceptable limit @@ -950,7 +950,7 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler, bo _accel_target_jerk_limited += accel_change; // lowpass filter on NE accel - _accel_target_filter.set_cutoff_frequency(min(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler)); + _accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler)); Vector2f accel_target_filtered = _accel_target_filter.apply(_accel_target_jerk_limited, dt); // rotate accelerations into body forward-right frame @@ -967,8 +967,8 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler, bo void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss) const { // rotate our roll, pitch angles into lat/lon frame - accel_x_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.cos_yaw() * _ahrs.sin_pitch() / max(_ahrs.cos_pitch(),0.5f)) - _ahrs.sin_yaw() * _ahrs.sin_roll() / max(_ahrs.cos_roll(),0.5f)); - accel_y_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.sin_yaw() * _ahrs.sin_pitch() / max(_ahrs.cos_pitch(),0.5f)) + _ahrs.cos_yaw() * _ahrs.sin_roll() / max(_ahrs.cos_roll(),0.5f)); + accel_x_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.cos_yaw() * _ahrs.sin_pitch() / MAX(_ahrs.cos_pitch(),0.5f)) - _ahrs.sin_yaw() * _ahrs.sin_roll() / MAX(_ahrs.cos_roll(),0.5f)); + accel_y_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.sin_yaw() * _ahrs.sin_pitch() / MAX(_ahrs.cos_pitch(),0.5f)) + _ahrs.cos_yaw() * _ahrs.sin_roll() / MAX(_ahrs.cos_roll(),0.5f)); } /// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 82e22e4dc0..d39b3e3583 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -237,7 +237,7 @@ float AC_Fence::get_breach_distance(uint8_t fence_type) const return _circle_breach_distance; break; case AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE: - return max(_alt_max_breach_distance,_circle_breach_distance); + return MAX(_alt_max_breach_distance,_circle_breach_distance); } // we don't recognise the fence type so just return 0 diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 6ed8988004..04ddd3ddcb 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -74,7 +74,7 @@ void AC_PID::filt_hz(float hz) _filt_hz.set(fabsf(hz)); // sanity check _filt_hz - _filt_hz = max(_filt_hz, AC_PID_FILT_HZ_MIN); + _filt_hz = MAX(_filt_hz, AC_PID_FILT_HZ_MIN); } // set_input_filter_all - set input to PID controller diff --git a/libraries/AC_PID/AC_PI_2D.cpp b/libraries/AC_PID/AC_PI_2D.cpp index 78f5df524b..41702d2a47 100644 --- a/libraries/AC_PID/AC_PI_2D.cpp +++ b/libraries/AC_PID/AC_PI_2D.cpp @@ -61,7 +61,7 @@ void AC_PI_2D::filt_hz(float hz) _filt_hz.set(fabsf(hz)); // sanity check _filt_hz - _filt_hz = max(_filt_hz, AC_PI_2D_FILT_HZ_MIN); + _filt_hz = MAX(_filt_hz, AC_PI_2D_FILT_HZ_MIN); // calculate the input filter alpha calc_filt_alpha(); @@ -110,7 +110,7 @@ Vector2f AC_PI_2D::get_i() Vector2f AC_PI_2D::get_i_shrink() { if (!is_zero(_ki) && !is_zero(_dt)) { - float integrator_length_orig = min(_integrator.length(),_imax); + float integrator_length_orig = MIN(_integrator.length(),_imax); _integrator += (_input * _ki) * _dt; float integrator_length_new = _integrator.length(); if ((integrator_length_new > integrator_length_orig) && (integrator_length_new > 0)) { diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 4f270ddb00..9e0dba63b4 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -159,7 +159,7 @@ void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm) _ef_angle_to_target.y = y_rad*_ahrs.sin_yaw() + x_rad*_ahrs.cos_yaw(); // get current altitude (constrained to no lower than 50cm) - float alt = max(alt_above_terrain_cm, 50.0f); + float alt = MAX(alt_above_terrain_cm, 50.0f); // convert earth-frame angles to earth-frame position offset _target_pos_offset.x = alt*tanf(_ef_angle_to_target.x); diff --git a/libraries/AC_Sprayer/AC_Sprayer.cpp b/libraries/AC_Sprayer/AC_Sprayer.cpp index 731976d512..02aac4b066 100644 --- a/libraries/AC_Sprayer/AC_Sprayer.cpp +++ b/libraries/AC_Sprayer/AC_Sprayer.cpp @@ -160,7 +160,7 @@ AC_Sprayer::update() // if spraying or testing update the pump servo position if (_flags.spraying || _flags.testing) { - RC_Channel_aux::move_servo(RC_Channel_aux::k_sprayer_pump, min(max(ground_speed * _pump_pct_1ms, 100 *_pump_min_pct),10000),0,10000); + RC_Channel_aux::move_servo(RC_Channel_aux::k_sprayer_pump, MIN(MAX(ground_speed * _pump_pct_1ms, 100 *_pump_min_pct),10000),0,10000); RC_Channel_aux::set_radio(RC_Channel_aux::k_sprayer_spinner, _spinner_pwm); }else{ // ensure sprayer and spinner are off diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index ba5cfb870f..edfe36641a 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -121,11 +121,11 @@ void AC_Circle::update() // ramp angular velocity to maximum if (_angular_vel < _angular_vel_max) { _angular_vel += fabsf(_angular_accel) * dt; - _angular_vel = min(_angular_vel, _angular_vel_max); + _angular_vel = MIN(_angular_vel, _angular_vel_max); } if (_angular_vel > _angular_vel_max) { _angular_vel -= fabsf(_angular_accel) * dt; - _angular_vel = max(_angular_vel, _angular_vel_max); + _angular_vel = MAX(_angular_vel, _angular_vel_max); } // update the target angle and total angle traveled @@ -210,17 +210,17 @@ void AC_Circle::calc_velocities(bool init_velocity) // if we are doing a panorama set the circle_angle to the current heading if (_radius <= 0) { _angular_vel_max = ToRad(_rate); - _angular_accel = max(fabsf(_angular_vel_max),ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN)); // reach maximum yaw velocity in 1 second + _angular_accel = MAX(fabsf(_angular_vel_max),ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN)); // reach maximum yaw velocity in 1 second }else{ // calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle - float velocity_max = min(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius)); + float velocity_max = MIN(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius)); // angular_velocity in radians per second _angular_vel_max = velocity_max/_radius; _angular_vel_max = constrain_float(ToRad(_rate),-_angular_vel_max,_angular_vel_max); // angular_velocity in radians per second - _angular_accel = max(_pos_control.get_accel_xy()/_radius, ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN)); + _angular_accel = MAX(_pos_control.get_accel_xy()/_radius, ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN)); } // initialise angular velocity diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index ec0924578a..a651fb2cb8 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -238,8 +238,8 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit) { // calculate a loiter speed limit which is the minimum of the value set by the WPNAV_LOITER_SPEED // parameter and the value set by the EKF to observe optical flow limits - float gnd_speed_limit_cms = min(_loiter_speed_cms,ekfGndSpdLimit*100.0f); - gnd_speed_limit_cms = max(gnd_speed_limit_cms, 10.0f); + float gnd_speed_limit_cms = MIN(_loiter_speed_cms,ekfGndSpdLimit*100.0f); + gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, 10.0f); // range check nav_dt if( nav_dt < 0 ) { @@ -290,10 +290,10 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit) float drag_speed_delta = -_loiter_accel_cmss*nav_dt*desired_speed/gnd_speed_limit_cms; if (_pilot_accel_fwd_cms == 0 && _pilot_accel_rgt_cms == 0) { - drag_speed_delta = min(drag_speed_delta,max(-_loiter_accel_min_cmss*nav_dt, -2.0f*desired_speed*nav_dt)); + drag_speed_delta = MIN(drag_speed_delta,MAX(-_loiter_accel_min_cmss*nav_dt, -2.0f*desired_speed*nav_dt)); } - desired_speed = max(desired_speed+drag_speed_delta,0.0f); + desired_speed = MAX(desired_speed+drag_speed_delta,0.0f); desired_vel = desired_vel_norm*desired_speed; } @@ -551,7 +551,7 @@ void AC_WPNav::advance_wp_target_along_track(float dt) } // calculate how far along the track we could move the intermediate target before reaching the end of the leash - track_leash_slack = min(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy); + track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy); if (track_leash_slack < 0) { track_desired_max = track_covered; }else{ @@ -595,7 +595,7 @@ void AC_WPNav::advance_wp_target_along_track(float dt) } // if target is slowing down, limit the speed if (_flags.slowing_down) { - _limited_speed_xy_cms = min(_limited_speed_xy_cms, get_slow_down_speed(dist_to_dest, _track_accel)); + _limited_speed_xy_cms = MIN(_limited_speed_xy_cms, get_slow_down_speed(dist_to_dest, _track_accel)); } } @@ -737,9 +737,9 @@ void AC_WPNav::calculate_wp_leash_length() _track_speed = speed_z/pos_delta_unit_z; _track_leash_length = leash_z/pos_delta_unit_z; }else{ - _track_accel = min(_wp_accel_z_cms/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy); - _track_speed = min(speed_z/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy); - _track_leash_length = min(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy); + _track_accel = MIN(_wp_accel_z_cms/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy); + _track_speed = MIN(speed_z/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy); + _track_leash_length = MIN(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy); } // calculate slow down distance (the distance from the destination when the target point should begin to slow down) @@ -952,7 +952,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt) } // calculate how far along the track we could move the intermediate target before reaching the end of the leash - float track_leash_slack = min(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy); + float track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy); if (track_leash_slack < 0.0f) { track_leash_slack = 0.0f; } @@ -961,7 +961,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt) float spline_dist_to_wp = (_destination - target_pos).length(); float vel_limit = _wp_speed_cms; if (!is_zero(dt)) { - vel_limit = min(vel_limit, track_leash_slack/dt); + vel_limit = MIN(vel_limit, track_leash_slack/dt); } // if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index f5294f4a55..ccdbd3bc6f 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -142,10 +142,10 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool float integrator_delta = rate_error * ki_rate * delta_time * scaler; if (_last_out < -45) { // prevent the integrator from increasing if surface defln demand is above the upper limit - integrator_delta = max(integrator_delta , 0); + integrator_delta = MAX(integrator_delta , 0); } else if (_last_out > 45) { // prevent the integrator from decreasing if surface defln demand is below the lower limit - integrator_delta = min(integrator_delta , 0); + integrator_delta = MIN(integrator_delta , 0); } _pid_info.I += integrator_delta; } @@ -162,7 +162,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool // Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law // No conversion is required for K_D float eas2tas = _ahrs.get_EAS2TAS(); - float kp_ff = max((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / eas2tas; + float kp_ff = MAX((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / eas2tas; float k_ff = gains.FF / eas2tas; // Calculate the demanded control surface deflection @@ -244,7 +244,7 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv // don't do turn coordination handling when at very high pitch angles rate_offset = 0; } else { - rate_offset = cosf(_ahrs.pitch)*fabsf(ToDeg((GRAVITY_MSS / max((aspeed * _ahrs.get_EAS2TAS()) , float(aparm.airspeed_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff; + rate_offset = cosf(_ahrs.pitch)*fabsf(ToDeg((GRAVITY_MSS / MAX((aspeed * _ahrs.get_EAS2TAS()) , float(aparm.airspeed_min))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff; } if (inverted) { rate_offset = -rate_offset; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 2b688e308d..52df22734c 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -103,7 +103,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool // No conversion is required for K_D float ki_rate = gains.I * gains.tau; float eas2tas = _ahrs.get_EAS2TAS(); - float kp_ff = max((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / eas2tas; + float kp_ff = MAX((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / eas2tas; float k_ff = gains.FF / eas2tas; float delta_time = (float)dt * 0.001f; @@ -137,10 +137,10 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool float integrator_delta = rate_error * ki_rate * delta_time * scaler; // prevent the integrator from increasing if surface defln demand is above the upper limit if (_last_out < -45) { - integrator_delta = max(integrator_delta , 0); + integrator_delta = MAX(integrator_delta , 0); } else if (_last_out > 45) { // prevent the integrator from decreasing if surface defln demand is below the lower limit - integrator_delta = min(integrator_delta, 0); + integrator_delta = MIN(integrator_delta, 0); } _pid_info.I += integrator_delta; } diff --git a/libraries/APM_Control/AP_SteerController.cpp b/libraries/APM_Control/AP_SteerController.cpp index fdc13303eb..7ff1cca332 100644 --- a/libraries/APM_Control/AP_SteerController.cpp +++ b/libraries/APM_Control/AP_SteerController.cpp @@ -119,7 +119,7 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) // Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law // No conversion is required for K_D float ki_rate = _K_I * _tau * 45.0f; - float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0) * 45.0f; + float kp_ff = MAX((_K_P - _K_I * _tau) * _tau - _K_D , 0) * 45.0f; float k_ff = _K_FF * 45.0f; float delta_time = (float)dt * 0.001f; @@ -131,10 +131,10 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) float integrator_delta = rate_error * ki_rate * delta_time * scaler; // prevent the integrator from increasing if steering defln demand is above the upper limit if (_last_out < -45) { - integrator_delta = max(integrator_delta , 0); + integrator_delta = MAX(integrator_delta , 0); } else if (_last_out > 45) { // prevent the integrator from decreasing if steering defln demand is below the lower limit - integrator_delta = min(integrator_delta, 0); + integrator_delta = MIN(integrator_delta, 0); } _pid_info.I += integrator_delta; } diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index abf4f247d7..df7add4b23 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -99,7 +99,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator) // If no airspeed available use average of min and max aspeed = 0.5f*(float(aspd_min) + float(aparm.airspeed_max)); } - rate_offset = (GRAVITY_MSS / max(aspeed , float(aspd_min))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF; + rate_offset = (GRAVITY_MSS / MAX(aspeed , float(aspd_min))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF; // Get body rate vector (radians/sec) float omega_z = _ahrs.get_gyro().z; @@ -131,10 +131,10 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator) { // prevent the integrator from increasing if surface defln demand is above the upper limit if (_last_out < -45) { - _integrator += max(integ_in * delta_time , 0); + _integrator += MAX(integ_in * delta_time , 0); } else if (_last_out > 45) { // prevent the integrator from decreasing if surface defln demand is below the lower limit - _integrator += min(integ_in * delta_time , 0); + _integrator += MIN(integ_in * delta_time , 0); } else { _integrator += integ_in * delta_time; } diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 803b4d21d8..d146f8f7f1 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -232,7 +232,7 @@ void AP_Airspeed::read(void) airspeed_pressure = fabsf(airspeed_pressure); break; } - airspeed_pressure = max(airspeed_pressure, 0); + airspeed_pressure = MAX(airspeed_pressure, 0); _last_pressure = airspeed_pressure; _raw_airspeed = sqrtf(airspeed_pressure * _ratio); _airspeed = 0.7f * _airspeed + 0.3f * _raw_airspeed; diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp index 77c674bcfa..350022cf21 100644 --- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp +++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp @@ -96,9 +96,9 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg) P.b.z = P.c.y = P23; // Constrain diagonals to be non-negative - protects against rounding errors - P.a.x = max(P.a.x, 0.0f); - P.b.y = max(P.b.y, 0.0f); - P.c.z = max(P.c.z, 0.0f); + P.a.x = MAX(P.a.x, 0.0f); + P.b.y = MAX(P.b.y, 0.0f); + P.c.z = MAX(P.c.z, 0.0f); state.x = constrain_float(state.x, -aparm.airspeed_max, aparm.airspeed_max); state.y = constrain_float(state.y, -aparm.airspeed_max, aparm.airspeed_max); diff --git a/libraries/AP_GPS/AP_GPS_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp index 9c8f391b52..333d8ff624 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP.cpp @@ -497,7 +497,7 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type, sender_id : sender_id, msg_len : msg_len, }; - memcpy(pkt.data1, msg_buff, min(msg_len,64)); + memcpy(pkt.data1, msg_buff, MIN(msg_len,64)); gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); if (msg_len > 64) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 7096149d4a..b13e3f3ed1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -567,7 +567,7 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact float &trim_roll, float &trim_pitch) { - uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES); + uint8_t num_accels = MIN(get_accel_count(), INS_MAX_INSTANCES); Vector3f samples[INS_MAX_INSTANCES][6]; Vector3f new_offsets[INS_MAX_INSTANCES]; Vector3f new_scaling[INS_MAX_INSTANCES]; @@ -914,7 +914,7 @@ bool AP_InertialSensor::use_accel(uint8_t instance) const void AP_InertialSensor::_init_gyro() { - uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES); + uint8_t num_gyros = MIN(get_gyro_count(), INS_MAX_INSTANCES); Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES]; Vector3f new_gyro_offset[INS_MAX_INSTANCES]; float best_diff[INS_MAX_INSTANCES]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp index de13f17caf..ca48d85d4b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp @@ -583,7 +583,7 @@ int16_t AP_InertialSensor_MPU9150::mpu_set_sample_rate(uint16_t rate) data); // sample_rate = 1000 / (1 + data); - // mpu_set_compass_sample_rate(min(sample_rate, MAX_COMPASS_SAMPLE_RATE), rate); + // mpu_set_compass_sample_rate(MIN(sample_rate, MAX_COMPASS_SAMPLE_RATE), rate); return 0; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index 5c326cf501..fa4cbed33b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -210,7 +210,7 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep // get time since last sample float dt = _accel_sample_time[i]; - _accel_dt_max[i] = max(_accel_dt_max[i],dt); + _accel_dt_max[i] = MAX(_accel_dt_max[i],dt); _accel_meas_count[i] ++; @@ -245,7 +245,7 @@ void AP_InertialSensor_PX4::_new_gyro_sample(uint8_t i, gyro_report &gyro_report // get time since last sample float dt = _gyro_sample_time[i]; - _gyro_dt_max[i] = max(_gyro_dt_max[i],dt); + _gyro_dt_max[i] = MAX(_gyro_dt_max[i],dt); _gyro_meas_count[i] ++; @@ -263,7 +263,7 @@ void AP_InertialSensor_PX4::_new_gyro_sample(uint8_t i, gyro_report &gyro_report void AP_InertialSensor_PX4::_get_sample() { - for (uint8_t i=0; i _L1_dist && alongTrackDist/max(WP_A_dist, 1.0f) < -0.7071f) + if (WP_A_dist > _L1_dist && alongTrackDist/MAX(WP_A_dist, 1.0f) < -0.7071f) { //Calc Nu to fly To WP A Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft @@ -208,7 +208,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct ltrackVel = _groundspeed_vector * AB; // Velocity along track float Nu2 = atan2f(xtrackVel,ltrackVel); //Calculate Nu1 angle (Angle to L1 reference point) - float sine_Nu1 = _crosstrack_error/max(_L1_dist, 0.1f); + float sine_Nu1 = _crosstrack_error/MAX(_L1_dist, 0.1f); //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f); float Nu1 = asinf(sine_Nu1); @@ -271,7 +271,7 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); //Calculate groundspeed - float groundSpeed = max(_groundspeed_vector.length() , 1.0f); + float groundSpeed = MAX(_groundspeed_vector.length() , 1.0f); // update _target_bearing_cd @@ -329,11 +329,11 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius //Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way if (ltrackVelCap < 0.0f && velTangent < 0.0f) { - latAccDemCircPD = max(latAccDemCircPD, 0.0f); + latAccDemCircPD = MAX(latAccDemCircPD, 0.0f); } // Calculate centripetal acceleration demand - float latAccDemCircCtr = velTangent * velTangent / max((0.5f * radius), (radius + xtrackErrCirc)); + float latAccDemCircCtr = velTangent * velTangent / MAX((0.5f * radius), (radius + xtrackErrCirc)); //Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr); diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 32156e1d16..af74837e13 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -221,8 +221,8 @@ static inline float pythagorous3(float a, float b, float c) { #endif /* The following three functions used to be arduino core macros */ -#define max(a,b) ((a)>(b)?(a):(b)) -#define min(a,b) ((a)<(b)?(a):(b)) +#define MAX(a,b) ((a)>(b)?(a):(b)) +#define MIN(a,b) ((a)<(b)?(a):(b)) static inline float maxf(float a, float b) { diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index c284edb087..bd735eda6e 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -212,8 +212,8 @@ void AP_MotorsCoax::output_armed_stabilizing() motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, _throttle_radio_max); // ensure motors don't drop below a minimum value and stop - motor_out[AP_MOTORS_MOT_3] = max(motor_out[AP_MOTORS_MOT_3], out_min); - motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4], out_min); + motor_out[AP_MOTORS_MOT_3] = MAX(motor_out[AP_MOTORS_MOT_3], out_min); + motor_out[AP_MOTORS_MOT_4] = MAX(motor_out[AP_MOTORS_MOT_4], out_min); // send output to each motor hal.rcout->cork(); diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index b7049d549d..5147f64c94 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -244,12 +244,12 @@ void AP_MotorsMatrix::output_armed_stabilizing() // We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control) // We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favour reducing throttle instead of better yaw control because the pilot has commanded it int16_t motor_mid = (rpy_low+rpy_high)/2; - out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(throttle_radio_output, throttle_radio_output*max(0,1.0f-_throttle_thr_mix)+get_hover_throttle_as_pwm()*_throttle_thr_mix)); + out_best_thr_pwm = MIN(out_mid_pwm - motor_mid, MAX(throttle_radio_output, throttle_radio_output*MAX(0,1.0f-_throttle_thr_mix)+get_hover_throttle_as_pwm()*_throttle_thr_mix)); // calculate amount of yaw we can fit into the throttle range // this is always equal to or less than the requested yaw from the pilot or rate controller - yaw_allowed = min(out_max_pwm - out_best_thr_pwm, out_best_thr_pwm - out_min_pwm) - (rpy_high-rpy_low)/2; - yaw_allowed = max(yaw_allowed, _yaw_headroom); + yaw_allowed = MIN(out_max_pwm - out_best_thr_pwm, out_best_thr_pwm - out_min_pwm) - (rpy_high-rpy_low)/2; + yaw_allowed = MAX(yaw_allowed, _yaw_headroom); if (yaw_pwm >= 0) { // if yawing right @@ -291,7 +291,7 @@ void AP_MotorsMatrix::output_armed_stabilizing() thr_adj = throttle_radio_output - out_best_thr_pwm; // calculate upper and lower limits of thr_adj - int16_t thr_adj_max = max(out_max_pwm-(out_best_thr_pwm+rpy_high),0); + int16_t thr_adj_max = MAX(out_max_pwm-(out_best_thr_pwm+rpy_high),0); // if we are increasing the throttle (situation #2 above).. if (thr_adj > 0) { @@ -306,7 +306,7 @@ void AP_MotorsMatrix::output_armed_stabilizing() // decrease throttle as close as possible to requested throttle // without going under out_min_pwm or over out_max_pwm // earlier code ensures we can't break both boundaries - int16_t thr_adj_min = min(out_min_pwm-(out_best_thr_pwm+rpy_low),0); + int16_t thr_adj_min = MIN(out_min_pwm-(out_best_thr_pwm+rpy_low),0); if (thr_adj > thr_adj_max) { thr_adj = thr_adj_max; limit.throttle_upper = true; diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 0fe3873970..4263732277 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -268,7 +268,7 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage() return; } - _batt_voltage_min = max(_batt_voltage_min, _batt_voltage_max * 0.6f); + _batt_voltage_min = MAX(_batt_voltage_min, _batt_voltage_max * 0.6f); // add current based voltage sag to battery voltage float batt_voltage = _batt_voltage + _batt_current * _batt_resistance; @@ -310,10 +310,10 @@ void AP_MotorsMulticopter::update_throttle_thr_mix() // slew _throttle_thr_mix to _throttle_thr_mix_desired if (_throttle_thr_mix < _throttle_thr_mix_desired) { // increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds) - _throttle_thr_mix += min(2.0f/_loop_rate, _throttle_thr_mix_desired-_throttle_thr_mix); + _throttle_thr_mix += MIN(2.0f/_loop_rate, _throttle_thr_mix_desired-_throttle_thr_mix); } else if (_throttle_thr_mix > _throttle_thr_mix_desired) { // reduce more slowly (from 0.9 to 0.1 in 1.6 seconds) - _throttle_thr_mix -= min(0.5f/_loop_rate, _throttle_thr_mix-_throttle_thr_mix_desired); + _throttle_thr_mix -= MIN(0.5f/_loop_rate, _throttle_thr_mix-_throttle_thr_mix_desired); } _throttle_thr_mix = constrain_float(_throttle_thr_mix, 0.1f, 1.0f); } diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 461b8bfd14..dfe7c7faa2 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -214,7 +214,7 @@ void AP_MotorsSingle::output_armed_stabilizing() throttle_radio_output = apply_thrust_curve_and_volt_scaling(throttle_radio_output, out_min, _throttle_radio_max); // ensure motor doesn't drop below a minimum value and stop - throttle_radio_output = max(throttle_radio_output, out_min); + throttle_radio_output = MAX(throttle_radio_output, out_min); // TODO: set limits.roll_pitch and limits.yaw diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index e61afa597a..3e63bad44c 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -285,9 +285,9 @@ void AP_MotorsTri::output_armed_stabilizing() motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, out_max); // ensure motors don't drop below a minimum value and stop - motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1], out_min); - motor_out[AP_MOTORS_MOT_2] = max(motor_out[AP_MOTORS_MOT_2], out_min); - motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4], out_min); + motor_out[AP_MOTORS_MOT_1] = MAX(motor_out[AP_MOTORS_MOT_1], out_min); + motor_out[AP_MOTORS_MOT_2] = MAX(motor_out[AP_MOTORS_MOT_2], out_min); + motor_out[AP_MOTORS_MOT_4] = MAX(motor_out[AP_MOTORS_MOT_4], out_min); } hal.rcout->cork(); diff --git a/libraries/AP_NavEKF/AP_NavEKF_core.cpp b/libraries/AP_NavEKF/AP_NavEKF_core.cpp index 9309bebb9e..f450d55781 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_core.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF_core.cpp @@ -744,7 +744,7 @@ void NavEKF_core::SelectFlowFusion() memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta)); flowUpdateCount = 0; // Set the flow noise used by the fusion processes - R_LOS = sq(max(frontend._flowNoise, 0.05f)); + R_LOS = sq(MAX(frontend._flowNoise, 0.05f)); // ensure that the covariance prediction is up to date before fusing data if (!covPredStep) CovariancePrediction(); // Fuse the optical flow X and Y axis data into the main filter sequentially @@ -2426,7 +2426,7 @@ void NavEKF_core::EstimateTerrainOffset() hal.util->perf_begin(_perf_OpticalFlowEKF); // constrain height above ground to be above range measured on ground - float heightAboveGndEst = max((terrainState - state.position.z), rngOnGnd); + float heightAboveGndEst = MAX((terrainState - state.position.z), rngOnGnd); // calculate a predicted LOS rate squared float velHorizSq = sq(state.velocity.x) + sq(state.velocity.y); @@ -2443,12 +2443,12 @@ void NavEKF_core::EstimateTerrainOffset() // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning float distanceTravelledSq = sq(statesAtRngTime.position[0] - prevPosN) + sq(statesAtRngTime.position[1] - prevPosE); - distanceTravelledSq = min(distanceTravelledSq, 100.0f); + distanceTravelledSq = MIN(distanceTravelledSq, 100.0f); prevPosN = statesAtRngTime.position[0]; prevPosE = statesAtRngTime.position[1]; // in addition to a terrain gradient error model, we also have a time based error growth that is scaled using the gradient parameter - float timeLapsed = min(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f); + float timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f); float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend._gndGradientSigma))) + sq(float(frontend._gndGradientSigma) * timeLapsed); Popt += Pincrement; timeAtLastAuxEKF_ms = imuSampleTime_ms; @@ -2456,7 +2456,7 @@ void NavEKF_core::EstimateTerrainOffset() // fuse range finder data if (fuseRngData) { // predict range - float predRngMeas = max((terrainState - statesAtRngTime.position[2]),rngOnGnd) / Tnb_flow.c.z; + float predRngMeas = MAX((terrainState - statesAtRngTime.position[2]),rngOnGnd) / Tnb_flow.c.z; // Copy required states to local variable names float q0 = statesAtRngTime.quat[0]; // quaternion at optical flow measurement time @@ -2475,7 +2475,7 @@ void NavEKF_core::EstimateTerrainOffset() varInnovRng = (R_RNG + Popt/sq(SK_RNG)); // constrain terrain height to be below the vehicle - terrainState = max(terrainState, statesAtRngTime.position[2] + rngOnGnd); + terrainState = MAX(terrainState, statesAtRngTime.position[2] + rngOnGnd); // Calculate the measurement innovation innovRng = predRngMeas - rngMea; @@ -2490,13 +2490,13 @@ void NavEKF_core::EstimateTerrainOffset() terrainState -= K_RNG * innovRng; // constrain the state - terrainState = max(terrainState, statesAtRngTime.position[2] + rngOnGnd); + terrainState = MAX(terrainState, statesAtRngTime.position[2] + rngOnGnd); // correct the covariance Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); // prevent the state variance from becoming negative - Popt = max(Popt,0.0f); + Popt = MAX(Popt,0.0f); } } @@ -2513,10 +2513,10 @@ void NavEKF_core::EstimateTerrainOffset() float H_OPT; // predict range to centre of image - float flowRngPred = max((terrainState - statesAtFlowTime.position[2]),rngOnGnd) / Tnb_flow.c.z; + float flowRngPred = MAX((terrainState - statesAtFlowTime.position[2]),rngOnGnd) / Tnb_flow.c.z; // constrain terrain height to be below the vehicle - terrainState = max(terrainState, statesAtFlowTime.position[2] + rngOnGnd); + terrainState = MAX(terrainState, statesAtFlowTime.position[2] + rngOnGnd); // calculate relative velocity in sensor frame relVelSensor = Tnb_flow*statesAtFlowTime.velocity; @@ -2572,19 +2572,19 @@ void NavEKF_core::EstimateTerrainOffset() auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(frontend._flowInnovGate) * auxFlowObsInnovVar); // don't fuse if optical flow data is outside valid range - if (max(flowRadXY[0],flowRadXY[1]) < frontend._maxFlowRate) { + if (MAX(flowRadXY[0],flowRadXY[1]) < frontend._maxFlowRate) { // correct the state terrainState -= K_OPT * auxFlowObsInnov; // constrain the state - terrainState = max(terrainState, statesAtFlowTime.position[2] + rngOnGnd); + terrainState = MAX(terrainState, statesAtFlowTime.position[2] + rngOnGnd); // correct the covariance Popt = Popt - K_OPT * H_OPT * Popt; // prevent the state variances from becoming negative - Popt = max(Popt,0.0f); + Popt = MAX(Popt,0.0f); } } } @@ -2623,7 +2623,7 @@ void NavEKF_core::FuseOptFlow() pd = statesAtFlowTime.position[2]; // constrain height above ground to be above range measured on ground - float heightAboveGndEst = max((terrainState - pd), rngOnGnd); + float heightAboveGndEst = MAX((terrainState - pd), rngOnGnd); // Calculate observation jacobians and Kalman gains if (obsIndex == 0) { // calculate range from ground plain to centre of sensor fov assuming flat earth @@ -3470,9 +3470,9 @@ void NavEKF_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGai { if (PV_AidingMode == AID_RELATIVE) { // allow 1.0 rad/sec margin for angular motion - ekfGndSpdLimit = max((frontend._maxFlowRate - 1.0f), 0.0f) * max((terrainState - state.position[2]), rngOnGnd); + ekfGndSpdLimit = MAX((frontend._maxFlowRate - 1.0f), 0.0f) * MAX((terrainState - state.position[2]), rngOnGnd); // use standard gains up to 5.0 metres height and reduce above that - ekfNavVelGainScaler = 4.0f / max((terrainState - state.position[2]),4.0f); + ekfNavVelGainScaler = 4.0f / MAX((terrainState - state.position[2]),4.0f); } else { ekfGndSpdLimit = 400.0f; //return 80% of max filter speed ekfNavVelGainScaler = 1.0f; @@ -3589,7 +3589,7 @@ bool NavEKF_core::getHAGL(float &HAGL) const // return data for debugging optical flow fusion void NavEKF_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const { - varFlow = max(flowTestRatio[0],flowTestRatio[1]); + varFlow = MAX(flowTestRatio[0],flowTestRatio[1]); gndOffset = terrainState; flowInnovX = innovOptFlow[0]; flowInnovY = innovOptFlow[1]; @@ -3790,7 +3790,7 @@ void NavEKF_core::ConstrainStates() // body magnetic field limit for (uint8_t i=19; i<=21; i++) states[i] = constrain_float(states[i],-0.5f,0.5f); // constrain the terrain offset state - terrainState = max(terrainState, state.position.z + rngOnGnd); + terrainState = MAX(terrainState, state.position.z + rngOnGnd); } bool NavEKF_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) { @@ -3954,7 +3954,7 @@ void NavEKF_core::readGpsData() if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) { gpsSpdAccuracy = 0.0f; } else { - gpsSpdAccuracy = max(gpsSpdAccuracy,gpsSpdAccRaw); + gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw); } // check if we have enough GPS satellites and increase the gps noise scaler if we don't @@ -4028,7 +4028,7 @@ void NavEKF_core::readHgtData() if (frontend._fusionModeGPS == 3 && frontend._altSource == 1) { if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) { // adjust range finder measurement to allow for effect of vehicle tilt and height of sensor - hgtMea = max(rngMea * Tnb_flow.c.z, rngOnGnd); + hgtMea = MAX(rngMea * Tnb_flow.c.z, rngOnGnd); // get states that were stored at the time closest to the measurement time, taking measurement delay into account statesAtHgtTime = statesAtFlowTime; // calculate offset to baro data that enables baro to be used as a backup @@ -4036,7 +4036,7 @@ void NavEKF_core::readHgtData() baroHgtOffset = 0.1f * (_baro.get_altitude() + state.position.z) + 0.9f * baroHgtOffset; } else if (vehicleArmed && takeOffDetected) { // use baro measurement and correct for baro offset - failsafe use only as baro will drift - hgtMea = max(_baro.get_altitude() - baroHgtOffset, rngOnGnd); + hgtMea = MAX(_baro.get_altitude() - baroHgtOffset, rngOnGnd); // get states that were stored at the time closest to the measurement time, taking measurement delay into account RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay)); } else { @@ -4065,7 +4065,7 @@ void NavEKF_core::readHgtData() } else if (vehicleArmed && getTakeoffExpected()) { // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent - hgtMea = max(hgtMea, meaHgtAtTakeOff); + hgtMea = MAX(hgtMea, meaHgtAtTakeOff); } // set flag to let other functions know new data has arrived @@ -5020,7 +5020,7 @@ bool NavEKF_core::calcGpsGoodToAlign(void) // Decay distance moved exponentially to zero gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst); // Clamp the fiter state to prevent excessive persistence of large transients - gpsDriftNE = min(gpsDriftNE,10.0f); + gpsDriftNE = MIN(gpsDriftNE,10.0f); // Fail if more than 3 metres drift after filtering whilst pre-armed when the vehicle is supposed to be stationary // This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m bool gpsDriftFail = (gpsDriftNE > 3.0f) && !vehicleArmed && (frontend._gpsCheck & MASK_GPS_POS_DRIFT); @@ -5151,7 +5151,7 @@ void NavEKF_core::readRangeFinder(void) } else { midIndex = 2; } - rngMea = max(storedRngMeas[midIndex],rngOnGnd); + rngMea = MAX(storedRngMeas[midIndex],rngOnGnd); newDataRng = true; rngValidMeaTime_ms = imuSampleTime_ms; // recall vehicle states at mid sample time for range finder @@ -5186,7 +5186,7 @@ bool NavEKF_core::getHeightControlLimit(float &height) const // only ask for limiting if we are doing optical flow navigation if (frontend._fusionModeGPS == 3) { // If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors - height = max(float(_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f); + height = MAX(float(_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f); return true; } else { return false; @@ -5293,7 +5293,7 @@ void NavEKF_core::calcGpsGoodForFlight(void) lpfFilterState = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * lpfFilterState),0.0f,10.0f); // apply a peak hold filter to the LPF output - peakHoldFilterState = max(lpfFilterState,((1.0f - alpha2) * peakHoldFilterState)); + peakHoldFilterState = MAX(lpfFilterState,((1.0f - alpha2) * peakHoldFilterState)); // Apply a threshold test with hysteresis to the filtered GPS speed accuracy data if (peakHoldFilterState > 1.5f ) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index 5321912128..7baf643b25 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp @@ -121,7 +121,7 @@ void NavEKF2_core::FuseAirspeed() innovVtas = VtasPred - tasDataDelayed.tas; // calculate the innovation consistency test ratio - tasTestRatio = sq(innovVtas) / (sq(max(0.01f * (float)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); + tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (float)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); // fail if the ratio is > 1, but don't fail if bad IMU data tasHealth = ((tasTestRatio < 1.0f) || badIMUdata); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 3313de4c81..b393db9174 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -81,7 +81,7 @@ void NavEKF2_core::alignYawGPS() float gpsYaw = atan2f(gpsDataNew.vel.y,gpsDataNew.vel.x); // Check the yaw angles for consistency - float yawErr = max(fabsf(wrap_PI(gpsYaw - velYaw)),max(fabsf(wrap_PI(gpsYaw - eulerAngles.z)),fabsf(wrap_PI(velYaw - eulerAngles.z)))); + float yawErr = MAX(fabsf(wrap_PI(gpsYaw - velYaw)),MAX(fabsf(wrap_PI(gpsYaw - eulerAngles.z)),fabsf(wrap_PI(velYaw - eulerAngles.z)))); // If the angles disagree by more than 45 degrees and GPS innovations are large, we declare the magnetic yaw as bad badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f)); @@ -321,7 +321,7 @@ void NavEKF2_core::FuseMagnetometer() // calculate the innovation test ratios for (uint8_t i = 0; i<=2; i++) { - magTestRatio[i] = sq(innovMag[i]) / (sq(max(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]); + magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]); } // check the last values from all components and set magnetometer health accordingly @@ -701,7 +701,7 @@ void NavEKF2_core::fuseCompass() } // calculate the innovation test ratio - yawTestRatio = sq(innovation) / (sq(max(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnov); + yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnov); // Declare the magnetometer unhealthy if the innovation test fails if (yawTestRatio > 1.0f) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 4245c5b8ca..579d06c88a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -68,7 +68,7 @@ void NavEKF2_core::readRangeFinder(void) } rangeDataNew.time_ms = storedRngMeasTime_ms[midIndex]; // limit the measured range to be no less than the on-ground range - rangeDataNew.rng = max(storedRngMeas[midIndex],rngOnGnd); + rangeDataNew.rng = MAX(storedRngMeas[midIndex],rngOnGnd); rngValidMeaTime_ms = imuSampleTime_ms; // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it storedRange.push(rangeDataNew); @@ -129,7 +129,7 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa // Correct for the average intersampling delay due to the filter updaterate ofDataNew.time_ms -= localFilterTimeStep_ms/2; // Prevent time delay exceeding age of oldest IMU data in the buffer - ofDataNew.time_ms = max(ofDataNew.time_ms,imuDataDelayed.time_ms); + ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms); // Save data to buffer storedOF.push(ofDataNew); // Check for data at the fusion time horizon @@ -251,7 +251,7 @@ void NavEKF2_core::readIMUData() } else { readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng); } - imuDataNew.delAngDT = max(ins.get_delta_time(),1.0e-4f); + imuDataNew.delAngDT = MAX(ins.get_delta_time(),1.0e-4f); // Get current time stamp imuDataNew.time_ms = imuSampleTime_ms; @@ -316,8 +316,8 @@ void NavEKF2_core::readIMUData() // extract the oldest available data from the FIFO buffer imuDataDelayed = storedIMU.pop_oldest_element(); float minDT = 0.1f*dtEkfAvg; - imuDataDelayed.delAngDT = max(imuDataDelayed.delAngDT,minDT); - imuDataDelayed.delVelDT = max(imuDataDelayed.delVelDT,minDT); + imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT); + imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT); } @@ -328,7 +328,7 @@ bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &d if (ins_index < ins.get_accel_count()) { ins.get_delta_velocity(ins_index,dVel); - dVel_dt = max(ins.get_delta_velocity_dt(ins_index),1.0e-4f); + dVel_dt = MAX(ins.get_delta_velocity_dt(ins_index),1.0e-4f); return true; } return false; @@ -362,7 +362,7 @@ void NavEKF2_core::readGpsData() gpsDataNew.time_ms -= localFilterTimeStep_ms/2; // Prevent time delay exceeding age of oldest IMU data in the buffer - gpsDataNew.time_ms = max(gpsDataNew.time_ms,imuDataDelayed.time_ms); + gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms); // read the NED velocity from the GPS gpsDataNew.vel = _ahrs->get_gps().velocity(); @@ -375,7 +375,7 @@ void NavEKF2_core::readGpsData() if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) { gpsSpdAccuracy = 0.0f; } else { - gpsSpdAccuracy = max(gpsSpdAccuracy,gpsSpdAccRaw); + gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw); } // check if we have enough GPS satellites and increase the gps noise scaler if we don't @@ -516,7 +516,7 @@ void NavEKF2_core::readBaroData() // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent if (isAiding && getTakeoffExpected()) { - baroDataNew.hgt = max(baroDataNew.hgt, meaHgtAtTakeOff); + baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff); } // time stamp used to check for new measurement @@ -529,7 +529,7 @@ void NavEKF2_core::readBaroData() baroDataNew.time_ms -= localFilterTimeStep_ms/2; // Prevent time delay exceeding age of oldest IMU data in the buffer - baroDataNew.time_ms = max(baroDataNew.time_ms,imuDataDelayed.time_ms); + baroDataNew.time_ms = MAX(baroDataNew.time_ms,imuDataDelayed.time_ms); // save baro measurement to buffer to be fused later storedBaro.push(baroDataNew); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index 25dd397115..ef79f0fb74 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -79,7 +79,7 @@ void NavEKF2_core::SelectFlowFusion() if (flowDataToFuse && tiltOK && PV_AidingMode == AID_RELATIVE) { // Set the flow noise used by the fusion processes - R_LOS = sq(max(frontend->_flowNoise, 0.05f)); + R_LOS = sq(MAX(frontend->_flowNoise, 0.05f)); // Fuse the optical flow X and Y axis data into the main filter sequentially FuseOptFlow(); // reset flag to indicate that no new flow data is available for fusion @@ -100,7 +100,7 @@ void NavEKF2_core::EstimateTerrainOffset() hal.util->perf_begin(_perf_TerrainOffset); // constrain height above ground to be above range measured on ground - float heightAboveGndEst = max((terrainState - stateStruct.position.z), rngOnGnd); + float heightAboveGndEst = MAX((terrainState - stateStruct.position.z), rngOnGnd); // calculate a predicted LOS rate squared float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y); @@ -117,12 +117,12 @@ void NavEKF2_core::EstimateTerrainOffset() // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning float distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE); - distanceTravelledSq = min(distanceTravelledSq, 100.0f); + distanceTravelledSq = MIN(distanceTravelledSq, 100.0f); prevPosN = stateStruct.position[0]; prevPosE = stateStruct.position[1]; // in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity - float timeLapsed = min(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f); + float timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f); float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend->gndGradientSigma))) + sq(timeLapsed)*P[5][5]; Popt += Pincrement; timeAtLastAuxEKF_ms = imuSampleTime_ms; @@ -130,7 +130,7 @@ void NavEKF2_core::EstimateTerrainOffset() // fuse range finder data if (rangeDataToFuse) { // predict range - float predRngMeas = max((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z; + float predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z; // Copy required states to local variable names float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time @@ -149,13 +149,13 @@ void NavEKF2_core::EstimateTerrainOffset() varInnovRng = (R_RNG + Popt/sq(SK_RNG)); // constrain terrain height to be below the vehicle - terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd); + terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd); // Calculate the measurement innovation innovRng = predRngMeas - rangeDataDelayed.rng; // calculate the innovation consistency test ratio - auxRngTestRatio = sq(innovRng) / (sq(max(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng); + auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng); // Check the innovation for consistency and don't fuse if > 5Sigma if ((sq(innovRng)*SK_RNG) < 25.0f) @@ -164,13 +164,13 @@ void NavEKF2_core::EstimateTerrainOffset() terrainState -= K_RNG * innovRng; // constrain the state - terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd); + terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd); // correct the covariance Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); // prevent the state variance from becoming negative - Popt = max(Popt,0.0f); + Popt = MAX(Popt,0.0f); } } @@ -187,10 +187,10 @@ void NavEKF2_core::EstimateTerrainOffset() float H_OPT; // predict range to centre of image - float flowRngPred = max((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z; + float flowRngPred = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z; // constrain terrain height to be below the vehicle - terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd); + terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd); // calculate relative velocity in sensor frame relVelSensor = Tnb_flow*stateStruct.velocity; @@ -243,22 +243,22 @@ void NavEKF2_core::EstimateTerrainOffset() K_OPT = Popt*H_OPT/auxFlowObsInnovVar; // calculate the innovation consistency test ratio - auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(max(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar); + auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar); // don't fuse if optical flow data is outside valid range - if (max(flowRadXY[0],flowRadXY[1]) < frontend->_maxFlowRate) { + if (MAX(flowRadXY[0],flowRadXY[1]) < frontend->_maxFlowRate) { // correct the state terrainState -= K_OPT * auxFlowObsInnov; // constrain the state - terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd); + terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd); // correct the covariance Popt = Popt - K_OPT * H_OPT * Popt; // prevent the state variances from becoming negative - Popt = max(Popt,0.0f); + Popt = MAX(Popt,0.0f); } } } @@ -291,7 +291,7 @@ void NavEKF2_core::FuseOptFlow() float pd = stateStruct.position.z; // constrain height above ground to be above range measured on ground - float heightAboveGndEst = max((terrainState - pd), rngOnGnd); + float heightAboveGndEst = MAX((terrainState - pd), rngOnGnd); float ptd = pd + heightAboveGndEst; // Calculate common expressions for observation jacobians @@ -627,7 +627,7 @@ void NavEKF2_core::FuseOptFlow() } // calculate the innovation consistency test ratio - flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(max(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]); + flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]); // Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index b1ed2ef6d0..7a90a824da 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -67,7 +67,7 @@ float NavEKF2_core::faultScore(void) const // return data for debugging optical flow fusion void NavEKF2_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const { - varFlow = max(flowTestRatio[0],flowTestRatio[1]); + varFlow = MAX(flowTestRatio[0],flowTestRatio[1]); gndOffset = terrainState; flowInnovX = innovOptFlow[0]; flowInnovY = innovOptFlow[1]; @@ -86,7 +86,7 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const // only ask for limiting if we are doing optical flow navigation if (frontend->_fusionModeGPS == 3) { // If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors - height = max(float(frontend->_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f); + height = MAX(float(frontend->_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f); // If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin if (frontend->_altSource != 1) { height -= terrainState; @@ -314,9 +314,9 @@ void NavEKF2_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGa { if (PV_AidingMode == AID_RELATIVE) { // allow 1.0 rad/sec margin for angular motion - ekfGndSpdLimit = max((frontend->_maxFlowRate - 1.0f), 0.0f) * max((terrainState - stateStruct.position[2]), rngOnGnd); + ekfGndSpdLimit = MAX((frontend->_maxFlowRate - 1.0f), 0.0f) * MAX((terrainState - stateStruct.position[2]), rngOnGnd); // use standard gains up to 5.0 metres height and reduce above that - ekfNavVelGainScaler = 4.0f / max((terrainState - stateStruct.position[2]),4.0f); + ekfNavVelGainScaler = 4.0f / MAX((terrainState - stateStruct.position[2]),4.0f); } else { ekfGndSpdLimit = 400.0f; //return 80% of max filter speed ekfNavVelGainScaler = 1.0f; @@ -376,9 +376,9 @@ void NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve posVar = sqrtf(posTestRatio); hgtVar = sqrtf(hgtTestRatio); // If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output - magVar.x = sqrtf(max(magTestRatio.x,yawTestRatio)); - magVar.y = sqrtf(max(magTestRatio.y,yawTestRatio)); - magVar.z = sqrtf(max(magTestRatio.z,yawTestRatio)); + magVar.x = sqrtf(MAX(magTestRatio.x,yawTestRatio)); + magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio)); + magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio)); tasVar = sqrtf(tasTestRatio); offset = posResetNE; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index ea8b2035d4..3b8fc1434f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -303,7 +303,7 @@ void NavEKF2_core::FuseVelPosNED() varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3]; varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4]; // apply an innovation consistency threshold test, but don't fail if bad IMU data - float maxPosInnov2 = sq(max(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]); + float maxPosInnov2 = sq(MAX(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]); posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2; posHealth = ((posTestRatio < 1.0f) || badIMUdata); // declare a timeout condition if we have been too long without data or not aiding @@ -361,7 +361,7 @@ void NavEKF2_core::FuseVelPosNED() } // apply an innovation consistency threshold test, but don't fail if bad IMU data // calculate the test ratio - velTestRatio = innovVelSumSq / (varVelSum * sq(max(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f))); + velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f))); // fail if the ratio is greater than 1 velHealth = ((velTestRatio < 1.0f) || badIMUdata); // declare a timeout if we have not fused velocity data for too long or not aiding @@ -391,7 +391,7 @@ void NavEKF2_core::FuseVelPosNED() innovVelPos[5] = stateStruct.position.z - observation[5]; varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5]; // calculate the innovation consistency test ratio - hgtTestRatio = sq(innovVelPos[5]) / (sq(max(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]); + hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]); // fail if the ratio is > 1, but don't fail if bad IMU data hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); // Fuse height data if healthy or timed out or in constant position mode @@ -580,7 +580,7 @@ void NavEKF2_core::selectHeightForFusion() // using range finder data // correct for tilt using a flat earth model if (prevTnb.c.z >= 0.7) { - hgtMea = max(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd); + hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd); // enable fusion fuseHgtData = true; // set the observation noise diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index ca38c56be9..eac51492c8 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -53,7 +53,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) // Decay distance moved exponentially to zero gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst); // Clamp the fiter state to prevent excessive persistence of large transients - gpsDriftNE = min(gpsDriftNE,10.0f); + gpsDriftNE = MIN(gpsDriftNE,10.0f); // Fail if more than 3 metres drift after filtering whilst on-ground // This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m bool gpsDriftFail = (gpsDriftNE > 3.0f*checkScaler) && onGround && (frontend->_gpsCheck & MASK_GPS_POS_DRIFT); @@ -233,7 +233,7 @@ void NavEKF2_core::calcGpsGoodForFlight(void) sAccFilterState1 = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f); // apply a peak hold filter to the LPF output - sAccFilterState2 = max(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2)); + sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2)); // Apply a threshold test with hysteresis to the filtered GPS speed accuracy data if (sAccFilterState2 > 1.5f ) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 80e4673937..24936bf894 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -118,7 +118,7 @@ void NavEKF2_core::InitialiseVariables() // calculate the nominal filter update rate const AP_InertialSensor &ins = _ahrs->get_ins(); localFilterTimeStep_ms = (uint8_t)(1000.0f/(float)ins.get_sample_rate()); - localFilterTimeStep_ms = max(localFilterTimeStep_ms,10); + localFilterTimeStep_ms = MAX(localFilterTimeStep_ms,10); // initialise time stamps imuSampleTime_ms = AP_HAL::millis(); @@ -280,7 +280,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) // Initialise IMU data dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate(); - dtEkfAvg = min(0.01f,dtIMUavg); + dtEkfAvg = MIN(0.01f,dtIMUavg); readIMUData(); storedIMU.reset_history(imuDataNew); imuDataDelayed = imuDataNew; @@ -1275,7 +1275,7 @@ void NavEKF2_core::ConstrainStates() // wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit for (uint8_t i=22; i<=23; i++) statesArray[i] = constrain_float(statesArray[i],-100.0f,100.0f); // constrain the terrain offset state - terrainState = max(terrainState, stateStruct.position.z + rngOnGnd); + terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd); } // calculate the NED earth spin vector in rad/sec diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index c49bfcb812..5a53226ea7 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -596,14 +596,14 @@ void RangeFinder::update_pre_arm_check(uint8_t instance) } // update min, max captured distances - state[instance].pre_arm_distance_min = min(state[instance].distance_cm, state[instance].pre_arm_distance_min); - state[instance].pre_arm_distance_max = max(state[instance].distance_cm, state[instance].pre_arm_distance_max); + state[instance].pre_arm_distance_min = MIN(state[instance].distance_cm, state[instance].pre_arm_distance_min); + state[instance].pre_arm_distance_max = MAX(state[instance].distance_cm, state[instance].pre_arm_distance_max); // Check that the range finder has been exercised through a realistic range of movement if (((state[instance].pre_arm_distance_max - state[instance].pre_arm_distance_min) > RANGEFINDER_PREARM_REQUIRED_CHANGE_CM) && (state[instance].pre_arm_distance_max < RANGEFINDER_PREARM_ALT_MAX_CM) && - ((int16_t)state[instance].pre_arm_distance_min < (max(_ground_clearance_cm[instance],min_distance_cm(instance)) + 10)) && - ((int16_t)state[instance].pre_arm_distance_min > (min(_ground_clearance_cm[instance],min_distance_cm(instance)) - 10))) { + ((int16_t)state[instance].pre_arm_distance_min < (MAX(_ground_clearance_cm[instance],min_distance_cm(instance)) + 10)) && + ((int16_t)state[instance].pre_arm_distance_min > (MIN(_ground_clearance_cm[instance],min_distance_cm(instance)) - 10))) { state[instance].pre_arm_check = true; } } diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 2b44a80cf1..6ebc36bebc 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -226,7 +226,7 @@ void AP_TECS::update_50hz(float hgt_afe) // Calculate time in seconds since last update uint32_t now = AP_HAL::micros(); - float DT = max((now - _update_50hz_last_usec),0)*1.0e-6f; + float DT = MAX((now - _update_50hz_last_usec),0)*1.0e-6f; if (DT > 1.0f) { _climb_rate = 0.0f; _height_filter.dd_height = 0.0f; @@ -284,7 +284,7 @@ void AP_TECS::_update_speed(float load_factor) { // Calculate time in seconds since last update uint32_t now = AP_HAL::micros(); - float DT = max((now - _update_speed_last_usec),0)*1.0e-6f; + float DT = MAX((now - _update_speed_last_usec),0)*1.0e-6f; _update_speed_last_usec = now; // Convert equivalent airspeeds to true airspeeds @@ -333,13 +333,13 @@ void AP_TECS::_update_speed(float load_factor) float integ4_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega; // Prevent state from winding up if (_integ5_state < 3.1f) { - integ4_input = max(integ4_input , 0.0f); + integ4_input = MAX(integ4_input , 0.0f); } _integ4_state = _integ4_state + integ4_input * DT; float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f; _integ5_state = _integ5_state + integ5_input * DT; // limit the airspeed to a minimum of 3 m/s - _integ5_state = max(_integ5_state, 3.0f); + _integ5_state = MAX(_integ5_state, 3.0f); } @@ -682,11 +682,11 @@ void AP_TECS::_update_pitch(void) float integ7_input = SEB_error * _integGain; if (_pitch_dem > _PITCHmaxf) { - integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem); + integ7_input = MIN(integ7_input, _PITCHmaxf - _pitch_dem); } else if (_pitch_dem < _PITCHminf) { - integ7_input = max(integ7_input, _PITCHminf - _pitch_dem); + integ7_input = MAX(integ7_input, _PITCHminf - _pitch_dem); } _integ7_state = _integ7_state + integ7_input * _DT; @@ -794,7 +794,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, { // Calculate time in seconds since last update uint32_t now = AP_HAL::micros(); - _DT = max((now - _update_pitch_throttle_last_usec),0)*1.0e-6f; + _DT = MAX((now - _update_pitch_throttle_last_usec),0)*1.0e-6f; _update_pitch_throttle_last_usec = now; // Update the speed estimate using a 2nd order complementary filter @@ -818,20 +818,20 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, if (_pitch_max <= 0) { _PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f; } else { - _PITCHmaxf = min(_pitch_max, aparm.pitch_limit_max_cd * 0.01f); + _PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f); } if (_pitch_min >= 0) { _PITCHminf = aparm.pitch_limit_min_cd * 0.01f; } else { - _PITCHminf = max(_pitch_min, aparm.pitch_limit_min_cd * 0.01f); + _PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min_cd * 0.01f); } if (flight_stage == FLIGHT_LAND_FINAL) { // in flare use min pitch from LAND_PITCH_CD - _PITCHminf = max(_PITCHminf, aparm.land_pitch_cd * 0.01f); + _PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f); // and use max pitch from TECS_LAND_PMAX if (_land_pitch_max > 0) { - _PITCHmaxf = min(_PITCHmaxf, _land_pitch_max); + _PITCHmaxf = MIN(_PITCHmaxf, _land_pitch_max); } // and allow zero throttle @@ -843,7 +843,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, float time_to_flare = (- hgt_afe / _climb_rate) - aparm.land_flare_sec; if (time_to_flare < 0) { // we should be flaring already - _PITCHminf = max(_PITCHminf, aparm.land_pitch_cd * 0.01f); + _PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f); } else if (time_to_flare < timeConstant()*2) { // smoothly move the min pitch to the flare min pitch over // twice the time constant @@ -853,7 +853,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, ::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n", time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate); #endif - _PITCHminf = max(_PITCHminf, pitch_limit_cd*0.01f); + _PITCHminf = MAX(_PITCHminf, pitch_limit_cd*0.01f); } } diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp index 5dc1788ba4..c18bd83972 100644 --- a/libraries/DataFlash/DataFlash_File.cpp +++ b/libraries/DataFlash/DataFlash_File.cpp @@ -962,7 +962,7 @@ void DataFlash_File::_io_timer(void) } if (_writebuf_head > _tail) { // only write to the end of the buffer - nbytes = min(nbytes, _writebuf_size - _writebuf_head); + nbytes = MIN(nbytes, _writebuf_size - _writebuf_head); } // try to align writes on a 512 byte boundary to avoid filesystem diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 1bcab9c031..8a3a23e15a 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1244,7 +1244,7 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) struct log_EKF5 pkt5 = { LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG), time_us : AP_HAL::micros64(), - normInnov : (uint8_t)(min(100*normInnov,255)), + normInnov : (uint8_t)(MIN(100*normInnov,255)), FIX : (int16_t)(1000*flowInnovX), FIY : (int16_t)(1000*flowInnovY), AFI : (int16_t)(1000*auxFlowInnov), @@ -1405,7 +1405,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) struct log_EKF5 pkt5 = { LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG), time_us : AP_HAL::micros64(), - normInnov : (uint8_t)(min(100*normInnov,255)), + normInnov : (uint8_t)(MIN(100*normInnov,255)), FIX : (int16_t)(1000*flowInnovX), FIY : (int16_t)(1000*flowInnovY), AFI : (int16_t)(1000*auxFlowInnov), diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 11b620b403..59f4599398 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -270,8 +270,8 @@ RC_Channel::zero_min_max() void RC_Channel::update_min_max() { - radio_min = min(radio_min.get(), radio_in); - radio_max = max(radio_max.get(), radio_in); + radio_min = MIN(radio_min.get(), radio_in); + radio_max = MAX(radio_max.get(), radio_in); } /*