From d8146ff3f65d0458bc188d5cdfff48345b12f727 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 May 2015 12:34:21 +1000 Subject: [PATCH] Copter: revert AP_Math class change --- ArduCopter/Attitude.pde | 2 +- ArduCopter/GCS_Mavlink.pde | 30 +++++++++++++++--------------- ArduCopter/commands_logic.pde | 2 +- ArduCopter/control_auto.pde | 6 +++--- ArduCopter/control_autotune.pde | 32 ++++++++++++++++---------------- ArduCopter/control_circle.pde | 2 +- ArduCopter/control_drift.pde | 2 +- ArduCopter/control_guided.pde | 10 +++++----- ArduCopter/control_poshold.pde | 28 ++++++++++++++-------------- ArduCopter/control_rtl.pde | 4 ++-- 10 files changed, 59 insertions(+), 59 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index a2306cb81a..4edb8a4f2c 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -82,7 +82,7 @@ static float get_look_ahead_yaw() static void update_thr_average() { // ensure throttle_average has been initialised - if( AP_Math::is_zero(throttle_average) ) { + if( is_zero(throttle_average) ) { throttle_average = g.throttle_mid; // update position controller pos_control.set_throttle_hover(throttle_average); diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index cac372adb6..f4d28ef676 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1053,7 +1053,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) float takeoff_alt = packet.param7 * 100; // Convert m to cm - if(do_user_takeoff(takeoff_alt, AP_Math::is_zero(packet.param3))) { + if(do_user_takeoff(takeoff_alt, is_zero(packet.param3))) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; @@ -1087,7 +1087,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // param4 : relative offset (1) or absolute angle (0) if ((packet.param1 >= 0.0f) && (packet.param1 <= 360.0f) && - (AP_Math::is_zero(packet.param4) || AP_Math::is_equal(packet.param4,1.0f))) { + (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) { set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4); result = MAV_RESULT_ACCEPTED; } else { @@ -1114,7 +1114,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // param6 : longitude // param7 : altitude (absolute) result = MAV_RESULT_FAILED; // assume failure - if(AP_Math::is_equal(packet.param1,1.0f) || (AP_Math::is_zero(packet.param5) && AP_Math::is_zero(packet.param6) && AP_Math::is_zero(packet.param7))) { + if(is_equal(packet.param1,1.0f) || (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7))) { if (set_home_to_current_location_and_lock()) { result = MAV_RESULT_ACCEPTED; } @@ -1158,7 +1158,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_FAILED; break; } - if (AP_Math::is_equal(packet.param1,1.0f)) { + if (is_equal(packet.param1,1.0f)) { // gyro offset calibration ins.init_gyro(); // reset ahrs gyro bias @@ -1168,13 +1168,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } else { result = MAV_RESULT_FAILED; } - } else if (AP_Math::is_equal(packet.param3,1.0f)) { + } else if (is_equal(packet.param3,1.0f)) { // fast barometer calibration init_barometer(false); result = MAV_RESULT_ACCEPTED; - } else if (AP_Math::is_equal(packet.param4,1.0f)) { + } else if (is_equal(packet.param4,1.0f)) { result = MAV_RESULT_UNSUPPORTED; - } else if (AP_Math::is_equal(packet.param5,1.0f)) { + } else if (is_equal(packet.param5,1.0f)) { // 3d accel calibration float trim_roll, trim_pitch; // this blocks @@ -1186,19 +1186,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } else { result = MAV_RESULT_FAILED; } - } else if (AP_Math::is_equal(packet.param6,1.0f)) { + } else if (is_equal(packet.param6,1.0f)) { // compassmot calibration result = mavlink_compassmot(chan); } break; case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: - if (AP_Math::is_equal(packet.param1,2.0f)) { + if (is_equal(packet.param1,2.0f)) { // save first compass's offsets compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4); result = MAV_RESULT_ACCEPTED; } - if (AP_Math::is_equal(packet.param1,5.0f)) { + if (is_equal(packet.param1,5.0f)) { // save secondary compass's offsets compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4); result = MAV_RESULT_ACCEPTED; @@ -1206,12 +1206,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_COMPONENT_ARM_DISARM: - if (AP_Math::is_equal(packet.param1,1.0f)) { + if (is_equal(packet.param1,1.0f)) { // attempt to arm and return success or failure if (init_arm_motors(true)) { result = MAV_RESULT_ACCEPTED; } - } else if (AP_Math::is_zero(packet.param1) && (mode_has_manual_throttle(control_mode) || ap.land_complete)) { + } else if (is_zero(packet.param1) && (mode_has_manual_throttle(control_mode) || ap.land_complete)) { init_disarm_motors(); result = MAV_RESULT_ACCEPTED; } else { @@ -1244,12 +1244,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (AP_Math::is_equal(packet.param1,1.0f) || AP_Math::is_equal(packet.param1,3.0f)) { + if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) { AP_Notify::events.firmware_update = 1; update_notify(); hal.scheduler->delay(50); // when packet.param1 == 3 we reboot to hold in bootloader - hal.scheduler->reboot(AP_Math::is_equal(packet.param1,3.0f)); + hal.scheduler->reboot(is_equal(packet.param1,3.0f)); result = MAV_RESULT_ACCEPTED; } break; @@ -1329,7 +1329,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { - if (AP_Math::is_equal(packet.param1,1.0f)) { + if (is_equal(packet.param1,1.0f)) { gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); result = MAV_RESULT_ACCEPTED; } diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 81d2574bb1..8be42cbce4 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -684,7 +684,7 @@ static bool verify_circle(const AP_Mission::Mission_Command& cmd) Vector3f circle_center = pv_location_to_vector(cmd.content.location); // set target altitude if not provided - if (AP_Math::is_zero(circle_center.z)) { + if (is_zero(circle_center.z)) { circle_center.z = curr_pos.z; } diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 386510eac2..a1af2f17c0 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -171,7 +171,7 @@ static void auto_wp_run() if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); - if (!AP_Math::is_zero(target_yaw_rate)) { + if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } @@ -227,7 +227,7 @@ static void auto_spline_run() if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); - if (!AP_Math::is_zero(target_yaw_rate)) { + if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } @@ -550,7 +550,7 @@ static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, i } // get turn speed - if (AP_Math::is_zero(turn_rate_dps)) { + if (is_zero(turn_rate_dps)) { // default to regular auto slew rate yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; }else{ diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index 424c432765..ee461d36fd 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -295,7 +295,7 @@ static void autotune_run() pos_control.set_alt_target_to_current_alt(); }else{ // check if pilot is overriding the controls - if (!AP_Math::is_zero(target_roll) || !AP_Math::is_zero(target_pitch) || !AP_Math::is_zero(target_yaw_rate) || !AP_Math::is_zero(target_climb_rate)) { + if (!is_zero(target_roll) || !is_zero(target_pitch) || !is_zero(target_yaw_rate) || !is_zero(target_climb_rate)) { if (!autotune_state.pilot_override) { autotune_state.pilot_override = true; // set gains to their original values @@ -789,7 +789,7 @@ static void autotune_load_orig_gains() // sanity check the gains bool failed = false; if (autotune_roll_enabled()) { - if (!AP_Math::is_zero(orig_roll_rp) || !AP_Math::is_zero(orig_roll_sp)) { + if (!is_zero(orig_roll_rp) || !is_zero(orig_roll_sp)) { g.pid_rate_roll.kP(orig_roll_rp); g.pid_rate_roll.kI(orig_roll_ri); g.pid_rate_roll.kD(orig_roll_rd); @@ -799,7 +799,7 @@ static void autotune_load_orig_gains() } } if (autotune_pitch_enabled()) { - if (!AP_Math::is_zero(orig_pitch_rp) || !AP_Math::is_zero(orig_pitch_sp)) { + if (!is_zero(orig_pitch_rp) || !is_zero(orig_pitch_sp)) { g.pid_rate_pitch.kP(orig_pitch_rp); g.pid_rate_pitch.kI(orig_pitch_ri); g.pid_rate_pitch.kD(orig_pitch_rd); @@ -809,7 +809,7 @@ static void autotune_load_orig_gains() } } if (autotune_yaw_enabled()) { - if (!AP_Math::is_zero(orig_yaw_rp) || !AP_Math::is_zero(orig_yaw_sp) || !AP_Math::is_zero(orig_yaw_rLPF)) { + if (!is_zero(orig_yaw_rp) || !is_zero(orig_yaw_sp) || !is_zero(orig_yaw_rLPF)) { g.pid_rate_yaw.kP(orig_yaw_rp); g.pid_rate_yaw.kI(orig_yaw_ri); g.pid_rate_yaw.kD(orig_yaw_rd); @@ -831,7 +831,7 @@ static void autotune_load_tuned_gains() // sanity check the gains bool failed = true; if (autotune_roll_enabled()) { - if (!AP_Math::is_zero(tune_roll_rp)) { + if (!is_zero(tune_roll_rp)) { g.pid_rate_roll.kP(tune_roll_rp); g.pid_rate_roll.kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); g.pid_rate_roll.kD(tune_roll_rd); @@ -840,7 +840,7 @@ static void autotune_load_tuned_gains() } } if (autotune_pitch_enabled()) { - if (!AP_Math::is_zero(tune_pitch_rp)) { + if (!is_zero(tune_pitch_rp)) { g.pid_rate_pitch.kP(tune_pitch_rp); g.pid_rate_pitch.kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); g.pid_rate_pitch.kD(tune_pitch_rd); @@ -849,7 +849,7 @@ static void autotune_load_tuned_gains() } } if (autotune_yaw_enabled()) { - if (!AP_Math::is_zero(tune_yaw_rp)) { + if (!is_zero(tune_yaw_rp)) { g.pid_rate_yaw.kP(tune_yaw_rp); g.pid_rate_yaw.kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); g.pid_rate_yaw.kD(0.0f); @@ -871,21 +871,21 @@ static void autotune_load_intra_test_gains() // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term) // sanity check the gains bool failed = true; - if (autotune_roll_enabled() && !AP_Math::is_zero(orig_roll_rp)) { + if (autotune_roll_enabled() && !is_zero(orig_roll_rp)) { g.pid_rate_roll.kP(orig_roll_rp); g.pid_rate_roll.kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); g.pid_rate_roll.kD(orig_roll_rd); g.p_stabilize_roll.kP(orig_roll_sp); failed = false; } - if (autotune_pitch_enabled() && !AP_Math::is_zero(orig_pitch_rp)) { + if (autotune_pitch_enabled() && !is_zero(orig_pitch_rp)) { g.pid_rate_pitch.kP(orig_pitch_rp); g.pid_rate_pitch.kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); g.pid_rate_pitch.kD(orig_pitch_rd); g.p_stabilize_pitch.kP(orig_pitch_sp); failed = false; } - if (autotune_yaw_enabled() && !AP_Math::is_zero(orig_yaw_rp)) { + if (autotune_yaw_enabled() && !is_zero(orig_yaw_rp)) { g.pid_rate_yaw.kP(orig_yaw_rp); g.pid_rate_yaw.kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); g.pid_rate_yaw.kD(orig_yaw_rd); @@ -906,7 +906,7 @@ static void autotune_load_twitch_gains() bool failed = true; switch (autotune_state.axis) { case AUTOTUNE_AXIS_ROLL: - if (!AP_Math::is_zero(tune_roll_rp)) { + if (!is_zero(tune_roll_rp)) { g.pid_rate_roll.kP(tune_roll_rp); g.pid_rate_roll.kI(tune_roll_rp*0.01f); g.pid_rate_roll.kD(tune_roll_rd); @@ -915,7 +915,7 @@ static void autotune_load_twitch_gains() } break; case AUTOTUNE_AXIS_PITCH: - if (!AP_Math::is_zero(tune_pitch_rp)) { + if (!is_zero(tune_pitch_rp)) { g.pid_rate_pitch.kP(tune_pitch_rp); g.pid_rate_pitch.kI(tune_pitch_rp*0.01f); g.pid_rate_pitch.kD(tune_pitch_rd); @@ -924,7 +924,7 @@ static void autotune_load_twitch_gains() } break; case AUTOTUNE_AXIS_YAW: - if (!AP_Math::is_zero(tune_yaw_rp)) { + if (!is_zero(tune_yaw_rp)) { g.pid_rate_yaw.kP(tune_yaw_rp); g.pid_rate_yaw.kI(tune_yaw_rp*0.01f); g.pid_rate_yaw.kD(0.0f); @@ -947,7 +947,7 @@ static void autotune_save_tuning_gains() // if we successfully completed tuning if (autotune_state.mode == AUTOTUNE_MODE_SUCCESS) { // sanity check the rate P values - if (autotune_roll_enabled() && !AP_Math::is_zero(tune_roll_rp)) { + if (autotune_roll_enabled() && !is_zero(tune_roll_rp)) { // rate roll gains g.pid_rate_roll.kP(tune_roll_rp); g.pid_rate_roll.kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); @@ -968,7 +968,7 @@ static void autotune_save_tuning_gains() orig_roll_sp = g.p_stabilize_roll.kP(); } - if (autotune_pitch_enabled() && !AP_Math::is_zero(tune_pitch_rp)) { + if (autotune_pitch_enabled() && !is_zero(tune_pitch_rp)) { // rate pitch gains g.pid_rate_pitch.kP(tune_pitch_rp); g.pid_rate_pitch.kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); @@ -989,7 +989,7 @@ static void autotune_save_tuning_gains() orig_pitch_sp = g.p_stabilize_pitch.kP(); } - if (autotune_yaw_enabled() && !AP_Math::is_zero(tune_yaw_rp)) { + if (autotune_yaw_enabled() && !is_zero(tune_yaw_rp)) { // rate yaw gains g.pid_rate_yaw.kP(tune_yaw_rp); g.pid_rate_yaw.kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde index 9e6b21f204..385c531aff 100644 --- a/ArduCopter/control_circle.pde +++ b/ArduCopter/control_circle.pde @@ -44,7 +44,7 @@ static void circle_run() if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); - if (!AP_Math::is_zero(target_yaw_rate)) { + if (!is_zero(target_yaw_rate)) { circle_pilot_yaw_override = true; } diff --git a/ArduCopter/control_drift.pde b/ArduCopter/control_drift.pde index a64f66c9c3..046f19d0ab 100644 --- a/ArduCopter/control_drift.pde +++ b/ArduCopter/control_drift.pde @@ -82,7 +82,7 @@ static void drift_run() target_roll = constrain_int16(target_roll, -4500, 4500); // If we let go of sticks, bring us to a stop - if(AP_Math::is_zero(target_pitch)){ + if(is_zero(target_pitch)){ // .14/ (.03 * 100) = 4.6 seconds till full breaking breaker += .03f; breaker = min(breaker, DRIFT_SPEEDGAIN); diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index bed67bf565..764b99dd94 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -232,7 +232,7 @@ static void guided_pos_control_run() if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); - if (!AP_Math::is_zero(target_yaw_rate)) { + if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } @@ -262,7 +262,7 @@ static void guided_vel_control_run() if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); - if (!AP_Math::is_zero(target_yaw_rate)) { + if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } @@ -301,7 +301,7 @@ static void guided_posvel_control_run() if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); - if (!AP_Math::is_zero(target_yaw_rate)) { + if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } @@ -389,12 +389,12 @@ static bool guided_limit_check() const Vector3f& curr_pos = inertial_nav.get_position(); // check if we have gone below min alt - if (!AP_Math::is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) { + if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) { return true; } // check if we have gone above max alt - if (!AP_Math::is_zero(guided_limit.alt_max_cm) && (curr_pos.z > guided_limit.alt_max_cm)) { + if (!is_zero(guided_limit.alt_max_cm) && (curr_pos.z > guided_limit.alt_max_cm)) { return true; } diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index fcff21140e..c741748b2b 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -228,7 +228,7 @@ static void poshold_run() poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); // switch to BRAKE mode for next iteration if no pilot input - if (AP_Math::is_zero(target_roll) && (abs(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) { + if (is_zero(target_roll) && (abs(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode poshold.roll_mode = POSHOLD_BRAKE; // Set brake roll mode poshold.brake_roll = 0; // initialise braking angle to zero @@ -277,7 +277,7 @@ static void poshold_run() poshold.roll = poshold.brake_roll + poshold.wind_comp_roll; // check for pilot input - if (!AP_Math::is_zero(target_roll)) { + if (!is_zero(target_roll)) { // init transition to pilot override poshold_roll_controller_to_pilot_override(); } @@ -322,7 +322,7 @@ static void poshold_run() poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); // switch to BRAKE mode for next iteration if no pilot input - if (AP_Math::is_zero(target_pitch) && (abs(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) { + if (is_zero(target_pitch) && (abs(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode poshold.pitch_mode = POSHOLD_BRAKE; // set brake pitch mode poshold.brake_pitch = 0; // initialise braking angle to zero @@ -371,7 +371,7 @@ static void poshold_run() poshold.pitch = poshold.brake_pitch + poshold.wind_comp_pitch; // check for pilot input - if (!AP_Math::is_zero(target_pitch)) { + if (!is_zero(target_pitch)) { // init transition to pilot override poshold_pitch_controller_to_pilot_override(); } @@ -453,9 +453,9 @@ static void poshold_run() poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, wp_nav.get_pitch()); // check for pilot input - if (!AP_Math::is_zero(target_roll) || !AP_Math::is_zero(target_pitch)) { + if (!is_zero(target_roll) || !is_zero(target_pitch)) { // if roll input switch to pilot override for roll - if (!AP_Math::is_zero(target_roll)) { + if (!is_zero(target_roll)) { // init transition to pilot override poshold_roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) @@ -463,10 +463,10 @@ static void poshold_run() poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; } // if pitch input switch to pilot override for pitch - if (!AP_Math::is_zero(target_pitch)) { + if (!is_zero(target_pitch)) { // init transition to pilot override poshold_pitch_controller_to_pilot_override(); - if (AP_Math::is_zero(target_roll)) { + if (is_zero(target_roll)) { // switch roll-mode to brake (but ready to go back to loiter anytime) // no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; @@ -487,9 +487,9 @@ static void poshold_run() poshold_update_wind_comp_estimate(); // check for pilot input - if (!AP_Math::is_zero(target_roll) || !AP_Math::is_zero(target_pitch)) { + if (!is_zero(target_roll) || !is_zero(target_pitch)) { // if roll input switch to pilot override for roll - if (!AP_Math::is_zero(target_roll)) { + if (!is_zero(target_roll)) { // init transition to pilot override poshold_roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) @@ -498,11 +498,11 @@ static void poshold_run() poshold.brake_pitch = 0; } // if pitch input switch to pilot override for pitch - if (!AP_Math::is_zero(target_pitch)) { + if (!is_zero(target_pitch)) { // init transition to pilot override poshold_pitch_controller_to_pilot_override(); // if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time) - if (AP_Math::is_zero(target_roll)) { + if (is_zero(target_roll)) { poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; poshold.brake_roll = 0; } @@ -613,14 +613,14 @@ static void poshold_update_wind_comp_estimate() const Vector3f& accel_target = pos_control.get_accel_target(); // update wind compensation in earth-frame lean angles - if (AP_Math::is_zero(poshold.wind_comp_ef.x)) { + if (is_zero(poshold.wind_comp_ef.x)) { // if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction poshold.wind_comp_ef.x = accel_target.x; } else { // low pass filter the position controller's lean angle output poshold.wind_comp_ef.x = (1.0f-TC_WIND_COMP)*poshold.wind_comp_ef.x + TC_WIND_COMP*accel_target.x; } - if (AP_Math::is_zero(poshold.wind_comp_ef.y)) { + if (is_zero(poshold.wind_comp_ef.y)) { // if wind compensation has not been initialised set it immediately to the pos controller's desired accel in north direction poshold.wind_comp_ef.y = accel_target.y; } else { diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 5b69c73259..fa8526b5de 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -144,7 +144,7 @@ static void rtl_climb_return_run() if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); - if (!AP_Math::is_zero(target_yaw_rate)) { + if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } @@ -200,7 +200,7 @@ static void rtl_loiterathome_run() if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); - if (!AP_Math::is_zero(target_yaw_rate)) { + if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } }