From 88da5bd4539adda8cc78463ad1ed1f80942de8e1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 1 Aug 2016 10:49:44 +0900 Subject: [PATCH] Copter: sport mode restructured to match althold and feedback from mid-stick This modifies sport mode to be structured like althold and loiter flight modes so that ongoing maintenance of the modes is hopefully easier. Also changes throttle feedback to occur from mid-stick --- ArduCopter/control_sport.cpp | 109 ++++++++++++++++++++++++----------- ArduCopter/defines.h | 8 +++ 2 files changed, 83 insertions(+), 34 deletions(-) diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index d0bc7e08d5..06d75866e0 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -24,30 +24,21 @@ bool Copter::sport_init(bool ignore_checks) // should be called at 100hz or more void Copter::sport_run() { - float target_roll_rate, target_pitch_rate, target_yaw_rate; - float target_climb_rate = 0; + SportModeState sport_state; float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); - // if not armed or throttle at zero, set throttle to zero and exit immediately - if (!motors.armed() || ap.throttle_zero || !motors.get_interlock()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - pos_control.relax_alt_hold_controllers(0.0f); - return; - } - // apply SIMPLE mode transform update_simple_mode(); // get pilot's desired roll and pitch rates // calculate rate requests - target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p; - target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p; + float target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p; + float target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p; int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); target_roll_rate -= constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; @@ -69,41 +60,91 @@ void Copter::sport_run() } // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); - // get takeoff adjusted pilot and takeoff climb rates - takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); +#if FRAME_CONFIG == HELI_FRAME + // helicopters are held on the ground until rotor speed runup has finished + bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete()); +#else + bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f); +#endif + + // State Machine Determination + if (!motors.armed() || !motors.get_interlock()) { + sport_state = Sport_MotorStopped; + } else if (takeoff_state.running || takeoff_triggered) { + sport_state = Sport_Takeoff; + } else if (!ap.auto_armed || ap.land_complete) { + sport_state = Sport_Landed; + } else { + sport_state = Sport_Flying; + } + + // State Machine + switch (sport_state) { + + case Sport_MotorStopped: + + motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); +#if FRAME_CONFIG == HELI_FRAME + // force descent rate and call position controller + pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); +#else + attitude_control.relax_attitude_controllers(); + attitude_control.reset_rate_controller_I_terms(); + attitude_control.set_yaw_target_to_current_heading(); + pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero +#endif + pos_control.update_z_controller(); + break; + + case Sport_Takeoff: + // set motors to full range + motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // check for take-off - if (ap.land_complete && (takeoff_state.running || (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()))) { + // initiate take-off if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); + // indicate we are taking off + set_land_complete(false); + // clear i terms + set_throttle_takeoff(); } - // indicate we are taking off - set_land_complete(false); - // clear i term when we're taking off - set_throttle_takeoff(); - } + // get take-off adjusted pilot and takeoff climb rates + takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); - // reset target lean angles and heading while landed - if (ap.land_complete) { - if (ap.throttle_zero) { + // call attitude controller + attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); + + // call position controller + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); + pos_control.update_z_controller(); + break; + + case Sport_Landed: + // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) + if (target_climb_rate < 0.0f) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - }else{ + } else { motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } - // move throttle to between minimum and non-takeoff-throttle to keep us on the ground - attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt); - pos_control.relax_alt_hold_controllers(0.0f); - }else{ - // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + attitude_control.reset_rate_controller_I_terms(); + attitude_control.set_yaw_target_to_current_heading(); + attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); + pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control.update_z_controller(); + break; + + case Sport_Flying: + motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); @@ -115,7 +156,7 @@ void Copter::sport_run() // call position controller pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control.update_z_controller(); + break; } } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index fb8715ed89..2cc411d7ec 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -238,6 +238,14 @@ enum LoiterModeState { Loiter_Landed }; +// Sport states +enum SportModeState { + Sport_MotorStopped, + Sport_Takeoff, + Sport_Flying, + Sport_Landed +}; + // Flip states enum FlipState { Flip_Start,