diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 234eb0ff1c..65383503b4 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -905,10 +905,10 @@ protected: private: void update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); - int16_t mix_controls(float mix_ratio, int16_t first_control, int16_t second_control); - void update_brake_angle_from_velocity(int16_t &brake_angle, float velocity); + float mix_controls(float mix_ratio, float first_control, float second_control); + void update_brake_angle_from_velocity(float &brake_angle, float velocity); void update_wind_comp_estimate(); - void get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle); + void get_wind_comp_lean_angles(float &roll_angle, float &pitch_angle); void roll_controller_to_pilot_override(); void pitch_controller_to_pilot_override(); @@ -933,30 +933,30 @@ private: // braking related variables float brake_gain; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from brake_rate) - int16_t brake_roll; // target roll angle during braking periods - int16_t brake_pitch; // target pitch angle during braking periods + float brake_roll; // target roll angle during braking periods + float brake_pitch; // target pitch angle during braking periods int16_t brake_timeout_roll; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking int16_t brake_timeout_pitch; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking - int16_t brake_angle_max_roll; // maximum lean angle achieved during braking. Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time - int16_t brake_angle_max_pitch; // maximum lean angle achieved during braking Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time + float brake_angle_max_roll; // maximum lean angle achieved during braking. Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time + float brake_angle_max_pitch; // maximum lean angle achieved during braking Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time int16_t brake_to_loiter_timer; // cycles to mix brake and loiter controls in POSHOLD_BRAKE_TO_LOITER // loiter related variables int16_t controller_to_pilot_timer_roll; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT int16_t controller_to_pilot_timer_pitch; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT - int16_t controller_final_roll; // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input) - int16_t controller_final_pitch; // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input) + float controller_final_roll; // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input) + float controller_final_pitch; // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input) // wind compensation related variables Vector2f wind_comp_ef; // wind compensation in earth frame, filtered lean angles from position controller - int16_t wind_comp_roll; // roll angle to compensate for wind - int16_t wind_comp_pitch; // pitch angle to compensate for wind + float wind_comp_roll; // roll angle to compensate for wind + float wind_comp_pitch; // pitch angle to compensate for wind uint16_t wind_comp_start_timer; // counter to delay start of wind compensation for a short time after loiter is engaged int8_t wind_comp_timer; // counter to reduce wind comp roll/pitch lean angle calcs to 10hz // final output - int16_t roll; // final roll angle sent to attitude controller - int16_t pitch; // final pitch angle sent to attitude controller + float roll; // final roll angle sent to attitude controller + float pitch; // final pitch angle sent to attitude controller }; diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 1af1442738..7ba3407745 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -37,8 +37,8 @@ bool ModePosHold::init(bool ignore_checks) } // initialise lean angles to current attitude - pilot_roll = 0; - pilot_pitch = 0; + pilot_roll = 0.0f; + pilot_pitch = 0.0f; // compute brake_gain brake_gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f; @@ -59,9 +59,9 @@ bool ModePosHold::init(bool ignore_checks) // initialise wind_comp each time PosHold is switched on wind_comp_ef.zero(); - wind_comp_roll = 0; - wind_comp_pitch = 0; wind_comp_timer = 0; + wind_comp_roll = 0.0f; + wind_comp_pitch = 0.0f; return true; } @@ -211,8 +211,8 @@ void ModePosHold::run() if (is_zero(target_roll) && (fabsf(pilot_roll) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode roll_mode = RPMode::BRAKE; // Set brake roll mode - brake_roll = 0; // initialise braking angle to zero - brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking + brake_roll = 0.0f; // initialise braking angle to zero + brake_angle_max_roll = 0.0f; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. braking_time_updated_roll = false; // flag the braking time can be re-estimated } @@ -229,11 +229,11 @@ void ModePosHold::run() // update braking time estimate if (!braking_time_updated_roll) { // check if brake angle is increasing - if (abs(brake_roll) >= brake_angle_max_roll) { - brake_angle_max_roll = abs(brake_roll); + if (fabsf(brake_roll) >= brake_angle_max_roll) { + brake_angle_max_roll = fabsf(brake_roll); } else { // braking angle has started decreasing so re-estimate braking time - brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. + brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(fabsf(brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. braking_time_updated_roll = true; } } @@ -305,8 +305,8 @@ void ModePosHold::run() if (is_zero(target_pitch) && (fabsf(pilot_pitch) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode pitch_mode = RPMode::BRAKE; // set brake pitch mode - brake_pitch = 0; // initialise braking angle to zero - brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking + brake_pitch = 0.0f; // initialise braking angle to zero + brake_angle_max_pitch = 0.0f; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. braking_time_updated_pitch = false; // flag the braking time can be re-estimated } @@ -323,11 +323,11 @@ void ModePosHold::run() // update braking time estimate if (!braking_time_updated_pitch) { // check if brake angle is increasing - if (abs(brake_pitch) >= brake_angle_max_pitch) { - brake_angle_max_pitch = abs(brake_pitch); + if (fabsf(brake_pitch) >= brake_angle_max_pitch) { + brake_angle_max_pitch = fabsf(brake_pitch); } else { // braking angle has started decreasing so re-estimate braking time - brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. + brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(fabsf(brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. braking_time_updated_pitch = true; } } @@ -473,7 +473,7 @@ void ModePosHold::run() // switch pitch-mode to brake (but ready to go back to loiter anytime) pitch_mode = RPMode::BRAKE_READY_TO_LOITER; // reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle - brake_pitch = 0; + brake_pitch = 0.0f; } // if pitch input switch to pilot override for pitch if (!is_zero(target_pitch)) { @@ -482,7 +482,7 @@ void ModePosHold::run() // if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time) if (is_zero(target_roll)) { roll_mode = RPMode::BRAKE_READY_TO_LOITER; - brake_roll = 0; + brake_roll = 0.0f; } // if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time) } @@ -497,8 +497,8 @@ void ModePosHold::run() // constrain target pitch/roll angles float angle_max = copter.aparm.angle_max; - roll = constrain_int16(roll, -angle_max, angle_max); - pitch = constrain_int16(pitch, -angle_max, angle_max); + roll = constrain_float(roll, -angle_max, angle_max); + pitch = constrain_float(pitch, -angle_max, angle_max); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll, pitch, target_yaw_rate); @@ -530,37 +530,37 @@ void ModePosHold::update_pilot_lean_angle(float &lean_angle_filtered, float &lea // mix_controls - mixes two controls based on the mix_ratio // mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly -int16_t ModePosHold::mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) +float ModePosHold::mix_controls(float mix_ratio, float first_control, float second_control) { mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f); - return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control)); + return mix_ratio * first_control + (1.0f - mix_ratio) * second_control; } // update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain // brake_angle is slewed with the wpnav.poshold_brake_rate and constrained by the wpnav.poshold_braking_angle_max // velocity is assumed to be in the same direction as lean angle so for pitch you should provide the velocity backwards (i.e. -ve forward velocity) -void ModePosHold::update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) +void ModePosHold::update_brake_angle_from_velocity(float &brake_angle, float velocity) { float lean_angle; - int16_t brake_rate = g.poshold_brake_rate; + float brake_rate = g.poshold_brake_rate; - brake_rate /= 4; - if (brake_rate <= 0) { - brake_rate = 1; + brake_rate /= 4.0f; + if (brake_rate <= 1.0f) { + brake_rate = 1.0f; } // calculate velocity-only based lean angle if (velocity >= 0) { - lean_angle = -brake_gain * velocity * (1.0f+500.0f/(velocity+60.0f)); + lean_angle = -brake_gain * velocity * (1.0f + 500.0f / (velocity + 60.0f)); } else { - lean_angle = -brake_gain * velocity * (1.0f+500.0f/(-velocity+60.0f)); + lean_angle = -brake_gain * velocity * (1.0f + 500.0f / (-velocity + 60.0f)); } // do not let lean_angle be too far from brake_angle - brake_angle = constrain_int16((int16_t)lean_angle, brake_angle - brake_rate, brake_angle + brake_rate); + brake_angle = constrain_float(lean_angle, brake_angle - brake_rate, brake_angle + brake_rate); // constrain final brake_angle - brake_angle = constrain_int16(brake_angle, -g.poshold_brake_angle_max, g.poshold_brake_angle_max); + brake_angle = constrain_float(brake_angle, -(float)g.poshold_brake_angle_max, (float)g.poshold_brake_angle_max); } // update_wind_comp_estimate - updates wind compensation estimate @@ -601,7 +601,7 @@ void ModePosHold::update_wind_comp_estimate() // get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles // should be called at the maximum loop rate -void ModePosHold::get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) +void ModePosHold::get_wind_comp_lean_angles(float &roll_angle, float &pitch_angle) { // reduce rate to 10hz wind_comp_timer++; @@ -611,8 +611,8 @@ void ModePosHold::get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_ wind_comp_timer = 0; // convert earth frame desired accelerations to body frame roll and pitch lean angles - roll_angle = atanf((-wind_comp_ef.x*ahrs.sin_yaw() + wind_comp_ef.y*ahrs.cos_yaw())/981)*(18000/M_PI); - pitch_angle = atanf(-(wind_comp_ef.x*ahrs.cos_yaw() + wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/M_PI); + roll_angle = atanf((-wind_comp_ef.x*ahrs.sin_yaw() + wind_comp_ef.y*ahrs.cos_yaw())/981.0f)*(18000.0f/M_PI); + pitch_angle = atanf(-(wind_comp_ef.x*ahrs.cos_yaw() + wind_comp_ef.y*ahrs.sin_yaw())/981.0f)*(18000.0f/M_PI); } // roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis @@ -621,7 +621,7 @@ void ModePosHold::roll_controller_to_pilot_override() roll_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE; controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; // initialise pilot_roll to 0, wind_comp will be updated to compensate and poshold_update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value - pilot_roll = 0; + pilot_roll = 0.0f; // store final controller output for mixing with pilot input controller_final_roll = roll; } @@ -632,7 +632,7 @@ void ModePosHold::pitch_controller_to_pilot_override() pitch_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE; controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; // initialise pilot_pitch to 0, wind_comp will be updated to compensate and update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value - pilot_pitch = 0; + pilot_pitch = 0.0f; // store final loiter outputs for mixing with pilot input controller_final_pitch = pitch; }