zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: PosHold fix for low brake_rate

mission-4.1.18
Leonard Hall 6 years ago committed by Randy Mackay
parent
93ca243987
commit
0e34d8b1ff
2 changed files with 47 additions and 47 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 26
      ArduCopter/mode.h
  2. 68
      ArduCopter/mode_poshold.cpp

26
ArduCopter/mode.h
Unescape 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
Unescape 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;
} }

Write Preview
Loading…
Cancel
Save