mirror of https://github.com/ArduPilot/ardupilot
Copter: move brake state into a structure
This commit is contained in:
parent
c350268918
commit
e7a9e0acb4
|
@ -1040,22 +1040,24 @@ private:
|
|||
RPMode roll_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
|
||||
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)
|
||||
|
||||
// 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_roll; // target roll 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_pitch; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
|
||||
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 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
|
||||
struct {
|
||||
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
|
||||
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
|
||||
|
||||
float gain; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from rate)
|
||||
float roll; // target roll angle during braking periods
|
||||
float pitch; // target pitch angle during braking periods
|
||||
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
|
||||
int16_t controller_to_pilot_timer_roll; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
|
||||
|
|
|
@ -42,7 +42,7 @@ bool ModePosHold::init(bool ignore_checks)
|
|||
pilot_pitch = 0.0f;
|
||||
|
||||
// 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 landed begin in loiter mode
|
||||
|
@ -69,7 +69,6 @@ bool ModePosHold::init(bool ignore_checks)
|
|||
void ModePosHold::run()
|
||||
{
|
||||
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_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
|
||||
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)) {
|
||||
// initialise BRAKE mode
|
||||
roll_mode = RPMode::BRAKE; // Set brake roll mode
|
||||
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_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.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.timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
|
||||
brake.time_updated_roll = false; // flag the braking time can be re-estimated
|
||||
}
|
||||
|
||||
// final lean angle should be pilot input plus wind compensation
|
||||
|
@ -218,29 +217,29 @@ void ModePosHold::run()
|
|||
|
||||
case RPMode::BRAKE:
|
||||
case RPMode::BRAKE_READY_TO_LOITER:
|
||||
// calculate brake_roll angle to counter-act velocity
|
||||
update_brake_angle_from_velocity(brake_roll, vel_right);
|
||||
// calculate brake.roll angle to counter-act velocity
|
||||
update_brake_angle_from_velocity(brake.roll, vel_right);
|
||||
|
||||
// update braking time estimate
|
||||
if (!braking_time_updated_roll) {
|
||||
if (!brake.time_updated_roll) {
|
||||
// check if brake angle is increasing
|
||||
if (fabsf(brake_roll) >= brake_angle_max_roll) {
|
||||
brake_angle_max_roll = fabsf(brake_roll);
|
||||
if (fabsf(brake.roll) >= brake.angle_max_roll) {
|
||||
brake.angle_max_roll = fabsf(brake.roll);
|
||||
} else {
|
||||
// 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.
|
||||
braking_time_updated_roll = true;
|
||||
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.time_updated_roll = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 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)) {
|
||||
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;
|
||||
}
|
||||
|
||||
// reduce braking timer
|
||||
if (brake_timeout_roll > 0) {
|
||||
brake_timeout_roll--;
|
||||
if (brake.timeout_roll > 0) {
|
||||
brake.timeout_roll--;
|
||||
} else {
|
||||
// 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
|
||||
|
@ -249,7 +248,7 @@ void ModePosHold::run()
|
|||
}
|
||||
|
||||
// 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
|
||||
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)) {
|
||||
// initialise BRAKE mode
|
||||
pitch_mode = RPMode::BRAKE; // set brake pitch mode
|
||||
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_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.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.timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
|
||||
brake.time_updated_pitch = false; // flag the braking time can be re-estimated
|
||||
}
|
||||
|
||||
// final lean angle should be pilot input plus wind compensation
|
||||
|
@ -313,28 +312,28 @@ void ModePosHold::run()
|
|||
case RPMode::BRAKE:
|
||||
case RPMode::BRAKE_READY_TO_LOITER:
|
||||
// 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
|
||||
if (!braking_time_updated_pitch) {
|
||||
if (!brake.time_updated_pitch) {
|
||||
// check if brake angle is increasing
|
||||
if (fabsf(brake_pitch) >= brake_angle_max_pitch) {
|
||||
brake_angle_max_pitch = fabsf(brake_pitch);
|
||||
if (fabsf(brake.pitch) >= brake.angle_max_pitch) {
|
||||
brake.angle_max_pitch = fabsf(brake.pitch);
|
||||
} else {
|
||||
// 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.
|
||||
braking_time_updated_pitch = true;
|
||||
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.time_updated_pitch = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 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)) {
|
||||
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;
|
||||
}
|
||||
|
||||
// reduce braking timer
|
||||
if (brake_timeout_pitch > 0) {
|
||||
brake_timeout_pitch--;
|
||||
if (brake.timeout_pitch > 0) {
|
||||
brake.timeout_pitch--;
|
||||
} else {
|
||||
// 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
|
||||
|
@ -343,7 +342,7 @@ void ModePosHold::run()
|
|||
}
|
||||
|
||||
// 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
|
||||
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) {
|
||||
roll_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
|
||||
loiter_nav->init_target(inertial_nav.get_position());
|
||||
// set delay to start of wind compensation estimate updates
|
||||
|
@ -401,29 +400,30 @@ void ModePosHold::run()
|
|||
|
||||
// handle combined roll+pitch mode
|
||||
switch (roll_mode) {
|
||||
case RPMode::BRAKE_TO_LOITER:
|
||||
case RPMode::BRAKE_TO_LOITER: {
|
||||
// reduce brake_to_loiter timer
|
||||
if (brake_to_loiter_timer > 0) {
|
||||
brake_to_loiter_timer--;
|
||||
if (brake.to_loiter_timer > 0) {
|
||||
brake.to_loiter_timer--;
|
||||
} else {
|
||||
// progress to full loiter on next iteration
|
||||
roll_mode = RPMode::LOITER;
|
||||
pitch_mode = RPMode::LOITER;
|
||||
}
|
||||
|
||||
// calculate percentage mix of loiter and brake control
|
||||
brake_to_loiter_mix = (float)brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER;
|
||||
// mix of brake and loiter controls. 0 = fully brake
|
||||
// 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
|
||||
update_brake_angle_from_velocity(brake_roll, vel_right);
|
||||
update_brake_angle_from_velocity(brake_pitch, -vel_fw);
|
||||
// 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.pitch, -vel_fw);
|
||||
|
||||
// run loiter controller
|
||||
loiter_nav->update(false);
|
||||
|
||||
// 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());
|
||||
pitch = 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)) {
|
||||
|
@ -432,7 +432,7 @@ void ModePosHold::run()
|
|||
// init transition 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
|
||||
// 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;
|
||||
}
|
||||
// if pitch input switch to pilot override for pitch
|
||||
|
@ -441,13 +441,13 @@ void ModePosHold::run()
|
|||
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
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
case RPMode::LOITER:
|
||||
// run loiter controller
|
||||
loiter_nav->update(false);
|
||||
|
@ -467,8 +467,8 @@ void ModePosHold::run()
|
|||
roll_controller_to_pilot_override();
|
||||
// switch pitch-mode to brake (but ready to go back to loiter anytime)
|
||||
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
|
||||
brake_pitch = 0.0f;
|
||||
// reset brake.pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
|
||||
brake.pitch = 0.0f;
|
||||
}
|
||||
// if pitch input switch to pilot override for 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 (is_zero(target_roll)) {
|
||||
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)
|
||||
}
|
||||
|
@ -546,9 +546,9 @@ void ModePosHold::update_brake_angle_from_velocity(float &brake_angle, float vel
|
|||
|
||||
// calculate velocity-only based lean angle
|
||||
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 {
|
||||
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
|
||||
|
|
Loading…
Reference in New Issue