* hybrid tries to improve upon regular loiter by mixing the pilot input with the loiter controller
*/
#define HYBRID_SPEED_0 10 //
#define HYBRID_SPEED_0 10 // speed below which it is always safe to switch to loiter
#define HYBRID_LOITER_STAB_TIMER 300 // Must be higher than HYBRID_BRAKE_LOITER_MIX_TIMER (twice is a good deal) set it from 100 to 500, the number of centiseconds between loiter engage and getting wind_comp (once loiter stabilized)
#define HYBRID_BRAKE_LOITER_MIX_TIMER 150 // Number of cycles to transition from brake mode to loiter mode. Must be lower than HYBRID_LOITER_STAB_TIMER
#define HYBRID_LOITER_MAN_MIX_TIMER 50 // Set it from 100 to 200, the number of centiseconds loiter and manual commands are mixed to make a smooth transition.
#define HYBRID_SMOOTH_RATE_FACTOR 0.04f // controls the smoothness of the transition from ?? to ??. 0.04 = longer smoother transitions, 0.07 means faster transitions
#define HYBRID_BRAKE_TIME_ESTIMATE_MAX 600 // max number of cycles the brake will be applied before we switch to loiter
#define HYBRID_BRAKE_TO_LOITER_TIMER 150 // Number of cycles to transition from brake mode to loiter mode. Must be lower than HYBRID_LOITER_STAB_TIMER
#define HYBRID_LOITER_TO_PILOT_MIX_TIMER 50 // Set it from 100 to 200, the number of centiseconds loiter and manual commands are mixed to make a smooth transition.
#define HYBRID_SMOOTH_RATE_FACTOR 0.04f // filter applied to pilot's roll/pitch input as it returns to center. A lower number will cause the roll/pitch to return to zero more slowly if the brake_rate is also low.
#define HYBRID_STICK_RELEASE_SMOOTH_ANGLE 1800 // max angle required (in centi-degrees) after which the smooth stick release effect is applied
#define HYBRID_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s
HYBRID_PILOT_OVERRIDE=0, // pilot is controlling this axis (i.e. roll or pitch)
HYBRID_BRAKE, // this axis is braking towards zero
HYBRID_BRAKE_READY_TO_LOITER, // this axis has completed braking and is ready to enter loiter mode (both modes must be this value before moving to next stage)
HYBRID_BRAKE_TO_LOITER, // both vehicle's axis (roll and pitch) are transitioning from braking to loiter mode (braking and loiter controls are mixed)
HYBRID_LOITER, // both vehicle axis are holding position
HYBRID_LOITER_TO_PILOT_OVERRIDE // pilot has input controls on this axis and this axis is transitioning to pilot override (other axis will transition to brake if no pilot input)
};
static struct {
hybrid_rp_mode roll_mode : 2; // roll mode: pilot override, brake or loiter
hybrid_rp_mode pitch_mode : 2; // pitch mode: pilot override, brake or loiter
uint8_t loiter_engaged : 1; // 1 if loiter target has been set and loiter controller is running
hybrid_rp_mode roll_mode : 3; // roll mode: pilot override, brake or loiter
hybrid_rp_mode pitch_mode : 3; // pitch mode: pilot override, brake or loiter
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
int16_t pilot_roll; // pilot requested roll angle (filtered to slow returns to zero)
int16_t 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)
int16_t brake_roll; // target roll angle during braking periods
int16_t 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
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
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
int16_t brake_to_loiter_timer; // cycles to mix brake and loiter controls in HYBRID_BRAKE_TO_LOITER
// loiter related variables
int16_t loiter_to_pilot_timer_roll; // cycles to mix loiter and pilot controls in HYBRID_LOITER_TO_PILOT
int16_t loiter_to_pilot_timer_pitch; // cycles to mix loiter and pilot controls in HYBRID_LOITER_TO_PILOT
int16_t loiter_final_roll; // final roll angle from loiter controller as we exit loiter mode (used for mixing with pilot input)
int16_t loiter_final_pitch; // final pitch angle from loiter controller as we exit loiter mode (used for mixing with pilot input)
// wind compensation related variables
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
int16_t wind_comp_pitch; // pitch angle to compensate for wind
int8_t wind_comp_timer; // counter to reduce wind_offset calcs to 10hz
// final output
int16_t roll; // final roll angle sent to attitude controller
int16_t pitch; // final pitch angle sent to attitude controller
} hybrid;
// wind compensation related variables
static Vector2f wind_comp_ef; // wind compensation in earth frame, filtered lean angles from position controller
static int16_t wind_offset_roll, wind_offset_pitch; // wind offsets for pitch/roll
static int8_t wind_offset_timer; // counter to reduce wind_offset calcs to 10hz
static int16_t wind_comp_start_timer; // counter to delay start of wind compensation calcs until loiter has settled
// breaking related variables
static float brake_gain; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from brake_rate)
static int16_t brake_roll = 0, brake_pitch = 0; // target roll and pitch angles during both pilot-override and braking periods. Updated during pilot override to match pilot's input
static int16_t brake_timeout_roll, brake_timeout_pitch; // time in seconds allowed for the braking to complete, this timeout will be updated at half-braking
static int16_t brake_roll_max, brake_pitch_max; // used to detect half braking
static float brake_loiter_mix; // varies from 0 to 1, allows a smooth loiter engage
static bool brake_timeout_roll_updated, brake_timeout_pitch_updated; // Allow the timeout to be updated only once per braking.
// loiter related variables
static float loiter_man_mix; // varies from 0 to 1, allow a smooth loiter to manual transition
static int16_t loiter_man_timer;
static int16_t loiter_roll, loiter_pitch; // store pitch/roll at loiter exit
// roll stick input detected, set roll mode to pilot override
hybrid.roll_mode = HYBRID_PILOT_OVERRIDE;
} else {
// roll stick is centered
// if still in pilot override mode and breaking will complete within 0.5seconds switch to braking mode
if ((hybrid.roll_mode == HYBRID_PILOT_OVERRIDE) && (abs(brake_roll) < 2*wp_nav._brake_rate)) {
hybrid.roll_mode = HYBRID_BRAKE; // Set brake roll mode
brake_roll = 0; // this avoid false brake_timeout computing
brake_timeout_roll = 600; // seconds*0.01 - time allowed for the braking to complete, updated at half-braking
brake_timeout_roll_updated = false; // Allow the timeout to be updated only once
brake_roll_max = 0; // used to detect half braking
} else { // manage brake-to-loiter transition
// brake timeout
if (brake_timeout_roll > 0) {
brake_timeout_roll--;
}
// Changed loiter engage : not once HYBRID_SPEED_0 is reached but after a little delay that let the copter stabilize if it remains some rate. (maybe compare omega.x/y rather)
if ((fabs(vel_right) < HYBRID_SPEED_0) && (brake_timeout_roll>50)) {
brake_timeout_roll = 50; // let 0.5s between brake reaches HYBRID_SPEED_0 and loiter engage
}
if ((hybrid.roll_mode == HYBRID_BRAKE) && (brake_timeout_roll==0)){ //stick released and transition finished (speed 0) or brake timeout => loiter mode
hybrid.roll_mode = HYBRID_LOITER; // Set loiter roll mode
}
}
}
// Roll state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final roll output to the attitude controller
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
switch (hybrid.roll_mode) {
// get pitch stick input
if (target_pitch != 0) {
// pitch stick input detected set pitch mode to pilot override
hybrid.pitch_mode = HYBRID_PILOT_OVERRIDE; // Set stab pitch mode
}else{
if((hybrid.pitch_mode == HYBRID_PILOT_OVERRIDE) && (abs(brake_pitch) < 2*wp_nav._brake_rate)){ // stick released from stab and copter horizontal (at wind_comp) => transition mode
hybrid.pitch_mode = HYBRID_BRAKE; // Set brake pitch mode
brake_pitch = 0; // this avoid false brake_timeout computing
brake_timeout_pitch = 600; // seconds*0.01 - time allowed for the braking to complete, updated at half-braking
brake_timeout_pitch_updated = false;// Allow the timeout to be updated only once
brake_pitch_max = 0; // used to detect half braking
}else{ // manage brake-to-loiter transition
// brake timeout
if (brake_timeout_pitch > 0) {
brake_timeout_pitch--;
}
// Changed loiter engage : not once HYBRID_SPEED_0 is reached but after a little delay that let the copter stabilize if it remains some rate. (maybe compare omega.x/y rather)
if ((fabs(vel_fw) < HYBRID_SPEED_0) && (brake_timeout_pitch > 50)) {
brake_timeout_pitch = 50; // let 0.5s between brake reaches HYBRID_SPEED_0 and loiter engage
}
if ((hybrid.pitch_mode == HYBRID_BRAKE) && (brake_timeout_pitch == 0)) {
hybrid.pitch_mode = HYBRID_LOITER; // Set loiter pitch mode
case HYBRID_PILOT_OVERRIDE:
// 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
hybrid.roll_mode = HYBRID_BRAKE; // Set brake roll mode
hybrid.brake_roll = 0; // initialise braking angle to zero
hybrid.brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
hybrid.brake_timeout_roll = HYBRID_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. To-Do: this must be adjusted based on loop rate
hybrid.braking_time_updated_roll = false; // flag the braking time can be re-estimated
// braking angle has started decreasing so re-estimate braking time
hybrid.brake_timeout_roll = 1+(uint16_t)(15L*(int32_t)(abs(hybrid.brake_roll))/(10L*(int32_t)g.hybrid_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
hybrid.braking_time_updated_roll = true;
}
}
} else {
brake_roll = target_roll;
}
}
// manual pitch with smooth decrease filter
if (hybrid.pitch_mode == HYBRID_PILOT_OVERRIDE) {
if (((int32_t)brake_pitch * (int32_t)target_pitch >= 0) && (abs(target_pitch) < HYBRID_STICK_RELEASE_SMOOTH_ANGLE)) { //Smooth decrease only when we want to stop, not if we have to quickly change direction
if (brake_pitch > 0) { // we use brake_pitch to save mem usage and also because it will be natural transition with brake mode.
if (abs(brake_roll) > brake_roll_max) { // detect half braking and update timeout
brake_roll_max = abs(brake_roll);
} else if (!brake_timeout_roll_updated) {
brake_timeout_roll = 1+(uint16_t)(15L*(int32_t)(abs(brake_roll))/(10L*(int32_t)wp_nav._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_updated = true;
}
}
// braking update: pitch
if (hybrid.pitch_mode >= HYBRID_BRAKE) { // Pitch: allow braking update to run also during loiter
if (vel_fw >= 0) { // positive pitch = go backward, negative pitch = go forward
if (abs(brake_pitch)>brake_pitch_max){ // detect half braking and update timeout
brake_pitch_max = abs(brake_pitch);
} else if (!brake_timeout_pitch_updated){
// Changes 12 by 15 to let the brake=>loiter 0.5s happens before this timeout ends
brake_timeout_pitch = 1+(int16_t)(15L*(int32_t)(abs(brake_pitch))/(10L*(int32_t)wp_nav._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_updated = true;
}
}
// loiter to manual mix
if ((hybrid.pitch_mode==HYBRID_PILOT_OVERRIDE)||(hybrid.roll_mode==HYBRID_PILOT_OVERRIDE)) {
hybrid.pitch_mode = HYBRID_BRAKE; // set brake pitch mode
hybrid.brake_pitch = 0; // initialise braking angle to zero
hybrid.brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
hybrid.brake_timeout_pitch = HYBRID_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. To-Do: this must be adjusted based on loop rate
hybrid.braking_time_updated_pitch = false; // flag the braking time can be re-estimated
}
// final lean angle should be pilot input plus wind compensation
// braking angle has started decreasing so re-estimate braking time
hybrid.brake_timeout_pitch = 1+(uint16_t)(15L*(int32_t)(abs(hybrid.brake_pitch))/(10L*(int32_t)g.hybrid_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
hybrid.braking_time_updated_pitch = true;
}
}
// if velocity is very low reduce braking time to 0.5seconds
// Note: this speed is extremely low (only 10cm/s) meaning this case is likely never executed
if ((fabs(vel_right) <= HYBRID_SPEED_0) && (hybrid.brake_timeout_pitch > 50)) {
hybrid.brake_timeout_pitch = 50;
}
// reduce braking timer
if (hybrid.brake_timeout_pitch > 0) {
hybrid.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 HYBRID_BRAKE_READY_TO_LOITER
// logic for engaging loiter is handled below the pitch and pitch mode switch statements
hybrid.pitch_mode = HYBRID_BRAKE_READY_TO_LOITER;
}
// check for pilot input
if (target_pitch != 0) {
hybrid.pitch_mode = HYBRID_PILOT_OVERRIDE;
}
// final lean angle is braking angle + wind compensation angle
Randy Mackay
11 years ago