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

Copter: move brake state into a structure

This commit is contained in:
Peter Barker 2019-04-09 10:58:11 +10:00 committed by Randy Mackay
parent c350268918
commit e7a9e0acb4
2 changed files with 65 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduCopter/mode.h
View File

@ -1040,22 +1040,24 @@ private:
RPMode roll_mode; RPMode roll_mode;
RPMode pitch_mode; RPMode pitch_mode;
uint8_t braking_time_updated_roll : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
uint8_t braking_time_updated_pitch : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
// pilot input related variables // pilot input related variables
float pilot_roll; // pilot requested roll angle (filtered to slow returns to zero) float pilot_roll; // pilot requested roll angle (filtered to slow returns to zero)
float pilot_pitch; // pilot requested roll angle (filtered to slow returns to zero) float pilot_pitch; // pilot requested roll angle (filtered to slow returns to zero)
// 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) struct {
float brake_roll; // target roll angle during braking periods uint8_t time_updated_roll : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
float brake_pitch; // target pitch angle during braking periods uint8_t time_updated_pitch : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after 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 float gain; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from rate)
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 roll; // target roll angle during braking periods
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 float pitch; // target pitch angle during braking periods
int16_t brake_to_loiter_timer; // cycles to mix brake and loiter controls in POSHOLD_BRAKE_TO_LOITER int16_t timeout_roll; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
int16_t timeout_pitch; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
float 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 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 to_loiter_timer; // cycles to mix brake and loiter controls in POSHOLD_TO_LOITER
} brake;
// 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

104
ArduCopter/mode_poshold.cpp
View File

@ -42,7 +42,7 @@ bool ModePosHold::init(bool ignore_checks)
pilot_pitch = 0.0f; 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;
if (copter.ap.land_complete) { if (copter.ap.land_complete) {
// if landed begin in loiter mode // if landed begin in loiter mode
@ -69,7 +69,6 @@ bool ModePosHold::init(bool ignore_checks)
void ModePosHold::run() void ModePosHold::run()
{ {
float takeoff_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f;
float brake_to_loiter_mix; // mix of brake and loiter controls. 0 = fully brake controls, 1 = fully loiter controls
float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
const Vector3f& vel = inertial_nav.get_velocity(); const Vector3f& vel = inertial_nav.get_velocity();
@ -206,10 +205,10 @@ 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.0f; // initialise braking angle to zero 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.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 brake.time_updated_roll = false; // flag the braking time can be re-estimated
} }
// final lean angle should be pilot input plus wind compensation // final lean angle should be pilot input plus wind compensation
@ -218,29 +217,29 @@ void ModePosHold::run()
case RPMode::BRAKE: case RPMode::BRAKE:
case RPMode::BRAKE_READY_TO_LOITER: case RPMode::BRAKE_READY_TO_LOITER:
// calculate brake_roll angle to counter-act velocity // calculate brake.roll angle to counter-act velocity
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 (!brake.time_updated_roll) {
// check if brake angle is increasing // check if brake angle is increasing
if (fabsf(brake_roll) >= brake_angle_max_roll) { if (fabsf(brake.roll) >= brake.angle_max_roll) {
brake_angle_max_roll = fabsf(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)(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. 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; brake.time_updated_roll = true;
} }
} }
// if velocity is very low reduce braking time to 0.5seconds // if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (brake_timeout_roll > 50*LOOP_RATE_FACTOR)) { if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (brake.timeout_roll > 50*LOOP_RATE_FACTOR)) {
brake_timeout_roll = 50*LOOP_RATE_FACTOR; brake.timeout_roll = 50*LOOP_RATE_FACTOR;
} }
// reduce braking timer // reduce braking timer
if (brake_timeout_roll > 0) { if (brake.timeout_roll > 0) {
brake_timeout_roll--; brake.timeout_roll--;
} else { } else {
// indicate that we are ready to move to Loiter. // indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER // Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER
@ -249,7 +248,7 @@ void ModePosHold::run()
} }
// final lean angle is braking angle + wind compensation angle // final lean angle is braking angle + wind compensation angle
roll = brake_roll + wind_comp_roll; roll = brake.roll + wind_comp_roll;
// check for pilot input // check for pilot input
if (!is_zero(target_roll)) { if (!is_zero(target_roll)) {
@ -300,10 +299,10 @@ 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.0f; // initialise braking angle to zero 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.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 brake.time_updated_pitch = false; // flag the braking time can be re-estimated
} }
// final lean angle should be pilot input plus wind compensation // final lean angle should be pilot input plus wind compensation
@ -313,28 +312,28 @@ void ModePosHold::run()
case RPMode::BRAKE: case RPMode::BRAKE:
case RPMode::BRAKE_READY_TO_LOITER: case RPMode::BRAKE_READY_TO_LOITER:
// calculate brake_pitch angle to counter-act velocity // calculate brake_pitch angle to counter-act velocity
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 (!brake.time_updated_pitch) {
// check if brake angle is increasing // check if brake angle is increasing
if (fabsf(brake_pitch) >= brake_angle_max_pitch) { if (fabsf(brake.pitch) >= brake.angle_max_pitch) {
brake_angle_max_pitch = fabsf(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)(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. 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; brake.time_updated_pitch = true;
} }
} }
// if velocity is very low reduce braking time to 0.5seconds // if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) { if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (brake.timeout_pitch > 50*LOOP_RATE_FACTOR)) {
brake_timeout_pitch = 50*LOOP_RATE_FACTOR; brake.timeout_pitch = 50*LOOP_RATE_FACTOR;
} }
// reduce braking timer // reduce braking timer
if (brake_timeout_pitch > 0) { if (brake.timeout_pitch > 0) {
brake_timeout_pitch--; brake.timeout_pitch--;
} else { } else {
// indicate that we are ready to move to Loiter. // indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER // Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to RPMode::BRAKE_READY_TO_LOITER
@ -343,7 +342,7 @@ void ModePosHold::run()
} }
// final lean angle is braking angle + wind compensation angle // final lean angle is braking angle + wind compensation angle
pitch = brake_pitch + wind_comp_pitch; pitch = brake.pitch + wind_comp_pitch;
// check for pilot input // check for pilot input
if (!is_zero(target_pitch)) { if (!is_zero(target_pitch)) {
@ -386,7 +385,7 @@ void ModePosHold::run()
if (roll_mode == RPMode::BRAKE_READY_TO_LOITER && pitch_mode == RPMode::BRAKE_READY_TO_LOITER) { if (roll_mode == RPMode::BRAKE_READY_TO_LOITER && pitch_mode == RPMode::BRAKE_READY_TO_LOITER) {
roll_mode = RPMode::BRAKE_TO_LOITER; roll_mode = RPMode::BRAKE_TO_LOITER;
pitch_mode = RPMode::BRAKE_TO_LOITER; pitch_mode = RPMode::BRAKE_TO_LOITER;
brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER; brake.to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
// init loiter controller // init loiter controller
loiter_nav->init_target(inertial_nav.get_position()); loiter_nav->init_target(inertial_nav.get_position());
// set delay to start of wind compensation estimate updates // set delay to start of wind compensation estimate updates
@ -401,29 +400,30 @@ void ModePosHold::run()
// handle combined roll+pitch mode // handle combined roll+pitch mode
switch (roll_mode) { switch (roll_mode) {
case RPMode::BRAKE_TO_LOITER: case RPMode::BRAKE_TO_LOITER: {
// reduce brake_to_loiter timer // reduce brake_to_loiter timer
if (brake_to_loiter_timer > 0) { if (brake.to_loiter_timer > 0) {
brake_to_loiter_timer--; brake.to_loiter_timer--;
} else { } else {
// progress to full loiter on next iteration // progress to full loiter on next iteration
roll_mode = RPMode::LOITER; roll_mode = RPMode::LOITER;
pitch_mode = RPMode::LOITER; pitch_mode = RPMode::LOITER;
} }
// calculate percentage mix of loiter and brake control // mix of brake and loiter controls. 0 = fully brake
brake_to_loiter_mix = (float)brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER; // controls, 1 = fully loiter controls
const float 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
update_brake_angle_from_velocity(brake_roll, vel_right); update_brake_angle_from_velocity(brake.roll, vel_right);
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(false); loiter_nav->update(false);
// 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 = 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 = 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)) {
@ -432,7 +432,7 @@ void ModePosHold::run()
// init transition to pilot override // init transition to pilot override
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 = RPMode::BRAKE_READY_TO_LOITER; pitch_mode = RPMode::BRAKE_READY_TO_LOITER;
} }
// if pitch input switch to pilot override for pitch // if pitch input switch to pilot override for pitch
@ -441,13 +441,13 @@ void ModePosHold::run()
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
roll_mode = RPMode::BRAKE_READY_TO_LOITER; roll_mode = RPMode::BRAKE_READY_TO_LOITER;
} }
} }
} }
break; break;
}
case RPMode::LOITER: case RPMode::LOITER:
// run loiter controller // run loiter controller
loiter_nav->update(false); loiter_nav->update(false);
@ -467,8 +467,8 @@ void ModePosHold::run()
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 = 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.0f; 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)) {
@ -477,7 +477,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.0f; 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)
} }
@ -546,9 +546,9 @@ void ModePosHold::update_brake_angle_from_velocity(float &brake_angle, float vel
// 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