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

Copter: PosHold fix for low brake_rate

This commit is contained in:
Leonard Hall 2019-07-07 22:43:43 +09:30 committed by Randy Mackay
parent 93ca243987
commit 0e34d8b1ff
2 changed files with 47 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduCopter/mode.h
View File

@ -905,10 +905,10 @@ protected:
private: private:
void 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 mix_controls(float mix_ratio, int16_t first_control, int16_t second_control); float mix_controls(float mix_ratio, float first_control, float second_control);
void update_brake_angle_from_velocity(int16_t &brake_angle, float velocity); void update_brake_angle_from_velocity(float &brake_angle, float velocity);
void update_wind_comp_estimate(); 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 roll_controller_to_pilot_override();
void pitch_controller_to_pilot_override(); void pitch_controller_to_pilot_override();
@ -933,30 +933,30 @@ private:
// braking related variables // braking related variables
float brake_gain; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from brake_rate) 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 float brake_roll; // target roll angle during braking periods
int16_t brake_pitch; // target pitch 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_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_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 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
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_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 int16_t brake_to_loiter_timer; // cycles to mix brake and loiter controls in POSHOLD_BRAKE_TO_LOITER
// loiter related variables // 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_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_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) float 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_pitch; // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
// wind compensation related variables // wind compensation related variables
Vector2f wind_comp_ef; // wind compensation in earth frame, filtered lean angles from position controller 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 float wind_comp_roll; // roll angle to compensate for wind
int16_t wind_comp_pitch; // pitch 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 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 int8_t wind_comp_timer; // counter to reduce wind comp roll/pitch lean angle calcs to 10hz
// final output // final output
int16_t roll; // final roll angle sent to attitude controller float roll; // final roll angle sent to attitude controller
int16_t pitch; // final pitch angle sent to attitude controller float pitch; // final pitch angle sent to attitude controller
}; };

68
ArduCopter/mode_poshold.cpp
View File

@ -37,8 +37,8 @@ bool ModePosHold::init(bool ignore_checks)
} }
// initialise lean angles to current attitude // initialise lean angles to current attitude
pilot_roll = 0; pilot_roll = 0.0f;
pilot_pitch = 0; pilot_pitch = 0.0f;
// compute brake_gain // compute brake_gain
brake_gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f; 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 // initialise wind_comp each time PosHold is switched on
wind_comp_ef.zero(); wind_comp_ef.zero();
wind_comp_roll = 0;
wind_comp_pitch = 0;
wind_comp_timer = 0; wind_comp_timer = 0;
wind_comp_roll = 0.0f;
wind_comp_pitch = 0.0f;
return true; return true;
} }
@ -211,8 +211,8 @@ void ModePosHold::run()
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)) {
// initialise BRAKE mode // initialise BRAKE mode
roll_mode = RPMode::BRAKE; // Set brake roll mode roll_mode = RPMode::BRAKE; // Set brake roll mode
brake_roll = 0; // initialise braking angle to zero brake_roll = 0.0f; // 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_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. 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 braking_time_updated_roll = false; // flag the braking time can be re-estimated
} }
@ -229,11 +229,11 @@ void ModePosHold::run()
// update braking time estimate // update braking time estimate
if (!braking_time_updated_roll) { if (!braking_time_updated_roll) {
// check if brake angle is increasing // check if brake angle is increasing
if (abs(brake_roll) >= brake_angle_max_roll) { if (fabsf(brake_roll) >= brake_angle_max_roll) {
brake_angle_max_roll = abs(brake_roll); brake_angle_max_roll = fabsf(brake_roll);
} else { } else {
// braking angle has started decreasing so re-estimate braking time // 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; 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)) { if (is_zero(target_pitch) && (fabsf(pilot_pitch) < 2 * g.poshold_brake_rate)) {
// initialise BRAKE mode // initialise BRAKE mode
pitch_mode = RPMode::BRAKE; // set brake pitch mode pitch_mode = RPMode::BRAKE; // set brake pitch mode
brake_pitch = 0; // initialise braking angle to zero brake_pitch = 0.0f; // 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_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. 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 braking_time_updated_pitch = false; // flag the braking time can be re-estimated
} }
@ -323,11 +323,11 @@ void ModePosHold::run()
// update braking time estimate // update braking time estimate
if (!braking_time_updated_pitch) { if (!braking_time_updated_pitch) {
// check if brake angle is increasing // check if brake angle is increasing
if (abs(brake_pitch) >= brake_angle_max_pitch) { if (fabsf(brake_pitch) >= brake_angle_max_pitch) {
brake_angle_max_pitch = abs(brake_pitch); brake_angle_max_pitch = fabsf(brake_pitch);
} else { } else {
// braking angle has started decreasing so re-estimate braking time // 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; 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) // switch pitch-mode to brake (but ready to go back to loiter anytime)
pitch_mode = RPMode::BRAKE_READY_TO_LOITER; 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 // 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 pitch input switch to pilot override for pitch
if (!is_zero(target_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 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 = RPMode::BRAKE_READY_TO_LOITER; 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) // 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 // constrain target pitch/roll angles
float angle_max = copter.aparm.angle_max; float angle_max = copter.aparm.angle_max;
roll = constrain_int16(roll, -angle_max, angle_max); roll = constrain_float(roll, -angle_max, angle_max);
pitch = constrain_int16(pitch, -angle_max, angle_max); pitch = constrain_float(pitch, -angle_max, angle_max);
// call attitude controller // call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll, pitch, target_yaw_rate); 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_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 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); 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 // 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 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; float lean_angle;
int16_t brake_rate = g.poshold_brake_rate; float brake_rate = g.poshold_brake_rate;
brake_rate /= 4; brake_rate /= 4.0f;
if (brake_rate <= 0) { if (brake_rate <= 1.0f) {
brake_rate = 1; brake_rate = 1.0f;
} }
// calculate velocity-only based lean angle // calculate velocity-only based lean angle
if (velocity >= 0) { 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 { } 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 // 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 // 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 // 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 // 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 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 // reduce rate to 10hz
wind_comp_timer++; 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; wind_comp_timer = 0;
// convert earth frame desired accelerations to body frame roll and pitch lean angles // 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); 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)*(18000/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 // 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; roll_mode = RPMode::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;
// 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 // 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 // store final controller output for mixing with pilot input
controller_final_roll = roll; controller_final_roll = roll;
} }
@ -632,7 +632,7 @@ void ModePosHold::pitch_controller_to_pilot_override()
pitch_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE; pitch_mode = RPMode::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 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.0f;
// 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;
} }