diff --git a/ArduCopter/control_hybrid.pde b/ArduCopter/control_hybrid.pde index a52020fae4..90d2f860a9 100644 --- a/ArduCopter/control_hybrid.pde +++ b/ArduCopter/control_hybrid.pde @@ -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)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)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)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)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) 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)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)) 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);