Copter: add hybrid state struct, formatting changes

This commit is contained in:
Randy Mackay 2014-04-05 23:20:39 +09:00
parent b5ed23f592
commit d9c685323a

View File

@ -1,11 +1,11 @@
////////////////////////////////////////////////////////////////////////////////
// Hybrid Mode : ST-JD
// flight mode = 12
////////////////////////////////////////////////////////////////////////////////
#define LOITER_DEADBAND 70
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* control_hybrid.pde - init and run calls for hybrid flight mode
* hybrid tries to improve upon regular loiter by mixing the pilot input with the loiter controller
*/
#define SPEED_0 10
#define NAV_HYBRID 1
#define NAV_NONE 0
#define LOITER_STAB_TIMER 300 // ST-JD : Must be higher than BRAKE_LOIT_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 BRAKE_LOIT_MIX_TIMER 150 // ST-JD : Must be lower than LOITER_STAB_TIMER set it from 100 to 200, the number of centiseconds brake and loiter commands are mixed to make a smooth transition.
#define LOITER_MAN_MIX_TIMER 50 // ST-JD : set it from 100 to 200, the number of centiseconds loiter and manual commands are mixed to make a smooth transition.
@ -14,15 +14,19 @@
#define THROTTLE_HYBRID_AH 1
#define THROTTLE_HYBRID_BK 3
//#define MX1HYBRID // Alt Hold when throttle in deadband, manual otherwise
#define MX2HYBRID // Alt Hold when throttle from 0 to deadband_high, manual otherwise (above deadband)
//#define MXHYBRID // switch Alt Hold <-> Throttle Assist
//define AHHYBRID // only Alt Hold
//define TAHYBRID // only Throttle Assist
//define JDHYBRID // stick at center=vertical brake and alt-hold when vertical speed is near zero. Stick out from deadband (+70/-70) manual throttle (throttle assist for both!)
// mission state enumeration
enum hybrid_rp_mode {
HYBRID_PILOT_OVERRIDE=0,
HYBRID_BRAKE=1,
HYBRID_LOITER=2
};
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;
static uint8_t hybrid_mode_roll; // 1=alt_hold; 2=brake 3=loiter
static uint8_t hybrid_mode_pitch; // 1=alt_hold; 2=brake 3=loiter
static int16_t brake_roll = 0,brake_pitch = 0;
static float K_brake;
static uint8_t throttle_mode=THROTTLE_HYBRID_MAN;
@ -39,29 +43,35 @@ static float brake_loiter_mix; // varies from 0 to
static float loiter_man_mix; // varies from 0 to 1, allow a smooth loiter to manual transition
static int16_t loiter_man_timer;
static int8_t update_wind_offset_timer; // update wind_offset decimator (10Hz)
static int8_t hybrid_nav_mode=NAV_NONE; // replace old nav_mode variable
// hybrid_init - initialise hybrid controller
static bool hybrid_init(bool ignore_checks)
{
/*
if (GPS_ok() || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();
// clear pilot's feed forward for roll and pitch
wp_nav.set_pilot_desired_acceleration(0, 0);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
// compute K_brake
K_brake=(15.0f*(float)wp_nav._brake_rate+95.0f)/100.0f;
if (ap.land_complete) {
// Loiter start
hybrid_mode_roll=3;
hybrid_mode_pitch=3;
hybrid.roll_mode = HYBRID_LOITER;
hybrid.pitch_mode = HYBRID_LOITER;
}else{
// Alt_hold like to avoid hard twitch if hybrid enabled in flight
hybrid_mode_roll=1;
hybrid_mode_pitch=1;
hybrid.roll_mode = HYBRID_PILOT_OVERRIDE;
hybrid.pitch_mode = HYBRID_PILOT_OVERRIDE;
}
wind_comp_x=wind_comp_y=0; // Init wind_comp (ef). For now, resetted each time hybrid is switched on
wind_offset_roll=0; // Init offset angles
wind_offset_pitch=0;
@ -90,8 +100,8 @@ static void hybrid_run()
/*
float target_yaw_rate = 0;
float target_climb_rate = 0;
Vector3f vel; // ST-JD : Used for Hybrid_mode
float vel_fw, vel_right; // ST-JD : Used for Hybrid_mode
const Vector3f& vel = inertial_nav.get_velocity();
float vel_fw, vel_right;
int16_t target_roll, target_pitch;
int16_t pilot_throttle_scaled=0;
@ -109,10 +119,6 @@ static void hybrid_run()
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
// To-Do: do we need to clear out feed forward if this is not called?
wp_nav.set_pilot_desired_acceleration(0, 0);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
@ -128,45 +134,55 @@ static void hybrid_run()
}
}
// if landed initialise loiter targets, set throttle to zero and exit
if (ap.land_complete) {
wp_nav.init_loiter_target();
attitude_control.init_targets();
attitude_control.set_throttle_out(0, false);
return;
}else{
// convert pilot input to lean angles
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch);
// speed
vel = inertial_nav.get_velocity();
vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw(); // bf -> ef
vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw(); // bf -> ef
// define roll/pitch modes from stick input
// get roll stick input and update new roll mode
if (abs(target_roll) > LOITER_DEADBAND) { //stick input detected => direct to stab mode
hybrid_mode_roll = 1; // Set stab roll mode
}else{
if((hybrid_mode_roll == 1) && (abs(brake_roll) < 2*wp_nav._brake_rate)){ // stick released from stab and copter horizontal (at wind comp) => transition mode
hybrid_mode_roll = 2; // Set brake roll mode
// calculate earth-frame velocities
vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw();
vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw();
// get roll stick input
if (target_roll != 0) {
// roll stick input detected set roll mode to pilot override
hybrid.roll_mode = HYBRID_PILOT_OVERRIDE; // Set stab roll mode
} else {
// stick released from stab and copter horizontal (at wind comp) => transition 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
timeout_roll = 600; // seconds*0.01 - time allowed for the braking to complete, updated at half-braking
timeout_roll_updated = false; // Allow the timeout to be updated only once
brake_max_roll = 0; // used to detect half braking
}else{ // manage brake-to-loiter transition
} else { // manage brake-to-loiter transition
// brake timeout
if (timeout_roll>0) timeout_roll--;
if (timeout_roll > 0) {
timeout_roll--;
}
// Changed loiter engage : not once speed_0 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)<SPEED_0) && (timeout_roll>50)) timeout_roll = 50; // let 0.5s between brake reaches speed_0 and loiter engage
if ((hybrid_mode_roll == 2) && (timeout_roll==0)){ //stick released and transition finished (speed 0) or brake timeout => loiter mode
hybrid_mode_roll = 3; // Set loiter roll mode
if ((fabs(vel_right) < SPEED_0) && (timeout_roll>50)) {
timeout_roll = 50; // let 0.5s between brake reaches speed_0 and loiter engage
}
if ((hybrid.roll_mode == HYBRID_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
}
}
}
//get pitch stick input and update new pitch mode
if (abs(target_pitch) > LOITER_DEADBAND){ //stick input detected => direct to stab mode
hybrid_mode_pitch = 1; // Set stab pitch 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_mode_pitch == 1) && (abs(brake_pitch) < 2*wp_nav._brake_rate)){ // stick released from stab and copter horizontal (at wind_comp) => transition mode
hybrid_mode_pitch = 2; // Set brake pitch mode
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
timeout_pitch=600; // seconds*0.01 - time allowed for the braking to complete, updated at half-braking
timeout_pitch_updated = false; // Allow the timeout to be updated only once
@ -175,27 +191,36 @@ static void hybrid_run()
// brake timeout
if (timeout_pitch>0) timeout_pitch--;
// Changed loiter engage : not once speed_0 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)<SPEED_0) && (timeout_pitch>50)) timeout_pitch = 50; // let 0.5s between brake reaches speed_0 and loiter engage
if ((hybrid_mode_pitch == 2) && (timeout_pitch==0)) {
hybrid_mode_pitch = 3; // Set loiter pitch mode
if ((fabs(vel_fw)<SPEED_0) && (timeout_pitch>50)) {
timeout_pitch = 50; // let 0.5s between brake reaches speed_0 and loiter engage
}
if ((hybrid.pitch_mode == HYBRID_BRAKE) && (timeout_pitch == 0)) {
hybrid.pitch_mode = HYBRID_LOITER; // Set loiter pitch mode
}
}
}
// manual roll/pitch with smooth decrease filter
// roll
if (hybrid_mode_roll == 1){
if (((long)brake_roll*(long)target_roll>=0)&&(abs(target_roll)<STICK_RELEASE_SMOOTH_ANGLE)){ //Smooth decrease only when we want to stop, not if we have to quickly change direction
if (brake_roll>0){ // we use brake_roll to save mem usage and also because it will be natural transition with brake mode.
brake_roll-=max((float)brake_roll*(float)wp_nav._smooth_rate_factor/100,wp_nav._brake_rate); //rate decrease
brake_roll=max(brake_roll,target_roll); // use the max value if we increase and because we could have a smoother manual decrease than this computed value
if (hybrid.roll_mode == HYBRID_PILOT_OVERRIDE) {
if (((long)brake_roll*(long)target_roll>=0) && (abs(target_roll)<STICK_RELEASE_SMOOTH_ANGLE)) {
// Smooth decrease only when we want to stop, not if we have to quickly change direction
if (brake_roll > 0){ // we use brake_roll to save mem usage and also because it will be natural transition with brake mode.
// rate decrease
brake_roll -= max((float)brake_roll*(float)wp_nav._smooth_rate_factor/100,wp_nav._brake_rate);
// use the max value if we increase and because we could have a smoother manual decrease than this computed value
brake_roll = max(brake_roll,target_roll);
}else{
brake_roll+=max(-(float)brake_roll*(float)wp_nav._smooth_rate_factor/100,wp_nav._brake_rate);
brake_roll=min(brake_roll,target_roll);
brake_roll += max(-(float)brake_roll*(float)wp_nav._smooth_rate_factor/100,wp_nav._brake_rate);
brake_roll = min(brake_roll,target_roll);
}
}else brake_roll=target_roll;
} else {
brake_roll=target_roll;
}
}
// pitch
if (hybrid_mode_pitch == 1) {
if (hybrid.pitch_mode == HYBRID_PILOT_OVERRIDE) {
if (((long)brake_pitch*(long)target_pitch>=0)&&(abs(target_pitch)<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.
brake_pitch-=max((float)brake_pitch*(float)wp_nav._smooth_rate_factor/100,wp_nav._brake_rate); //rate decrease
@ -209,21 +234,21 @@ static void hybrid_run()
}
}
// braking update: roll
if (hybrid_mode_roll>=2) { // Roll: allow braking update to run also during loiter
if (vel_right>=0) { // negative roll = go left, positive roll = go right
brake_roll = max(brake_roll-wp_nav._brake_rate,max((-K_brake*vel_right*(1.0f+500.0f/(vel_right+60.0f))),-wp_nav._max_braking_angle)); // centidegrees
if (hybrid.roll_mode >= HYBRID_BRAKE) { // Roll: allow braking update to run also during loiter
if (vel_right >= 0) { // negative roll = go left, positive roll = go right
brake_roll = max(brake_roll-wp_nav._brake_rate, max((-K_brake*vel_right*(1.0f+500.0f/(vel_right+60.0f))),-wp_nav._max_braking_angle)); // centidegrees
}else{
brake_roll = min(brake_roll+wp_nav._brake_rate,min((-K_brake*vel_right*(1.0f+500.0f/(-vel_right+60.0f))),wp_nav._max_braking_angle)); // centidegrees
brake_roll = min(brake_roll+wp_nav._brake_rate, min((-K_brake*vel_right*(1.0f+500.0f/(-vel_right+60.0f))),wp_nav._max_braking_angle)); // centidegrees
}
if (abs(brake_roll)>brake_max_roll) { // detect half braking and update timeout
brake_max_roll=abs(brake_roll);
if (abs(brake_roll) > brake_max_roll) { // detect half braking and update timeout
brake_max_roll = abs(brake_roll);
} else if (!timeout_roll_updated){
timeout_roll = 1+(uint16_t)(15L*(long)(abs(brake_roll))/(10L*(long)wp_nav._brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
timeout_roll_updated = true;
}
}
// braking update: pitch
if (hybrid_mode_pitch>=2) { // Pitch: allow braking update to run also during loiter
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
brake_pitch = min(brake_pitch+wp_nav._brake_rate,min((K_brake*vel_fw*(1.0f+(500.0f/(vel_fw+60.0f)))),wp_nav._max_braking_angle)); // centidegrees
} else {
@ -238,16 +263,16 @@ static void hybrid_run()
}
}
// loiter to manual mix
if ((hybrid_mode_pitch==1)||(hybrid_mode_roll==1)) {
if ((hybrid.pitch_mode==HYBRID_PILOT_OVERRIDE)||(hybrid.roll_mode==HYBRID_PILOT_OVERRIDE)) {
if (!ap.land_complete && loiter_man_timer!=0) {
loiter_man_mix = constrain_float((float)(loiter_man_timer)/(float)LOITER_MAN_MIX_TIMER, 0, 1.0);//constrain_float((float)(LOITER_MAN_MIX_TIMER-loiter_man_timer)/(float)LOITER_MAN_MIX_TIMER, 0, 1.0);
loiter_man_timer--;
}
}
// loitering/moving:
if ((hybrid_mode_pitch==3)&&(hybrid_mode_roll==3)){
if (hybrid.pitch_mode == HYBRID_LOITER && hybrid.roll_mode == HYBRID_LOITER) {
// while loitering, updates average lat/lon wind offset angles from I terms
if (hybrid_nav_mode==NAV_HYBRID){
if (hybrid.loiter_engaged) {
if (!ap.land_complete && loiter_stab_timer!=0) {
loiter_stab_timer--;
} else if (max(fabs(vel.x),fabs(vel.y))<SPEED_0) { //Or maybe 2*, 3* speed_0...
@ -257,7 +282,7 @@ static void hybrid_run()
// Brake_Loiter commands mix factor
brake_loiter_mix = constrain_float((float)(LOITER_STAB_TIMER-loiter_stab_timer)/(float)BRAKE_LOIT_MIX_TIMER, 0, 1.0);
} else {
hybrid_nav_mode=NAV_HYBRID; // turns on NAV_HYBRID if both sticks are at rest
hybrid.loiter_engaged = true; // turns on NAV_HYBRID if both sticks are at rest
pos_control.init_I=false; // restore previous i_terms in Reset_I() => to avoid the stop_and_go effect
wp_nav.init_loiter_target(); // init loiter controller and sets XY stopping point
pos_control.set_target_to_stopping_point_z(); // init altitude
@ -266,8 +291,9 @@ static void hybrid_run()
brake_pitch = 1; // required for next mode_1 smooth stick release and to avoid twitch
}
} else {
if (hybrid_nav_mode!=NAV_NONE) { // transition from Loiter to Manual
hybrid_nav_mode=NAV_NONE;
// transition from Loiter to Manual
if (hybrid.loiter_engaged) {
hybrid.loiter_engaged = false;
loiter_man_timer=LOITER_MAN_MIX_TIMER;
// save pitch/roll at loiter exit
loiter_roll=wp_nav.get_roll();
@ -279,43 +305,45 @@ static void hybrid_run()
wind_offset_pitch = (float)fast_atan(-(wind_comp_x*ahrs.cos_yaw() + wind_comp_y*ahrs.sin_yaw())/981)*(18000/M_PI);
wind_offset_roll = (float)fast_atan((-wind_comp_x*ahrs.sin_yaw() + wind_comp_y*ahrs.cos_yaw())/981)*(18000/M_PI);
update_wind_offset_timer=10;
} else update_wind_offset_timer--;
} else {
update_wind_offset_timer--;
}
}
// if required, update loiter controller
if(hybrid_nav_mode == NAV_HYBRID) {
if(hybrid.loiter_engaged) {
wp_nav.update_loiter();
}
// select output to stabilize controllers
switch (hybrid_mode_roll) {
switch (hybrid.roll_mode) {
// To-Do: try to mix loiter->manual using brake_loiter_mix variable as we are doing on loiter engage
case 1:
case HYBRID_PILOT_OVERRIDE:
// Loiter-Manual mix at loiter exit
target_roll = loiter_man_mix*(float)loiter_roll+(1.0f-loiter_man_mix)*(float)(brake_roll+wind_offset_roll);
break;
case 2:
target_roll = brake_roll+wind_offset_roll;
case HYBRID_BRAKE:
target_roll = brake_roll + wind_offset_roll;
break;
case 3:
if (hybrid_nav_mode == NAV_HYBRID) { // if nav_hybrid enabled...
case HYBRID_LOITER:
if (hybrid.loiter_engaged) { // if nav_hybrid enabled...
// Brake_Loiter mix at loiter engage
target_roll = brake_loiter_mix*(float)wp_nav.get_roll()+(1.0f-brake_loiter_mix)*(float)(brake_roll+wind_offset_roll);
}else {
target_roll = brake_roll+wind_offset_roll;
target_roll = brake_roll + wind_offset_roll;
}
break;
}
switch (hybrid_mode_pitch){
case 1:
switch (hybrid.pitch_mode){
case HYBRID_PILOT_OVERRIDE:
//Loiter-Manual mix at loiter exit
target_pitch = loiter_man_mix*(float)loiter_pitch+(1.0f-loiter_man_mix)*(float)(brake_pitch+wind_offset_pitch);
break;
case 2:
case HYBRID_BRAKE:
target_pitch = brake_pitch+wind_offset_pitch;
break;
case 3:
if(hybrid_nav_mode == NAV_HYBRID) { // if nav_hybrid enabled...
case HYBRID_LOITER:
if(hybrid.loiter_engaged) { // if nav_hybrid enabled...
// Brake_Loiter mix at loiter engage
target_pitch = brake_loiter_mix*(float)wp_nav.get_pitch()+(1.0f-brake_loiter_mix)*(float)(brake_pitch+wind_offset_pitch);
} else {
@ -323,11 +351,15 @@ static void hybrid_run()
}
break;
}
// clip target pitch/roll
// constrain target pitch/roll angles
target_roll=constrain_int16(target_roll,-aparm.angle_max,aparm.angle_max);
target_pitch=constrain_int16(target_pitch,-aparm.angle_max,aparm.angle_max);
// update attitude controller targets
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(target_roll, target_pitch, target_yaw_rate);
// throttle control
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
// if sonar is ok, use surface tracking
target_climb_rate = get_throttle_surface_tracking(target_climb_rate, pos_control.get_alt_target(), G_Dt);