Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Global: rename min and max macros to uppercase 

The problem with using min() and max() is that they conflict with some
C++ headers. Name the macros in uppercase instead. We may go case by
case later converting them to be typesafe.

Changes generated with:

	git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)max(/\1MAX(/g'
	git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)min(/\1MIN(/g'
This commit is contained in:
Lucas De Marchi 2015-11-27 15:11:58 -02:00
parent efbe350182
commit 2591261af6
58 changed files with 232 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
APMrover2/test.cpp
View File

@ -456,10 +456,10 @@ int8_t Rover::test_sonar(uint8_t argc, const Menu::arg *argv)
sonar_dist_cm_min = dist_cm; sonar_dist_cm_min = dist_cm;
voltage_min = voltage; voltage_min = voltage;
} }
sonar_dist_cm_max = max(sonar_dist_cm_max, dist_cm); sonar_dist_cm_max = MAX(sonar_dist_cm_max, dist_cm);
sonar_dist_cm_min = min(sonar_dist_cm_min, dist_cm); sonar_dist_cm_min = MIN(sonar_dist_cm_min, dist_cm);
voltage_min = min(voltage_min, voltage); voltage_min = MIN(voltage_min, voltage);
voltage_max = max(voltage_max, voltage); voltage_max = MAX(voltage_max, voltage);
dist_cm = sonar.distance_cm(1); dist_cm = sonar.distance_cm(1);
voltage = sonar.voltage_mv(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; sonar2_dist_cm_min = dist_cm;
voltage2_min = voltage; voltage2_min = voltage;
} }
sonar2_dist_cm_max = max(sonar2_dist_cm_max, dist_cm); sonar2_dist_cm_max = MAX(sonar2_dist_cm_max, dist_cm);
sonar2_dist_cm_min = min(sonar2_dist_cm_min, dist_cm); sonar2_dist_cm_min = MIN(sonar2_dist_cm_min, dist_cm);
voltage2_min = min(voltage2_min, voltage); voltage2_min = MIN(voltage2_min, voltage);
voltage2_max = max(voltage2_max, voltage); voltage2_max = MAX(voltage2_max, voltage);
if (now - last_print >= 200) { if (now - last_print >= 200) {
cliSerial->printf("sonar1 dist=%.1f:%.1fcm volt1=%.2f:%.2f sonar2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n", cliSerial->printf("sonar1 dist=%.1f:%.1fcm volt1=%.2f:%.2f sonar2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n",

8
ArduCopter/commands_logic.cpp
View File

@ -285,8 +285,8 @@ void Copter::do_takeoff(const AP_Mission::Mission_Command& cmd)
{ {
// Set wp navigation target to safe altitude above current position // Set wp navigation target to safe altitude above current position
float takeoff_alt = cmd.content.location.alt; float takeoff_alt = cmd.content.location.alt;
takeoff_alt = max(takeoff_alt,current_loc.alt); takeoff_alt = MAX(takeoff_alt,current_loc.alt);
takeoff_alt = max(takeoff_alt,100.0f); takeoff_alt = MAX(takeoff_alt,100.0f);
auto_takeoff_start(takeoff_alt); auto_takeoff_start(takeoff_alt);
} }
@ -750,7 +750,7 @@ void Copter::do_yaw(const AP_Mission::Mission_Command& cmd)
bool Copter::verify_wait_delay() 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; condition_value = 0;
return true; return true;
} }
@ -767,7 +767,7 @@ bool Copter::verify_within_distance()
{ {
// update distance calculation // update distance calculation
calc_wp_distance(); calc_wp_distance();
if (wp_distance < max(condition_value,0)) { if (wp_distance < MAX(condition_value,0)) {
condition_value = 0; condition_value = 0;
return true; return true;
} }

4
ArduCopter/compassmot.cpp
View File

@ -217,8 +217,8 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
} }
// record maximum throttle and current // record maximum throttle and current
throttle_pct_max = max(throttle_pct_max, throttle_pct); throttle_pct_max = MAX(throttle_pct_max, throttle_pct);
current_amps_max = max(current_amps_max, battery.current_amps()); current_amps_max = MAX(current_amps_max, battery.current_amps());
} }
if (AP_HAL::millis() - last_send_time > 500) { if (AP_HAL::millis() - last_send_time > 500) {

2
ArduCopter/control_acro.cpp
View File

@ -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.y += rate_bf_level.y;
rate_bf_request.z += rate_bf_level.z; rate_bf_request.z += rate_bf_level.z;
}else{ }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 // Scale leveling rates by stick input
rate_bf_level = rate_bf_level*acro_level_mix; rate_bf_level = rate_bf_level*acro_level_mix;

32
ArduCopter/control_autotune.cpp
View File

@ -639,16 +639,16 @@ void Copter::autotune_attitude_control()
autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1);
switch (autotune_state.axis) { switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL: case AUTOTUNE_AXIS_ROLL:
tune_roll_rd = max(g.autotune_min_d, tune_roll_rd * 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); tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF);
break; break;
case AUTOTUNE_AXIS_PITCH: case AUTOTUNE_AXIS_PITCH:
tune_pitch_rd = max(g.autotune_min_d, tune_pitch_rd * 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); tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF);
break; break;
case AUTOTUNE_AXIS_YAW: case AUTOTUNE_AXIS_YAW:
tune_yaw_rLPF = max(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * 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); tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF);
break; break;
} }
break; break;
@ -656,13 +656,13 @@ void Copter::autotune_attitude_control()
autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1); autotune_state.tune_type = AutoTuneTuneType(autotune_state.tune_type + 1);
switch (autotune_state.axis) { switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL: 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; break;
case AUTOTUNE_AXIS_PITCH: 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; break;
case AUTOTUNE_AXIS_YAW: 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;
} }
break; break;
@ -677,8 +677,8 @@ void Copter::autotune_attitude_control()
bool autotune_complete = false; bool autotune_complete = false;
switch (autotune_state.axis) { switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL: case AUTOTUNE_AXIS_ROLL:
tune_roll_sp = max(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_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); tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
if (autotune_pitch_enabled()) { if (autotune_pitch_enabled()) {
autotune_state.axis = AUTOTUNE_AXIS_PITCH; autotune_state.axis = AUTOTUNE_AXIS_PITCH;
} else if (autotune_yaw_enabled()) { } else if (autotune_yaw_enabled()) {
@ -688,8 +688,8 @@ void Copter::autotune_attitude_control()
} }
break; break;
case AUTOTUNE_AXIS_PITCH: case AUTOTUNE_AXIS_PITCH:
tune_pitch_sp = max(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_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); tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
if (autotune_yaw_enabled()) { if (autotune_yaw_enabled()) {
autotune_state.axis = AUTOTUNE_AXIS_YAW; autotune_state.axis = AUTOTUNE_AXIS_YAW;
} else { } else {
@ -697,8 +697,8 @@ void Copter::autotune_attitude_control()
} }
break; break;
case AUTOTUNE_AXIS_YAW: case AUTOTUNE_AXIS_YAW:
tune_yaw_sp = max(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_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); tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, autotune_test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF);
autotune_complete = true; autotune_complete = true;
break; break;
} }
@ -1062,7 +1062,7 @@ void Copter::autotune_twitching_test(float measurement, float target, float &mea
if (measurement_max < target * 0.75f) { if (measurement_max < target * 0.75f) {
// the measurement not reached the 90% threshold yet // 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 = 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) { if (measurement_max > target) {

4
ArduCopter/control_drift.cpp
View File

@ -68,7 +68,7 @@ void Copter::drift_run()
float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel
// gain sceduling for Yaw // 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; 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); roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
@ -87,7 +87,7 @@ void Copter::drift_run()
if(is_zero(target_pitch)){ if(is_zero(target_pitch)){
// .14/ (.03 * 100) = 4.6 seconds till full breaking // .14/ (.03 * 100) = 4.6 seconds till full breaking
breaker += .03f; breaker += .03f;
breaker = min(breaker, DRIFT_SPEEDGAIN); breaker = MIN(breaker, DRIFT_SPEEDGAIN);
target_pitch = pitch_vel * breaker; target_pitch = pitch_vel * breaker;
}else{ }else{
breaker = 0.0f; breaker = 0.0f;

6
ArduCopter/control_flip.cpp
View File

@ -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); attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0);
// decrease throttle // decrease throttle
if (throttle_out >= g.throttle_min) { 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 // 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); attitude_control.rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// decrease throttle // decrease throttle
if (throttle_out >= g.throttle_min) { 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 // 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); attitude_control.rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// decrease throttle // decrease throttle
if (throttle_out >= g.throttle_min) { 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 // check roll for inversion

2
ArduCopter/control_guided.cpp
View File

@ -459,7 +459,7 @@ void Copter::guided_angle_control_run()
float roll_in = guided_angle_state.roll_cd; float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd; float pitch_in = guided_angle_state.pitch_cd;
float total_in = pythagorous2(roll_in, pitch_in); 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) { if (total_in > angle_max) {
float ratio = angle_max / total_in; float ratio = angle_max / total_in;
roll_in *= ratio; roll_in *= ratio;

8
ArduCopter/control_poshold.cpp
View File

@ -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 // lean_angle_raw must be pulling lean_angle_filtered towards zero, smooth the decrease
if (lean_angle_filtered > 0) { if (lean_angle_filtered > 0) {
// reduce the filtered lean angle at 5% or the brake rate (whichever is faster). // 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. // 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 // 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{ }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 += 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 = MIN(lean_angle_filtered, lean_angle_raw);
} }
} }
} }

10
ArduCopter/control_rtl.cpp
View File

@ -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 // 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); 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 -= 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); destination.z = pv_alt_above_origin(rally_point.alt);
#else #else
destination.z = pv_alt_above_origin(rtl_alt); 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 // 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); 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 -= 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); Vector3f destination = pv_location_to_vector(rally_point);
#else #else
Vector3f destination = pv_location_to_vector(ahrs.get_home()); Vector3f destination = pv_location_to_vector(ahrs.get_home());
@ -420,13 +420,13 @@ void Copter::rtl_land_run()
float Copter::get_RTL_alt() float Copter::get_RTL_alt()
{ {
// maximum of current altitude + climb_min and rtl altitude // maximum of current altitude + climb_min and rtl altitude
float ret = max(current_loc.alt + max(0, g.rtl_climb_min), g.rtl_altitude); float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), g.rtl_altitude);
ret = max(ret, RTL_ALT_MIN); ret = MAX(ret, RTL_ALT_MIN);
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
// ensure not above fence altitude if alt fence is enabled // ensure not above fence altitude if alt fence is enabled
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { 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 #endif

2
ArduCopter/motor_test.cpp
View File

@ -129,7 +129,7 @@ uint8_t Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_s
// set timeout // set timeout
motor_test_start_ms = AP_HAL::millis(); 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 // store required output
motor_test_seq = motor_seq; motor_test_seq = motor_seq;

2
ArduCopter/sensors.cpp
View File

@ -59,7 +59,7 @@ int16_t Copter::read_sonar(void)
#if SONAR_TILT_CORRECTION == 1 #if SONAR_TILT_CORRECTION == 1
// correct alt for angle of the sonar // correct alt for angle of the sonar
float temp = ahrs.cos_pitch() * ahrs.cos_roll(); float temp = ahrs.cos_pitch() * ahrs.cos_roll();
temp = max(temp, 0.707f); temp = MAX(temp, 0.707f);
temp_alt = (float)temp_alt * temp; temp_alt = (float)temp_alt * temp;
#endif #endif

2
ArduCopter/switches.cpp
View File

@ -315,7 +315,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
cmd.p1 = 0; cmd.p1 = 0;
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 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. // use the current altitude for the target alt for takeoff.
// only altitude will matter to the AP mission script for takeoff. // only altitude will matter to the AP mission script for takeoff.

6
ArduCopter/takeoff.cpp
View File

@ -57,7 +57,7 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
void Copter::takeoff_timer_start(float alt_cm) void Copter::takeoff_timer_start(float alt_cm)
{ {
// calculate climb rate // 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 // sanity check speed and target
if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) { 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 // acceleration of 50cm/s/s
static const float takeoff_accel = 50.0f; 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 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 time_to_max_speed = (takeoff_state.max_speed-takeoff_minspeed)/takeoff_accel;
float height_gained; float height_gained;

4
ArduPlane/Attitude.cpp
View File

@ -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 = (float)channel->radio_in - (float)channel->radio_trim;
ch_inf = fabsf(ch_inf); 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); ch_inf = ((400.0f - ch_inf) / 400.0f);
servo_out *= ch_inf; servo_out *= ch_inf;
servo_out += channel->pwm_to_angle(); 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; uint32_t launch_duration_ms = ((int32_t)g.takeoff_throttle_delay)*100 + 2000;
if (is_flying() && 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 adjusted_relative_altitude_cm() > 500 && // are >5m above AGL/home
labs(ahrs.pitch_sensor) < 3000 && // not high pitch, which happens when held before launch labs(ahrs.pitch_sensor) < 3000 && // not high pitch, which happens when held before launch
gps_movement) { // definate gps movement gps_movement) { // definate gps movement

4
ArduPlane/commands_logic.cpp
View File

@ -692,7 +692,7 @@ bool Plane::verify_loiter_to_alt()
bool Plane::verify_RTL() bool Plane::verify_RTL()
{ {
update_loiter(); 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()) { nav_controller->reached_loiter_target()) {
gcs_send_text(MAV_SEVERITY_INFO,"Reached HOME"); gcs_send_text(MAV_SEVERITY_INFO,"Reached HOME");
return true; return true;
@ -826,7 +826,7 @@ bool Plane::verify_change_alt()
bool Plane::verify_within_distance() 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; condition_value = 0;
return true; return true;
} }

2
ArduPlane/geofence.cpp
View File

@ -49,7 +49,7 @@ static const StorageAccess fence_storage(StorageManager::StorageFence);
*/ */
uint8_t Plane::max_fencepoints(void) uint8_t Plane::max_fencepoints(void)
{ {
return min(255, fence_storage.size() / sizeof(Vector2l)); return MIN(255, fence_storage.size() / sizeof(Vector2l));
} }
/* /*

2
ArduPlane/takeoff.cpp
View File

@ -18,7 +18,7 @@ bool Plane::auto_takeoff_check(void)
static bool launchTimerStarted; static bool launchTimerStarted;
static uint32_t last_tkoff_arm_time; static uint32_t last_tkoff_arm_time;
static uint32_t last_check_ms; 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 // Reset states if process has been interrupted
if (last_check_ms && (now - last_check_ms) > 200) { if (last_check_ms && (now - last_check_ms) > 200) {

10
Tools/Replay/Replay.cpp
View File

@ -834,11 +834,11 @@ void Replay::log_check_solution(void)
float vel_error = (velocity - check_state.velocity).length(); float vel_error = (velocity - check_state.velocity).length();
float pos_error = get_distance(check_state.pos, loc); 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_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_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_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_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_pos_error = MAX(check_result.max_pos_error, pos_error);
} }

12
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -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. // jerk_z is calculated to reach full acceleration in 1000ms.
float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO; 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 += 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; 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); _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 // limit acceleration if necessary
if (use_althold_lean_angle) { 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 // 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; _accel_target_jerk_limited += accel_change;
// lowpass filter on NE accel // 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); Vector2f accel_target_filtered = _accel_target_filter.apply(_accel_target_jerk_limited, dt);
// rotate accelerations into body forward-right frame // 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 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 // 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_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_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 /// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain

2
libraries/AC_Fence/AC_Fence.cpp
View File

@ -237,7 +237,7 @@ float AC_Fence::get_breach_distance(uint8_t fence_type) const
return _circle_breach_distance; return _circle_breach_distance;
break; break;
case AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE: 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 // we don't recognise the fence type so just return 0

2
libraries/AC_PID/AC_PID.cpp
View File

@ -74,7 +74,7 @@ void AC_PID::filt_hz(float hz)
_filt_hz.set(fabsf(hz)); _filt_hz.set(fabsf(hz));
// sanity check _filt_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 // set_input_filter_all - set input to PID controller

4
libraries/AC_PID/AC_PI_2D.cpp
View File

@ -61,7 +61,7 @@ void AC_PI_2D::filt_hz(float hz)
_filt_hz.set(fabsf(hz)); _filt_hz.set(fabsf(hz));
// sanity check _filt_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 // calculate the input filter alpha
calc_filt_alpha(); calc_filt_alpha();
@ -110,7 +110,7 @@ Vector2f AC_PI_2D::get_i()
Vector2f AC_PI_2D::get_i_shrink() Vector2f AC_PI_2D::get_i_shrink()
{ {
if (!is_zero(_ki) && !is_zero(_dt)) { 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; _integrator += (_input * _ki) * _dt;
float integrator_length_new = _integrator.length(); float integrator_length_new = _integrator.length();
if ((integrator_length_new > integrator_length_orig) && (integrator_length_new > 0)) { if ((integrator_length_new > integrator_length_orig) && (integrator_length_new > 0)) {

2
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -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(); _ef_angle_to_target.y = y_rad*_ahrs.sin_yaw() + x_rad*_ahrs.cos_yaw();
// get current altitude (constrained to no lower than 50cm) // 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 // convert earth-frame angles to earth-frame position offset
_target_pos_offset.x = alt*tanf(_ef_angle_to_target.x); _target_pos_offset.x = alt*tanf(_ef_angle_to_target.x);

2
libraries/AC_Sprayer/AC_Sprayer.cpp
View File

@ -160,7 +160,7 @@ AC_Sprayer::update()
// if spraying or testing update the pump servo position // if spraying or testing update the pump servo position
if (_flags.spraying || _flags.testing) { 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); RC_Channel_aux::set_radio(RC_Channel_aux::k_sprayer_spinner, _spinner_pwm);
}else{ }else{
// ensure sprayer and spinner are off // ensure sprayer and spinner are off

10
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -121,11 +121,11 @@ void AC_Circle::update()
// ramp angular velocity to maximum // ramp angular velocity to maximum
if (_angular_vel < _angular_vel_max) { if (_angular_vel < _angular_vel_max) {
_angular_vel += fabsf(_angular_accel) * dt; _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) { if (_angular_vel > _angular_vel_max) {
_angular_vel -= fabsf(_angular_accel) * dt; _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 // 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 we are doing a panorama set the circle_angle to the current heading
if (_radius <= 0) { if (_radius <= 0) {
_angular_vel_max = ToRad(_rate); _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{ }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 // 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_velocity in radians per second
_angular_vel_max = velocity_max/_radius; _angular_vel_max = velocity_max/_radius;
_angular_vel_max = constrain_float(ToRad(_rate),-_angular_vel_max,_angular_vel_max); _angular_vel_max = constrain_float(ToRad(_rate),-_angular_vel_max,_angular_vel_max);
// angular_velocity in radians per second // 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 // initialise angular velocity

22
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -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 // 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 // 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); float gnd_speed_limit_cms = MIN(_loiter_speed_cms,ekfGndSpdLimit*100.0f);
gnd_speed_limit_cms = max(gnd_speed_limit_cms, 10.0f); gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, 10.0f);
// range check nav_dt // range check nav_dt
if( nav_dt < 0 ) { 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; 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) { 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; 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 // 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) { if (track_leash_slack < 0) {
track_desired_max = track_covered; track_desired_max = track_covered;
}else{ }else{
@ -595,7 +595,7 @@ void AC_WPNav::advance_wp_target_along_track(float dt)
} }
// if target is slowing down, limit the speed // if target is slowing down, limit the speed
if (_flags.slowing_down) { 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_speed = speed_z/pos_delta_unit_z;
_track_leash_length = leash_z/pos_delta_unit_z; _track_leash_length = leash_z/pos_delta_unit_z;
}else{ }else{
_track_accel = min(_wp_accel_z_cms/pos_delta_unit_z, _wp_accel_cms/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_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_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) // 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 // 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) { if (track_leash_slack < 0.0f) {
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 spline_dist_to_wp = (_destination - target_pos).length();
float vel_limit = _wp_speed_cms; float vel_limit = _wp_speed_cms;
if (!is_zero(dt)) { 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 // if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration

8
libraries/APM_Control/AP_PitchController.cpp
View File

@ -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; float integrator_delta = rate_error * ki_rate * delta_time * scaler;
if (_last_out < -45) { if (_last_out < -45) {
// prevent the integrator from increasing if surface defln demand is above the upper limit // 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) { } else if (_last_out > 45) {
// prevent the integrator from decreasing if surface defln demand is below the lower limit // 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; _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 // 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 // No conversion is required for K_D
float eas2tas = _ahrs.get_EAS2TAS(); 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 k_ff = gains.FF / eas2tas;
// Calculate the demanded control surface deflection // 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 // don't do turn coordination handling when at very high pitch angles
rate_offset = 0; rate_offset = 0;
} else { } 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) { if (inverted) {
rate_offset = -rate_offset; rate_offset = -rate_offset;

6
libraries/APM_Control/AP_RollController.cpp
View File

@ -103,7 +103,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
// No conversion is required for K_D // No conversion is required for K_D
float ki_rate = gains.I * gains.tau; float ki_rate = gains.I * gains.tau;
float eas2tas = _ahrs.get_EAS2TAS(); 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 k_ff = gains.FF / eas2tas;
float delta_time = (float)dt * 0.001f; 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; float integrator_delta = rate_error * ki_rate * delta_time * scaler;
// prevent the integrator from increasing if surface defln demand is above the upper limit // prevent the integrator from increasing if surface defln demand is above the upper limit
if (_last_out < -45) { if (_last_out < -45) {
integrator_delta = max(integrator_delta , 0); integrator_delta = MAX(integrator_delta , 0);
} else if (_last_out > 45) { } else if (_last_out > 45) {
// prevent the integrator from decreasing if surface defln demand is below the lower limit // 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; _pid_info.I += integrator_delta;
} }

6
libraries/APM_Control/AP_SteerController.cpp
View File

@ -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 // 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 // No conversion is required for K_D
float ki_rate = _K_I * _tau * 45.0f; 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 k_ff = _K_FF * 45.0f;
float delta_time = (float)dt * 0.001f; 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; float integrator_delta = rate_error * ki_rate * delta_time * scaler;
// prevent the integrator from increasing if steering defln demand is above the upper limit // prevent the integrator from increasing if steering defln demand is above the upper limit
if (_last_out < -45) { if (_last_out < -45) {
integrator_delta = max(integrator_delta , 0); integrator_delta = MAX(integrator_delta , 0);
} else if (_last_out > 45) { } else if (_last_out > 45) {
// prevent the integrator from decreasing if steering defln demand is below the lower limit // 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; _pid_info.I += integrator_delta;
} }

6
libraries/APM_Control/AP_YawController.cpp
View File

@ -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 // If no airspeed available use average of min and max
aspeed = 0.5f*(float(aspd_min) + float(aparm.airspeed_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) // Get body rate vector (radians/sec)
float omega_z = _ahrs.get_gyro().z; 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 // prevent the integrator from increasing if surface defln demand is above the upper limit
if (_last_out < -45) { if (_last_out < -45) {
_integrator += max(integ_in * delta_time , 0); _integrator += MAX(integ_in * delta_time , 0);
} else if (_last_out > 45) { } else if (_last_out > 45) {
// prevent the integrator from decreasing if surface defln demand is below the lower limit // 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 { } else {
_integrator += integ_in * delta_time; _integrator += integ_in * delta_time;
} }

2
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -232,7 +232,7 @@ void AP_Airspeed::read(void)
airspeed_pressure = fabsf(airspeed_pressure); airspeed_pressure = fabsf(airspeed_pressure);
break; break;
} }
airspeed_pressure = max(airspeed_pressure, 0); airspeed_pressure = MAX(airspeed_pressure, 0);
_last_pressure = airspeed_pressure; _last_pressure = airspeed_pressure;
_raw_airspeed = sqrtf(airspeed_pressure * _ratio); _raw_airspeed = sqrtf(airspeed_pressure * _ratio);
_airspeed = 0.7f * _airspeed + 0.3f * _raw_airspeed; _airspeed = 0.7f * _airspeed + 0.3f * _raw_airspeed;

6
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

@ -96,9 +96,9 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
P.b.z = P.c.y = P23; P.b.z = P.c.y = P23;
// Constrain diagonals to be non-negative - protects against rounding errors // Constrain diagonals to be non-negative - protects against rounding errors
P.a.x = max(P.a.x, 0.0f); P.a.x = MAX(P.a.x, 0.0f);
P.b.y = max(P.b.y, 0.0f); P.b.y = MAX(P.b.y, 0.0f);
P.c.z = max(P.c.z, 0.0f); P.c.z = MAX(P.c.z, 0.0f);
state.x = constrain_float(state.x, -aparm.airspeed_max, aparm.airspeed_max); state.x = constrain_float(state.x, -aparm.airspeed_max, aparm.airspeed_max);
state.y = constrain_float(state.y, -aparm.airspeed_max, aparm.airspeed_max); state.y = constrain_float(state.y, -aparm.airspeed_max, aparm.airspeed_max);

2
libraries/AP_GPS/AP_GPS_SBP.cpp
View File

@ -497,7 +497,7 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
sender_id : sender_id, sender_id : sender_id,
msg_len : msg_len, 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)); gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
if (msg_len > 64) { if (msg_len > 64) {

4
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -567,7 +567,7 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
float &trim_roll, float &trim_roll,
float &trim_pitch) 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 samples[INS_MAX_INSTANCES][6];
Vector3f new_offsets[INS_MAX_INSTANCES]; Vector3f new_offsets[INS_MAX_INSTANCES];
Vector3f new_scaling[INS_MAX_INSTANCES]; Vector3f new_scaling[INS_MAX_INSTANCES];
@ -914,7 +914,7 @@ bool AP_InertialSensor::use_accel(uint8_t instance) const
void void
AP_InertialSensor::_init_gyro() 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 last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES];
Vector3f new_gyro_offset[INS_MAX_INSTANCES]; Vector3f new_gyro_offset[INS_MAX_INSTANCES];
float best_diff[INS_MAX_INSTANCES]; float best_diff[INS_MAX_INSTANCES];

2
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
View File

@ -583,7 +583,7 @@ int16_t AP_InertialSensor_MPU9150::mpu_set_sample_rate(uint16_t rate)
data); data);
// sample_rate = 1000 / (1 + 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; return 0;
} }

6
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -210,7 +210,7 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep
// get time since last sample // get time since last sample
float dt = _accel_sample_time[i]; 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] ++; _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 // get time since last sample
float dt = _gyro_sample_time[i]; 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] ++; _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() void AP_InertialSensor_PX4::_get_sample()
{ {
for (uint8_t i=0; i<max(_num_accel_instances,_num_gyro_instances);i++) { for (uint8_t i=0; i<MAX(_num_accel_instances,_num_gyro_instances);i++) {
struct accel_report accel_report; struct accel_report accel_report;
struct gyro_report gyro_report; struct gyro_report gyro_report;

12
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -83,7 +83,7 @@ int32_t AP_L1_Control::target_bearing_cd(void) const
float AP_L1_Control::turn_distance(float wp_radius) const float AP_L1_Control::turn_distance(float wp_radius) const
{ {
wp_radius *= sq(_ahrs.get_EAS2TAS()); wp_radius *= sq(_ahrs.get_EAS2TAS());
return min(wp_radius, _L1_dist); return MIN(wp_radius, _L1_dist);
} }
/* /*
@ -192,7 +192,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct
//Otherwise do normal L1 guidance //Otherwise do normal L1 guidance
float WP_A_dist = A_air.length(); float WP_A_dist = A_air.length();
float alongTrackDist = A_air * AB; float alongTrackDist = A_air * AB;
if (WP_A_dist > _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 //Calc Nu to fly To WP A
Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft 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 ltrackVel = _groundspeed_vector * AB; // Velocity along track
float Nu2 = atan2f(xtrackVel,ltrackVel); float Nu2 = atan2f(xtrackVel,ltrackVel);
//Calculate Nu1 angle (Angle to L1 reference point) //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 //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg
sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f); sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f);
float Nu1 = asinf(sine_Nu1); float Nu1 = asinf(sine_Nu1);
@ -271,7 +271,7 @@ void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius
Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();
//Calculate groundspeed //Calculate groundspeed
float groundSpeed = max(_groundspeed_vector.length() , 1.0f); float groundSpeed = MAX(_groundspeed_vector.length() , 1.0f);
// update _target_bearing_cd // update _target_bearing_cd
@ -329,11 +329,11 @@ void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius
//Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way //Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way
if (ltrackVelCap < 0.0f && velTangent < 0.0f) { if (ltrackVelCap < 0.0f && velTangent < 0.0f) {
latAccDemCircPD = max(latAccDemCircPD, 0.0f); latAccDemCircPD = MAX(latAccDemCircPD, 0.0f);
} }
// Calculate centripetal acceleration demand // 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 //Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand
float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr); float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr);

4
libraries/AP_Math/AP_Math.h
View File

@ -221,8 +221,8 @@ static inline float pythagorous3(float a, float b, float c) {
#endif #endif
/* The following three functions used to be arduino core macros */ /* The following three functions used to be arduino core macros */
#define max(a,b) ((a)>(b)?(a):(b)) #define MAX(a,b) ((a)>(b)?(a):(b))
#define min(a,b) ((a)<(b)?(a):(b)) #define MIN(a,b) ((a)<(b)?(a):(b))
static inline float maxf(float a, float b) static inline float maxf(float a, float b)
{ {

4
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -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); 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 // 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_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_4] = MAX(motor_out[AP_MOTORS_MOT_4], out_min);
// send output to each motor // send output to each motor
hal.rcout->cork(); hal.rcout->cork();

10
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -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 #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 // 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; 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 // 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 // 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 = 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 = MAX(yaw_allowed, _yaw_headroom);
if (yaw_pwm >= 0) { if (yaw_pwm >= 0) {
// if yawing right // if yawing right
@ -291,7 +291,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
thr_adj = throttle_radio_output - out_best_thr_pwm; thr_adj = throttle_radio_output - out_best_thr_pwm;
// calculate upper and lower limits of thr_adj // 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 we are increasing the throttle (situation #2 above)..
if (thr_adj > 0) { if (thr_adj > 0) {
@ -306,7 +306,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// decrease throttle as close as possible to requested throttle // decrease throttle as close as possible to requested throttle
// without going under out_min_pwm or over out_max_pwm // without going under out_min_pwm or over out_max_pwm
// earlier code ensures we can't break both boundaries // 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) { if (thr_adj > thr_adj_max) {
thr_adj = thr_adj_max; thr_adj = thr_adj_max;
limit.throttle_upper = true; limit.throttle_upper = true;

6
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -268,7 +268,7 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
return; 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 // add current based voltage sag to battery voltage
float batt_voltage = _batt_voltage + _batt_current * _batt_resistance; 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 // slew _throttle_thr_mix to _throttle_thr_mix_desired
if (_throttle_thr_mix < _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) // 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) { } else if (_throttle_thr_mix > _throttle_thr_mix_desired) {
// reduce more slowly (from 0.9 to 0.1 in 1.6 seconds) // 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); _throttle_thr_mix = constrain_float(_throttle_thr_mix, 0.1f, 1.0f);
} }

2
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -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); 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 // 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 // TODO: set limits.roll_pitch and limits.yaw

6
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -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); 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 // 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_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_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_4] = MAX(motor_out[AP_MOTORS_MOT_4], out_min);
} }
hal.rcout->cork(); hal.rcout->cork();

52
libraries/AP_NavEKF/AP_NavEKF_core.cpp
View File

@ -744,7 +744,7 @@ void NavEKF_core::SelectFlowFusion()
memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta)); memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta));
flowUpdateCount = 0; flowUpdateCount = 0;
// Set the flow noise used by the fusion processes // 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 // ensure that the covariance prediction is up to date before fusing data
if (!covPredStep) CovariancePrediction(); if (!covPredStep) CovariancePrediction();
// Fuse the optical flow X and Y axis data into the main filter sequentially // 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); hal.util->perf_begin(_perf_OpticalFlowEKF);
// constrain height above ground to be above range measured on ground // 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 // calculate a predicted LOS rate squared
float velHorizSq = sq(state.velocity.x) + sq(state.velocity.y); 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 // 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 // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
float distanceTravelledSq = sq(statesAtRngTime.position[0] - prevPosN) + sq(statesAtRngTime.position[1] - prevPosE); 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]; prevPosN = statesAtRngTime.position[0];
prevPosE = statesAtRngTime.position[1]; 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 // 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); float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend._gndGradientSigma))) + sq(float(frontend._gndGradientSigma) * timeLapsed);
Popt += Pincrement; Popt += Pincrement;
timeAtLastAuxEKF_ms = imuSampleTime_ms; timeAtLastAuxEKF_ms = imuSampleTime_ms;
@ -2456,7 +2456,7 @@ void NavEKF_core::EstimateTerrainOffset()
// fuse range finder data // fuse range finder data
if (fuseRngData) { if (fuseRngData) {
// predict range // 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 // Copy required states to local variable names
float q0 = statesAtRngTime.quat[0]; // quaternion at optical flow measurement time 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)); varInnovRng = (R_RNG + Popt/sq(SK_RNG));
// constrain terrain height to be below the vehicle // 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 // Calculate the measurement innovation
innovRng = predRngMeas - rngMea; innovRng = predRngMeas - rngMea;
@ -2490,13 +2490,13 @@ void NavEKF_core::EstimateTerrainOffset()
terrainState -= K_RNG * innovRng; terrainState -= K_RNG * innovRng;
// constrain the state // constrain the state
terrainState = max(terrainState, statesAtRngTime.position[2] + rngOnGnd); terrainState = MAX(terrainState, statesAtRngTime.position[2] + rngOnGnd);
// correct the covariance // correct the covariance
Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); 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 // 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; float H_OPT;
// predict range to centre of image // 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 // 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 // calculate relative velocity in sensor frame
relVelSensor = Tnb_flow*statesAtFlowTime.velocity; relVelSensor = Tnb_flow*statesAtFlowTime.velocity;
@ -2572,19 +2572,19 @@ void NavEKF_core::EstimateTerrainOffset()
auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(frontend._flowInnovGate) * auxFlowObsInnovVar); auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(frontend._flowInnovGate) * auxFlowObsInnovVar);
// don't fuse if optical flow data is outside valid range // 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 // correct the state
terrainState -= K_OPT * auxFlowObsInnov; terrainState -= K_OPT * auxFlowObsInnov;
// constrain the state // constrain the state
terrainState = max(terrainState, statesAtFlowTime.position[2] + rngOnGnd); terrainState = MAX(terrainState, statesAtFlowTime.position[2] + rngOnGnd);
// correct the covariance // correct the covariance
Popt = Popt - K_OPT * H_OPT * Popt; Popt = Popt - K_OPT * H_OPT * Popt;
// prevent the state variances from becoming negative // 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]; pd = statesAtFlowTime.position[2];
// constrain height above ground to be above range measured on ground // 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 // Calculate observation jacobians and Kalman gains
if (obsIndex == 0) { if (obsIndex == 0) {
// calculate range from ground plain to centre of sensor fov assuming flat earth // 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) { if (PV_AidingMode == AID_RELATIVE) {
// allow 1.0 rad/sec margin for angular motion // 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 // 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 { } else {
ekfGndSpdLimit = 400.0f; //return 80% of max filter speed ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
ekfNavVelGainScaler = 1.0f; ekfNavVelGainScaler = 1.0f;
@ -3589,7 +3589,7 @@ bool NavEKF_core::getHAGL(float &HAGL) const
// return data for debugging optical flow fusion // 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 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; gndOffset = terrainState;
flowInnovX = innovOptFlow[0]; flowInnovX = innovOptFlow[0];
flowInnovY = innovOptFlow[1]; flowInnovY = innovOptFlow[1];
@ -3790,7 +3790,7 @@ void NavEKF_core::ConstrainStates()
// body magnetic field limit // body magnetic field limit
for (uint8_t i=19; i<=21; i++) states[i] = constrain_float(states[i],-0.5f,0.5f); for (uint8_t i=19; i<=21; i++) states[i] = constrain_float(states[i],-0.5f,0.5f);
// constrain the terrain offset state // 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) { 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)) { if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccuracy = 0.0f; gpsSpdAccuracy = 0.0f;
} else { } 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 // 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 (frontend._fusionModeGPS == 3 && frontend._altSource == 1) {
if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) { if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) {
// adjust range finder measurement to allow for effect of vehicle tilt and height of sensor // 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 // get states that were stored at the time closest to the measurement time, taking measurement delay into account
statesAtHgtTime = statesAtFlowTime; statesAtHgtTime = statesAtFlowTime;
// calculate offset to baro data that enables baro to be used as a backup // 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; baroHgtOffset = 0.1f * (_baro.get_altitude() + state.position.z) + 0.9f * baroHgtOffset;
} else if (vehicleArmed && takeOffDetected) { } else if (vehicleArmed && takeOffDetected) {
// use baro measurement and correct for baro offset - failsafe use only as baro will drift // 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 // get states that were stored at the time closest to the measurement time, taking measurement delay into account
RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay)); RecallStates(statesAtHgtTime, (imuSampleTime_ms - msecHgtDelay));
} else { } else {
@ -4065,7 +4065,7 @@ void NavEKF_core::readHgtData()
} else if (vehicleArmed && getTakeoffExpected()) { } 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 // 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 // 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 // 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 // Decay distance moved exponentially to zero
gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst); gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst);
// Clamp the fiter state to prevent excessive persistence of large transients // 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 // 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 // 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); bool gpsDriftFail = (gpsDriftNE > 3.0f) && !vehicleArmed && (frontend._gpsCheck & MASK_GPS_POS_DRIFT);
@ -5151,7 +5151,7 @@ void NavEKF_core::readRangeFinder(void)
} else { } else {
midIndex = 2; midIndex = 2;
} }
rngMea = max(storedRngMeas[midIndex],rngOnGnd); rngMea = MAX(storedRngMeas[midIndex],rngOnGnd);
newDataRng = true; newDataRng = true;
rngValidMeaTime_ms = imuSampleTime_ms; rngValidMeaTime_ms = imuSampleTime_ms;
// recall vehicle states at mid sample time for range finder // 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 // only ask for limiting if we are doing optical flow navigation
if (frontend._fusionModeGPS == 3) { 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 // 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; return true;
} else { } else {
return false; return false;
@ -5293,7 +5293,7 @@ void NavEKF_core::calcGpsGoodForFlight(void)
lpfFilterState = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * lpfFilterState),0.0f,10.0f); lpfFilterState = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * lpfFilterState),0.0f,10.0f);
// apply a peak hold filter to the LPF output // 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 // Apply a threshold test with hysteresis to the filtered GPS speed accuracy data
if (peakHoldFilterState > 1.5f ) { if (peakHoldFilterState > 1.5f ) {

2
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

@ -121,7 +121,7 @@ void NavEKF2_core::FuseAirspeed()
innovVtas = VtasPred - tasDataDelayed.tas; innovVtas = VtasPred - tasDataDelayed.tas;
// calculate the innovation consistency test ratio // 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 // fail if the ratio is > 1, but don't fail if bad IMU data
tasHealth = ((tasTestRatio < 1.0f) || badIMUdata); tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);

6
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -81,7 +81,7 @@ void NavEKF2_core::alignYawGPS()
float gpsYaw = atan2f(gpsDataNew.vel.y,gpsDataNew.vel.x); float gpsYaw = atan2f(gpsDataNew.vel.y,gpsDataNew.vel.x);
// Check the yaw angles for consistency // 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 // 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)); badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f));
@ -321,7 +321,7 @@ void NavEKF2_core::FuseMagnetometer()
// calculate the innovation test ratios // calculate the innovation test ratios
for (uint8_t i = 0; i<=2; i++) { 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 // 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 // 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 // Declare the magnetometer unhealthy if the innovation test fails
if (yawTestRatio > 1.0f) { if (yawTestRatio > 1.0f) {

20
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -68,7 +68,7 @@ void NavEKF2_core::readRangeFinder(void)
} }
rangeDataNew.time_ms = storedRngMeasTime_ms[midIndex]; rangeDataNew.time_ms = storedRngMeasTime_ms[midIndex];
// limit the measured range to be no less than the on-ground range // 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; rngValidMeaTime_ms = imuSampleTime_ms;
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
storedRange.push(rangeDataNew); 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 // Correct for the average intersampling delay due to the filter updaterate
ofDataNew.time_ms -= localFilterTimeStep_ms/2; ofDataNew.time_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer // 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 // Save data to buffer
storedOF.push(ofDataNew); storedOF.push(ofDataNew);
// Check for data at the fusion time horizon // Check for data at the fusion time horizon
@ -251,7 +251,7 @@ void NavEKF2_core::readIMUData()
} else { } else {
readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng); 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 // Get current time stamp
imuDataNew.time_ms = imuSampleTime_ms; imuDataNew.time_ms = imuSampleTime_ms;
@ -316,8 +316,8 @@ void NavEKF2_core::readIMUData()
// extract the oldest available data from the FIFO buffer // extract the oldest available data from the FIFO buffer
imuDataDelayed = storedIMU.pop_oldest_element(); imuDataDelayed = storedIMU.pop_oldest_element();
float minDT = 0.1f*dtEkfAvg; float minDT = 0.1f*dtEkfAvg;
imuDataDelayed.delAngDT = max(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = max(imuDataDelayed.delVelDT,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()) { if (ins_index < ins.get_accel_count()) {
ins.get_delta_velocity(ins_index,dVel); 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 true;
} }
return false; return false;
@ -362,7 +362,7 @@ void NavEKF2_core::readGpsData()
gpsDataNew.time_ms -= localFilterTimeStep_ms/2; gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer // 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 // read the NED velocity from the GPS
gpsDataNew.vel = _ahrs->get_gps().velocity(); gpsDataNew.vel = _ahrs->get_gps().velocity();
@ -375,7 +375,7 @@ void NavEKF2_core::readGpsData()
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) { if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccuracy = 0.0f; gpsSpdAccuracy = 0.0f;
} else { } 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 // 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 // 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 // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
if (isAiding && getTakeoffExpected()) { if (isAiding && getTakeoffExpected()) {
baroDataNew.hgt = max(baroDataNew.hgt, meaHgtAtTakeOff); baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
} }
// time stamp used to check for new measurement // time stamp used to check for new measurement
@ -529,7 +529,7 @@ void NavEKF2_core::readBaroData()
baroDataNew.time_ms -= localFilterTimeStep_ms/2; baroDataNew.time_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer // 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 // save baro measurement to buffer to be fused later
storedBaro.push(baroDataNew); storedBaro.push(baroDataNew);

34
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -79,7 +79,7 @@ void NavEKF2_core::SelectFlowFusion()
if (flowDataToFuse && tiltOK && PV_AidingMode == AID_RELATIVE) if (flowDataToFuse && tiltOK && PV_AidingMode == AID_RELATIVE)
{ {
// Set the flow noise used by the fusion processes // 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 // Fuse the optical flow X and Y axis data into the main filter sequentially
FuseOptFlow(); FuseOptFlow();
// reset flag to indicate that no new flow data is available for fusion // 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); hal.util->perf_begin(_perf_TerrainOffset);
// constrain height above ground to be above range measured on ground // 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 // calculate a predicted LOS rate squared
float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y); 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 // 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 // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
float distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE); 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]; prevPosN = stateStruct.position[0];
prevPosE = stateStruct.position[1]; 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 // 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]; float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend->gndGradientSigma))) + sq(timeLapsed)*P[5][5];
Popt += Pincrement; Popt += Pincrement;
timeAtLastAuxEKF_ms = imuSampleTime_ms; timeAtLastAuxEKF_ms = imuSampleTime_ms;
@ -130,7 +130,7 @@ void NavEKF2_core::EstimateTerrainOffset()
// fuse range finder data // fuse range finder data
if (rangeDataToFuse) { if (rangeDataToFuse) {
// predict range // 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 // Copy required states to local variable names
float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time 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)); varInnovRng = (R_RNG + Popt/sq(SK_RNG));
// constrain terrain height to be below the vehicle // 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 // Calculate the measurement innovation
innovRng = predRngMeas - rangeDataDelayed.rng; innovRng = predRngMeas - rangeDataDelayed.rng;
// calculate the innovation consistency test ratio // 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 // Check the innovation for consistency and don't fuse if > 5Sigma
if ((sq(innovRng)*SK_RNG) < 25.0f) if ((sq(innovRng)*SK_RNG) < 25.0f)
@ -164,13 +164,13 @@ void NavEKF2_core::EstimateTerrainOffset()
terrainState -= K_RNG * innovRng; terrainState -= K_RNG * innovRng;
// constrain the state // constrain the state
terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd); terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
// correct the covariance // correct the covariance
Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); 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 // 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; float H_OPT;
// predict range to centre of image // 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 // 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 // calculate relative velocity in sensor frame
relVelSensor = Tnb_flow*stateStruct.velocity; relVelSensor = Tnb_flow*stateStruct.velocity;
@ -243,22 +243,22 @@ void NavEKF2_core::EstimateTerrainOffset()
K_OPT = Popt*H_OPT/auxFlowObsInnovVar; K_OPT = Popt*H_OPT/auxFlowObsInnovVar;
// calculate the innovation consistency test ratio // 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 // 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 // correct the state
terrainState -= K_OPT * auxFlowObsInnov; terrainState -= K_OPT * auxFlowObsInnov;
// constrain the state // constrain the state
terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd); terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
// correct the covariance // correct the covariance
Popt = Popt - K_OPT * H_OPT * Popt; Popt = Popt - K_OPT * H_OPT * Popt;
// prevent the state variances from becoming negative // 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; float pd = stateStruct.position.z;
// constrain height above ground to be above range measured on ground // 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; float ptd = pd + heightAboveGndEst;
// Calculate common expressions for observation jacobians // Calculate common expressions for observation jacobians
@ -627,7 +627,7 @@ void NavEKF2_core::FuseOptFlow()
} }
// calculate the innovation consistency test ratio // 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 // 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)) { if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {

14
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -67,7 +67,7 @@ float NavEKF2_core::faultScore(void) const
// return data for debugging optical flow fusion // 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 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; gndOffset = terrainState;
flowInnovX = innovOptFlow[0]; flowInnovX = innovOptFlow[0];
flowInnovY = innovOptFlow[1]; 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 // only ask for limiting if we are doing optical flow navigation
if (frontend->_fusionModeGPS == 3) { 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 // 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 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) { if (frontend->_altSource != 1) {
height -= terrainState; height -= terrainState;
@ -314,9 +314,9 @@ void NavEKF2_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGa
{ {
if (PV_AidingMode == AID_RELATIVE) { if (PV_AidingMode == AID_RELATIVE) {
// allow 1.0 rad/sec margin for angular motion // 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 // 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 { } else {
ekfGndSpdLimit = 400.0f; //return 80% of max filter speed ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
ekfNavVelGainScaler = 1.0f; ekfNavVelGainScaler = 1.0f;
@ -376,9 +376,9 @@ void NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve
posVar = sqrtf(posTestRatio); posVar = sqrtf(posTestRatio);
hgtVar = sqrtf(hgtTestRatio); 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 // 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.x = sqrtf(MAX(magTestRatio.x,yawTestRatio));
magVar.y = sqrtf(max(magTestRatio.y,yawTestRatio)); magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio));
magVar.z = sqrtf(max(magTestRatio.z,yawTestRatio)); magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtf(tasTestRatio); tasVar = sqrtf(tasTestRatio);
offset = posResetNE; offset = posResetNE;
} }

8
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -303,7 +303,7 @@ void NavEKF2_core::FuseVelPosNED()
varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3]; varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4]; varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
// apply an innovation consistency threshold test, but don't fail if bad IMU data // 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; posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
posHealth = ((posTestRatio < 1.0f) || badIMUdata); posHealth = ((posTestRatio < 1.0f) || badIMUdata);
// declare a timeout condition if we have been too long without data or not aiding // 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 // apply an innovation consistency threshold test, but don't fail if bad IMU data
// calculate the test ratio // 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 // fail if the ratio is greater than 1
velHealth = ((velTestRatio < 1.0f) || badIMUdata); velHealth = ((velTestRatio < 1.0f) || badIMUdata);
// declare a timeout if we have not fused velocity data for too long or not aiding // 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]; innovVelPos[5] = stateStruct.position.z - observation[5];
varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5]; varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
// calculate the innovation consistency test ratio // 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 // fail if the ratio is > 1, but don't fail if bad IMU data
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
// Fuse height data if healthy or timed out or in constant position mode // 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 // using range finder data
// correct for tilt using a flat earth model // correct for tilt using a flat earth model
if (prevTnb.c.z >= 0.7) { if (prevTnb.c.z >= 0.7) {
hgtMea = max(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd); hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
// enable fusion // enable fusion
fuseHgtData = true; fuseHgtData = true;
// set the observation noise // set the observation noise

4
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -53,7 +53,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
// Decay distance moved exponentially to zero // Decay distance moved exponentially to zero
gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst); gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst);
// Clamp the fiter state to prevent excessive persistence of large transients // 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 // 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 // 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); 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); sAccFilterState1 = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
// apply a peak hold filter to the LPF output // 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 // Apply a threshold test with hysteresis to the filtered GPS speed accuracy data
if (sAccFilterState2 > 1.5f ) { if (sAccFilterState2 > 1.5f ) {

6
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -118,7 +118,7 @@ void NavEKF2_core::InitialiseVariables()
// calculate the nominal filter update rate // calculate the nominal filter update rate
const AP_InertialSensor &ins = _ahrs->get_ins(); const AP_InertialSensor &ins = _ahrs->get_ins();
localFilterTimeStep_ms = (uint8_t)(1000.0f/(float)ins.get_sample_rate()); 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 // initialise time stamps
imuSampleTime_ms = AP_HAL::millis(); imuSampleTime_ms = AP_HAL::millis();
@ -280,7 +280,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
// Initialise IMU data // Initialise IMU data
dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate(); dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate();
dtEkfAvg = min(0.01f,dtIMUavg); dtEkfAvg = MIN(0.01f,dtIMUavg);
readIMUData(); readIMUData();
storedIMU.reset_history(imuDataNew); storedIMU.reset_history(imuDataNew);
imuDataDelayed = 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 // 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); for (uint8_t i=22; i<=23; i++) statesArray[i] = constrain_float(statesArray[i],-100.0f,100.0f);
// constrain the terrain offset state // 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 // calculate the NED earth spin vector in rad/sec

8
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -596,14 +596,14 @@ void RangeFinder::update_pre_arm_check(uint8_t instance)
} }
// update min, max captured distances // 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_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_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 // 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) && 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) && (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 < (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 > (MIN(_ground_clearance_cm[instance],min_distance_cm(instance)) - 10))) {
state[instance].pre_arm_check = true; state[instance].pre_arm_check = true;
} }
} }

26
libraries/AP_TECS/AP_TECS.cpp
View File

@ -226,7 +226,7 @@ void AP_TECS::update_50hz(float hgt_afe)
// Calculate time in seconds since last update // Calculate time in seconds since last update
uint32_t now = AP_HAL::micros(); 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) { if (DT > 1.0f) {
_climb_rate = 0.0f; _climb_rate = 0.0f;
_height_filter.dd_height = 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 // Calculate time in seconds since last update
uint32_t now = AP_HAL::micros(); 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; _update_speed_last_usec = now;
// Convert equivalent airspeeds to true airspeeds // Convert equivalent airspeeds to true airspeeds
@ -333,13 +333,13 @@ void AP_TECS::_update_speed(float load_factor)
float integ4_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega; float integ4_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega;
// Prevent state from winding up // Prevent state from winding up
if (_integ5_state < 3.1f) { 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; _integ4_state = _integ4_state + integ4_input * DT;
float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f; float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
_integ5_state = _integ5_state + integ5_input * DT; _integ5_state = _integ5_state + integ5_input * DT;
// limit the airspeed to a minimum of 3 m/s // 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; float integ7_input = SEB_error * _integGain;
if (_pitch_dem > _PITCHmaxf) 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) 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; _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 // Calculate time in seconds since last update
uint32_t now = AP_HAL::micros(); 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_pitch_throttle_last_usec = now;
// Update the speed estimate using a 2nd order complementary filter // 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) { if (_pitch_max <= 0) {
_PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f; _PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f;
} else { } 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) { if (_pitch_min >= 0) {
_PITCHminf = aparm.pitch_limit_min_cd * 0.01f; _PITCHminf = aparm.pitch_limit_min_cd * 0.01f;
} else { } 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) { if (flight_stage == FLIGHT_LAND_FINAL) {
// in flare use min pitch from LAND_PITCH_CD // 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 // and use max pitch from TECS_LAND_PMAX
if (_land_pitch_max > 0) { if (_land_pitch_max > 0) {
_PITCHmaxf = min(_PITCHmaxf, _land_pitch_max); _PITCHmaxf = MIN(_PITCHmaxf, _land_pitch_max);
} }
// and allow zero throttle // 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; float time_to_flare = (- hgt_afe / _climb_rate) - aparm.land_flare_sec;
if (time_to_flare < 0) { if (time_to_flare < 0) {
// we should be flaring already // 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) { } else if (time_to_flare < timeConstant()*2) {
// smoothly move the min pitch to the flare min pitch over // smoothly move the min pitch to the flare min pitch over
// twice the time constant // 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", ::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); time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate);
#endif #endif
_PITCHminf = max(_PITCHminf, pitch_limit_cd*0.01f); _PITCHminf = MAX(_PITCHminf, pitch_limit_cd*0.01f);
} }
} }

2
libraries/DataFlash/DataFlash_File.cpp
View File

@ -962,7 +962,7 @@ void DataFlash_File::_io_timer(void)
} }
if (_writebuf_head > _tail) { if (_writebuf_head > _tail) {
// only write to the end of the buffer // 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 // try to align writes on a 512 byte boundary to avoid filesystem

4
libraries/DataFlash/LogFile.cpp
View File

@ -1244,7 +1244,7 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
struct log_EKF5 pkt5 = { struct log_EKF5 pkt5 = {
LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG), LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
normInnov : (uint8_t)(min(100*normInnov,255)), normInnov : (uint8_t)(MIN(100*normInnov,255)),
FIX : (int16_t)(1000*flowInnovX), FIX : (int16_t)(1000*flowInnovX),
FIY : (int16_t)(1000*flowInnovY), FIY : (int16_t)(1000*flowInnovY),
AFI : (int16_t)(1000*auxFlowInnov), 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 = { struct log_EKF5 pkt5 = {
LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG), LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
normInnov : (uint8_t)(min(100*normInnov,255)), normInnov : (uint8_t)(MIN(100*normInnov,255)),
FIX : (int16_t)(1000*flowInnovX), FIX : (int16_t)(1000*flowInnovX),
FIY : (int16_t)(1000*flowInnovY), FIY : (int16_t)(1000*flowInnovY),
AFI : (int16_t)(1000*auxFlowInnov), AFI : (int16_t)(1000*auxFlowInnov),

4
libraries/RC_Channel/RC_Channel.cpp
View File

@ -270,8 +270,8 @@ RC_Channel::zero_min_max()
void void
RC_Channel::update_min_max() RC_Channel::update_min_max()
{ {
radio_min = min(radio_min.get(), radio_in); radio_min = MIN(radio_min.get(), radio_in);
radio_max = max(radio_max.get(), radio_in); radio_max = MAX(radio_max.get(), radio_in);
} }
/* /*