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

Coter: compiler warnings: apply is_zero(float) or is_equal(float) and float to doubles

This commit is contained in:
Tom Pittenger 2015-05-02 02:26:19 -07:00 committed by Andrew Tridgell
parent d8d8593708
commit 20dc48ed16
10 changed files with 65 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Attitude.pde
View File

@ -82,7 +82,7 @@ static float get_look_ahead_yaw()
static void update_thr_average()
{
// ensure throttle_average has been initialised
if( throttle_average == 0 ) {
if( AP_Math::is_zero(throttle_average) ) {
throttle_average = g.throttle_mid;
// update position controller
pos_control.set_throttle_hover(throttle_average);

36
ArduCopter/GCS_Mavlink.pde
View File

@ -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, !packet.param3)) {
if(do_user_takeoff(takeoff_alt, AP_Math::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) &&
((packet.param4 == 0) || (packet.param4 == 1))) {
(AP_Math::is_zero(packet.param4) || AP_Math::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(packet.param1 == 1 || (packet.param5 == 0 && packet.param6 == 0 && packet.param7 == 0)) {
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 (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 (packet.param1 == 1) {
if (AP_Math::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 (packet.param3 == 1) {
} else if (AP_Math::is_equal(packet.param3,1.0f)) {
// fast barometer calibration
init_barometer(false);
result = MAV_RESULT_ACCEPTED;
} else if (packet.param4 == 1) {
} else if (AP_Math::is_equal(packet.param4,1.0f)) {
result = MAV_RESULT_UNSUPPORTED;
} else if (packet.param5 == 1) {
} else if (AP_Math::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 (packet.param6 == 1) {
} else if (AP_Math::is_equal(packet.param6,1.0f)) {
// compassmot calibration
result = mavlink_compassmot(chan);
}
break;
case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
if (packet.param1 == 2) {
if (AP_Math::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 (packet.param1 == 5) {
if (AP_Math::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 (packet.param1 == 1.0f) {
if (AP_Math::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 (packet.param1 == 0.0f && (mode_has_manual_throttle(control_mode) || ap.land_complete)) {
} else if (AP_Math::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 (packet.param1 == 1 || packet.param1 == 3) {
if (AP_Math::is_equal(packet.param1,1.0f) || AP_Math::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(packet.param1 == 3);
hal.scheduler->reboot(AP_Math::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 (packet.param1 == 1) {
if (AP_Math::is_equal(packet.param1,1.0f)) {
gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
result = MAV_RESULT_ACCEPTED;
}
@ -1481,9 +1481,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// m/s/s
Vector3f accels;
accels.x = packet.xacc * (GRAVITY_MSS/1000.0);
accels.y = packet.yacc * (GRAVITY_MSS/1000.0);
accels.z = packet.zacc * (GRAVITY_MSS/1000.0);
accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
ins.set_gyro(0, gyros);

2
ArduCopter/commands_logic.pde
View File

@ -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 (circle_center.z == 0) {
if (AP_Math::is_zero(circle_center.z)) {
circle_center.z = curr_pos.z;
}

6
ArduCopter/control_auto.pde
View File

@ -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 (target_yaw_rate != 0) {
if (!AP_Math::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 (target_yaw_rate != 0) {
if (!AP_Math::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 (turn_rate_dps == 0 ) {
if (AP_Math::is_zero(turn_rate_dps)) {
// default to regular auto slew rate
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
}else{

36
ArduCopter/control_autotune.pde
View File

@ -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 (target_roll != 0 || target_pitch != 0 || target_yaw_rate != 0.0f || target_climb_rate != 0) {
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 (!autotune_state.pilot_override) {
autotune_state.pilot_override = true;
// set gains to their original values
@ -334,7 +334,7 @@ static void autotune_attitude_control()
{
float rotation_rate = 0.0f; // rotation rate in radians/second
float lean_angle = 0.0f;
const float direction_sign = autotune_state.positive_direction ? 1.0 : -1.0;
const float direction_sign = autotune_state.positive_direction ? 1.0f : -1.0f;
// check tuning step
switch (autotune_state.step) {
@ -747,7 +747,7 @@ static void autotune_backup_gains_and_initialise()
autotune_desired_yaw = ahrs.yaw_sensor;
g.autotune_aggressiveness = constrain_float(g.autotune_aggressiveness, 0.05, 0.1);
g.autotune_aggressiveness = constrain_float(g.autotune_aggressiveness, 0.05f, 0.1f);
// backup original pids and initialise tuned pid values
if (autotune_roll_enabled()) {
@ -789,7 +789,7 @@ static void autotune_load_orig_gains()
// sanity check the gains
bool failed = false;
if (autotune_roll_enabled()) {
if ((orig_roll_rp != 0) || (orig_roll_sp != 0)) {
if (!AP_Math::is_zero(orig_roll_rp) || !AP_Math::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 ((orig_pitch_rp != 0) || (orig_pitch_sp != 0)) {
if (!AP_Math::is_zero(orig_pitch_rp) || !AP_Math::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 ((orig_yaw_rp != 0) || (orig_yaw_sp != 0) || (orig_yaw_rLPF != 0)) {
if (!AP_Math::is_zero(orig_yaw_rp) || !AP_Math::is_zero(orig_yaw_sp) || !AP_Math::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 (tune_roll_rp != 0) {
if (!AP_Math::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 (tune_pitch_rp != 0) {
if (!AP_Math::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 (tune_yaw_rp != 0) {
if (!AP_Math::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() && (orig_roll_rp != 0)) {
if (autotune_roll_enabled() && !AP_Math::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() && (orig_pitch_rp != 0)) {
if (autotune_pitch_enabled() && !AP_Math::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() && (orig_yaw_rp != 0)) {
if (autotune_yaw_enabled() && !AP_Math::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 (tune_roll_rp != 0) {
if (!AP_Math::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 (tune_pitch_rp != 0) {
if (!AP_Math::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 (tune_yaw_rp != 0) {
if (!AP_Math::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() && (tune_roll_rp != 0)) {
if (autotune_roll_enabled() && !AP_Math::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() && (tune_pitch_rp != 0)) {
if (autotune_pitch_enabled() && !AP_Math::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() && (tune_yaw_rp != 0)) {
if (autotune_yaw_enabled() && !AP_Math::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);

2
ArduCopter/control_circle.pde
View File

@ -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 (target_yaw_rate != 0) {
if (!AP_Math::is_zero(target_yaw_rate)) {
circle_pilot_yaw_override = true;
}

2
ArduCopter/control_drift.pde
View File

@ -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(target_pitch == 0){
if(AP_Math::is_zero(target_pitch)){
// .14/ (.03 * 100) = 4.6 seconds till full breaking
breaker += .03f;
breaker = min(breaker, DRIFT_SPEEDGAIN);

10
ArduCopter/control_guided.pde
View File

@ -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 (target_yaw_rate != 0) {
if (!AP_Math::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 (target_yaw_rate != 0) {
if (!AP_Math::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 (target_yaw_rate != 0) {
if (!AP_Math::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 ((guided_limit.alt_min_cm != 0.0f) && (curr_pos.z < guided_limit.alt_min_cm)) {
if (!AP_Math::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 ((guided_limit.alt_max_cm != 0.0f) && (curr_pos.z > guided_limit.alt_max_cm)) {
if (!AP_Math::is_zero(guided_limit.alt_max_cm) && (curr_pos.z > guided_limit.alt_max_cm)) {
return true;
}

28
ArduCopter/control_poshold.pde
View File

@ -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 ((target_roll == 0) && (abs(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) {
if (AP_Math::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 (target_roll != 0) {
if (!AP_Math::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 ((target_pitch == 0) && (abs(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) {
if (AP_Math::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 (target_pitch != 0) {
if (!AP_Math::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 (target_roll != 0 || target_pitch != 0) {
if (!AP_Math::is_zero(target_roll) || !AP_Math::is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (target_roll != 0) {
if (!AP_Math::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 (target_pitch != 0) {
if (!AP_Math::is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
if (target_roll == 0) {
if (AP_Math::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 (target_roll != 0 || target_pitch != 0) {
if (!AP_Math::is_zero(target_roll) || !AP_Math::is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (target_roll != 0) {
if (!AP_Math::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 (target_pitch != 0) {
if (!AP_Math::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 (target_roll == 0) {
if (AP_Math::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 (poshold.wind_comp_ef.x == 0) {
if (AP_Math::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 (poshold.wind_comp_ef.y == 0) {
if (AP_Math::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 {

6
ArduCopter/control_rtl.pde
View File

@ -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 (target_yaw_rate != 0) {
if (!AP_Math::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 (target_yaw_rate != 0) {
if (!AP_Math::is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
@ -294,7 +294,7 @@ static void rtl_descent_run()
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// check if we've reached within 20cm of final altitude
rtl_state_complete = fabs(pv_alt_above_origin(g.rtl_alt_final) - inertial_nav.get_altitude()) < 20.0f;
rtl_state_complete = fabsf(pv_alt_above_origin(g.rtl_alt_final) - inertial_nav.get_altitude()) < 20.0f;
}
// rtl_loiterathome_start - initialise controllers to loiter over home