From e28c6b9dc6b6b006aee9871f12faa5aa40ec8ad8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Apr 2019 11:54:48 +1000 Subject: [PATCH] Copter: remove redundant poshold_ prefix on PosHold methods --- ArduCopter/mode.h | 14 ++++---- ArduCopter/mode_poshold.cpp | 68 ++++++++++++++++++------------------- 2 files changed, 41 insertions(+), 41 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 67f0255219..c0bdaab341 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -901,13 +901,13 @@ protected: private: - void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); - int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control); - void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity); - void poshold_update_wind_comp_estimate(); - void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle); - void poshold_roll_controller_to_pilot_override(); - void poshold_pitch_controller_to_pilot_override(); + 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); + void update_wind_comp_estimate(); + void get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle); + void roll_controller_to_pilot_override(); + void pitch_controller_to_pilot_override(); // PosHold states enum PosHoldModeState { diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index d521ea9bed..65fce211c1 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -196,7 +196,7 @@ void Copter::ModePosHold::run() // If not in LOITER, retrieve latest wind compensation lean angles related to current yaw if (roll_mode != POSHOLD_LOITER || pitch_mode != POSHOLD_LOITER) { - poshold_get_wind_comp_lean_angles(wind_comp_roll, wind_comp_pitch); + get_wind_comp_lean_angles(wind_comp_roll, wind_comp_pitch); } // Roll state machine @@ -209,7 +209,7 @@ void Copter::ModePosHold::run() case POSHOLD_PILOT_OVERRIDE: // update pilot desired roll angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(pilot_roll, target_roll); + update_pilot_lean_angle(pilot_roll, target_roll); // switch to BRAKE mode for next iteration if no pilot input if (is_zero(target_roll) && (fabsf(pilot_roll) < 2 * g.poshold_brake_rate)) { @@ -228,7 +228,7 @@ void Copter::ModePosHold::run() case POSHOLD_BRAKE: case POSHOLD_BRAKE_READY_TO_LOITER: // calculate brake_roll angle to counter-act velocity - poshold_update_brake_angle_from_velocity(brake_roll, vel_right); + update_brake_angle_from_velocity(brake_roll, vel_right); // update braking time estimate if (!braking_time_updated_roll) { @@ -263,7 +263,7 @@ void Copter::ModePosHold::run() // check for pilot input if (!is_zero(target_roll)) { // init transition to pilot override - poshold_roll_controller_to_pilot_override(); + roll_controller_to_pilot_override(); } break; @@ -275,7 +275,7 @@ void Copter::ModePosHold::run() case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: // update pilot desired roll angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(pilot_roll, target_roll); + update_pilot_lean_angle(pilot_roll, target_roll); // count-down loiter to pilot timer if (controller_to_pilot_timer_roll > 0) { @@ -289,7 +289,7 @@ void Copter::ModePosHold::run() controller_to_pilot_roll_mix = (float)controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; // mix final loiter lean angle and pilot desired lean angles - roll = poshold_mix_controls(controller_to_pilot_roll_mix, controller_final_roll, pilot_roll + wind_comp_roll); + roll = mix_controls(controller_to_pilot_roll_mix, controller_final_roll, pilot_roll + wind_comp_roll); break; } @@ -303,7 +303,7 @@ void Copter::ModePosHold::run() case POSHOLD_PILOT_OVERRIDE: // update pilot desired pitch angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(pilot_pitch, target_pitch); + update_pilot_lean_angle(pilot_pitch, target_pitch); // switch to BRAKE mode for next iteration if no pilot input if (is_zero(target_pitch) && (fabsf(pilot_pitch) < 2 * g.poshold_brake_rate)) { @@ -322,7 +322,7 @@ void Copter::ModePosHold::run() case POSHOLD_BRAKE: case POSHOLD_BRAKE_READY_TO_LOITER: // calculate brake_pitch angle to counter-act velocity - poshold_update_brake_angle_from_velocity(brake_pitch, -vel_fw); + update_brake_angle_from_velocity(brake_pitch, -vel_fw); // update braking time estimate if (!braking_time_updated_pitch) { @@ -357,7 +357,7 @@ void Copter::ModePosHold::run() // check for pilot input if (!is_zero(target_pitch)) { // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); + pitch_controller_to_pilot_override(); } break; @@ -369,7 +369,7 @@ void Copter::ModePosHold::run() case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: // update pilot desired pitch angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(pilot_pitch, target_pitch); + update_pilot_lean_angle(pilot_pitch, target_pitch); // count-down loiter to pilot timer if (controller_to_pilot_timer_pitch > 0) { @@ -383,7 +383,7 @@ void Copter::ModePosHold::run() controller_to_pilot_pitch_mix = (float)controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; // mix final loiter lean angle and pilot desired lean angles - pitch = poshold_mix_controls(controller_to_pilot_pitch_mix, controller_final_pitch, pilot_pitch + wind_comp_pitch); + pitch = mix_controls(controller_to_pilot_pitch_mix, controller_final_pitch, pilot_pitch + wind_comp_pitch); break; } @@ -424,22 +424,22 @@ void Copter::ModePosHold::run() brake_to_loiter_mix = (float)brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER; // calculate brake_roll and pitch angles to counter-act velocity - poshold_update_brake_angle_from_velocity(brake_roll, vel_right); - poshold_update_brake_angle_from_velocity(brake_pitch, -vel_fw); + update_brake_angle_from_velocity(brake_roll, vel_right); + update_brake_angle_from_velocity(brake_pitch, -vel_fw); // run loiter controller loiter_nav->update(); // calculate final roll and pitch output by mixing loiter and brake controls - roll = poshold_mix_controls(brake_to_loiter_mix, brake_roll + wind_comp_roll, loiter_nav->get_roll()); - pitch = poshold_mix_controls(brake_to_loiter_mix, brake_pitch + wind_comp_pitch, loiter_nav->get_pitch()); + roll = mix_controls(brake_to_loiter_mix, brake_roll + wind_comp_roll, loiter_nav->get_roll()); + pitch = mix_controls(brake_to_loiter_mix, brake_pitch + wind_comp_pitch, loiter_nav->get_pitch()); // check for pilot input if (!is_zero(target_roll) || !is_zero(target_pitch)) { // if roll input switch to pilot override for roll if (!is_zero(target_roll)) { // init transition to pilot override - poshold_roll_controller_to_pilot_override(); + roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) // no need to reset brake_pitch here as wind comp has not been updated since last brake_pitch computation pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; @@ -447,7 +447,7 @@ void Copter::ModePosHold::run() // if pitch input switch to pilot override for pitch if (!is_zero(target_pitch)) { // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); + pitch_controller_to_pilot_override(); if (is_zero(target_roll)) { // switch roll-mode to brake (but ready to go back to loiter anytime) // no need to reset brake_roll here as wind comp has not been updated since last brake_roll computation @@ -466,14 +466,14 @@ void Copter::ModePosHold::run() pitch = loiter_nav->get_pitch(); // update wind compensation estimate - poshold_update_wind_comp_estimate(); + update_wind_comp_estimate(); // check for pilot input if (!is_zero(target_roll) || !is_zero(target_pitch)) { // if roll input switch to pilot override for roll if (!is_zero(target_roll)) { // init transition to pilot override - poshold_roll_controller_to_pilot_override(); + roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; // reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle @@ -482,7 +482,7 @@ void Copter::ModePosHold::run() // if pitch input switch to pilot override for pitch if (!is_zero(target_pitch)) { // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); + 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 (is_zero(target_roll)) { roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; @@ -512,7 +512,7 @@ void Copter::ModePosHold::run() } // poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received -void Copter::ModePosHold::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) +void Copter::ModePosHold::update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) { // if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) { @@ -532,18 +532,18 @@ void Copter::ModePosHold::poshold_update_pilot_lean_angle(float &lean_angle_filt } } -// poshold_mix_controls - mixes two controls based on the mix_ratio +// 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 Copter::ModePosHold::poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) +int16_t Copter::ModePosHold::mix_controls(float mix_ratio, int16_t first_control, int16_t 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)); } -// poshold_update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain +// 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 Copter::ModePosHold::poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) +void Copter::ModePosHold::update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) { float lean_angle; int16_t brake_rate = g.poshold_brake_rate; @@ -567,9 +567,9 @@ void Copter::ModePosHold::poshold_update_brake_angle_from_velocity(int16_t &brak brake_angle = constrain_int16(brake_angle, -g.poshold_brake_angle_max, g.poshold_brake_angle_max); } -// poshold_update_wind_comp_estimate - updates wind compensation estimate +// update_wind_comp_estimate - updates wind compensation estimate // should be called at the maximum loop rate when loiter is engaged -void Copter::ModePosHold::poshold_update_wind_comp_estimate() +void Copter::ModePosHold::update_wind_comp_estimate() { // check wind estimate start has not been delayed if (wind_comp_start_timer > 0) { @@ -603,9 +603,9 @@ void Copter::ModePosHold::poshold_update_wind_comp_estimate() } } -// poshold_get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles +// 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 Copter::ModePosHold::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) +void Copter::ModePosHold::get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) { // reduce rate to 10hz wind_comp_timer++; @@ -619,8 +619,8 @@ void Copter::ModePosHold::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, pitch_angle = atanf(-(wind_comp_ef.x*ahrs.cos_yaw() + wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/M_PI); } -// poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::ModePosHold::poshold_roll_controller_to_pilot_override() +// roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis +void Copter::ModePosHold::roll_controller_to_pilot_override() { roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; @@ -630,12 +630,12 @@ void Copter::ModePosHold::poshold_roll_controller_to_pilot_override() controller_final_roll = roll; } -// poshold_pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::ModePosHold::poshold_pitch_controller_to_pilot_override() +// pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis +void Copter::ModePosHold::pitch_controller_to_pilot_override() { pitch_mode = POSHOLD_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 poshold_update_pilot_lean_angle function shall not smooth this transition at next iteration. so 0 is the right value + // 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; // store final loiter outputs for mixing with pilot input controller_final_pitch = pitch;