#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_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.
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)
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.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.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
