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

Copter: remove redundant poshold_ prefix on PosHold methods

This commit is contained in:
Peter Barker 2019-04-23 11:54:48 +10:00 committed by Randy Mackay
parent f57a9f11cc
commit e28c6b9dc6
2 changed files with 41 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/mode.h
View File

@ -901,13 +901,13 @@ protected:
private: private:
void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); void 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); int16_t 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 update_brake_angle_from_velocity(int16_t &brake_angle, float velocity);
void poshold_update_wind_comp_estimate(); void update_wind_comp_estimate();
void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle); void get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle);
void poshold_roll_controller_to_pilot_override(); void roll_controller_to_pilot_override();
void poshold_pitch_controller_to_pilot_override(); void pitch_controller_to_pilot_override();
// PosHold states // PosHold states
enum PosHoldModeState { enum PosHoldModeState {

68
ArduCopter/mode_poshold.cpp
View File

@ -196,7 +196,7 @@ void Copter::ModePosHold::run()
// If not in LOITER, retrieve latest wind compensation lean angles related to current yaw // If not in LOITER, retrieve latest wind compensation lean angles related to current yaw
if (roll_mode != POSHOLD_LOITER || pitch_mode != POSHOLD_LOITER) { 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 // Roll state machine
@ -209,7 +209,7 @@ void Copter::ModePosHold::run()
case POSHOLD_PILOT_OVERRIDE: case POSHOLD_PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input // 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 // 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 // switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_roll) && (fabsf(pilot_roll) < 2 * g.poshold_brake_rate)) { 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:
case POSHOLD_BRAKE_READY_TO_LOITER: case POSHOLD_BRAKE_READY_TO_LOITER:
// calculate brake_roll angle to counter-act velocity // 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 // update braking time estimate
if (!braking_time_updated_roll) { if (!braking_time_updated_roll) {
@ -263,7 +263,7 @@ void Copter::ModePosHold::run()
// check for pilot input // check for pilot input
if (!is_zero(target_roll)) { if (!is_zero(target_roll)) {
// init transition to pilot override // init transition to pilot override
poshold_roll_controller_to_pilot_override(); roll_controller_to_pilot_override();
} }
break; break;
@ -275,7 +275,7 @@ void Copter::ModePosHold::run()
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input // 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 // 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 // count-down loiter to pilot timer
if (controller_to_pilot_timer_roll > 0) { 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; 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 // 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; break;
} }
@ -303,7 +303,7 @@ void Copter::ModePosHold::run()
case POSHOLD_PILOT_OVERRIDE: case POSHOLD_PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input // 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 // 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 // switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_pitch) && (fabsf(pilot_pitch) < 2 * g.poshold_brake_rate)) { 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:
case POSHOLD_BRAKE_READY_TO_LOITER: case POSHOLD_BRAKE_READY_TO_LOITER:
// calculate brake_pitch angle to counter-act velocity // 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 // update braking time estimate
if (!braking_time_updated_pitch) { if (!braking_time_updated_pitch) {
@ -357,7 +357,7 @@ void Copter::ModePosHold::run()
// check for pilot input // check for pilot input
if (!is_zero(target_pitch)) { if (!is_zero(target_pitch)) {
// init transition to pilot override // init transition to pilot override
poshold_pitch_controller_to_pilot_override(); pitch_controller_to_pilot_override();
} }
break; break;
@ -369,7 +369,7 @@ void Copter::ModePosHold::run()
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input // 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 // 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 // count-down loiter to pilot timer
if (controller_to_pilot_timer_pitch > 0) { 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; 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 // 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; break;
} }
@ -424,22 +424,22 @@ void Copter::ModePosHold::run()
brake_to_loiter_mix = (float)brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER; 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 // calculate brake_roll and pitch angles to counter-act velocity
poshold_update_brake_angle_from_velocity(brake_roll, vel_right); 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_pitch, -vel_fw);
// run loiter controller // run loiter controller
loiter_nav->update(); loiter_nav->update();
// calculate final roll and pitch output by mixing loiter and brake controls // 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()); roll = 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()); pitch = mix_controls(brake_to_loiter_mix, brake_pitch + wind_comp_pitch, loiter_nav->get_pitch());
// check for pilot input // check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) { if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll // if roll input switch to pilot override for roll
if (!is_zero(target_roll)) { if (!is_zero(target_roll)) {
// init transition to pilot override // 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) // 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 // 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; 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 pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) { if (!is_zero(target_pitch)) {
// init transition to pilot override // init transition to pilot override
poshold_pitch_controller_to_pilot_override(); pitch_controller_to_pilot_override();
if (is_zero(target_roll)) { if (is_zero(target_roll)) {
// switch roll-mode to brake (but ready to go back to loiter anytime) // 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 // 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(); pitch = loiter_nav->get_pitch();
// update wind compensation estimate // update wind compensation estimate
poshold_update_wind_comp_estimate(); update_wind_comp_estimate();
// check for pilot input // check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) { if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll // if roll input switch to pilot override for roll
if (!is_zero(target_roll)) { if (!is_zero(target_roll)) {
// init transition to pilot override // 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) // switch pitch-mode to brake (but ready to go back to loiter anytime)
pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; 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 // 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 pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) { if (!is_zero(target_pitch)) {
// init transition to pilot override // 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 roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time)
if (is_zero(target_roll)) { if (is_zero(target_roll)) {
roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; 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 // 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 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)) { 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 // 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); mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f);
return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control)); 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 // 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) // 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; float lean_angle;
int16_t brake_rate = g.poshold_brake_rate; 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); 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 // 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 // check wind estimate start has not been delayed
if (wind_comp_start_timer > 0) { 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 // 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 // reduce rate to 10hz
wind_comp_timer++; 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); 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 // 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() void Copter::ModePosHold::roll_controller_to_pilot_override()
{ {
roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE;
controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; 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; 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 // 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() void Copter::ModePosHold::pitch_controller_to_pilot_override()
{ {
pitch_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; pitch_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE;
controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; 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; pilot_pitch = 0;
// store final loiter outputs for mixing with pilot input // store final loiter outputs for mixing with pilot input
controller_final_pitch = pitch; controller_final_pitch = pitch;