From a642c88e3440f2a0008d608fda519cecea1b3df1 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Tue, 9 Jun 2015 20:06:33 -0400 Subject: [PATCH] Copter: AltHold state machine --- ArduCopter/control_althold.cpp | 85 +++++++++++++++++++++++++--------- ArduCopter/defines.h | 8 ++++ 2 files changed, 71 insertions(+), 22 deletions(-) diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index 8f5d379479..8b0932619f 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -23,37 +23,54 @@ bool Copter::althold_init(bool ignore_checks) // should be called at 100hz or more void Copter::althold_run() { - float target_roll, target_pitch; - float target_yaw_rate; - float target_climb_rate; + AltHoldModeState althold_state; float takeoff_climb_rate = 0.0f; - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if(!ap.auto_armed || !motors.get_interlock()) { - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); - return; - } - // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // get pilot desired lean angles - // To-Do: convert get_pilot_desired_lean_angles to return angles as floats + float target_roll, target_pitch; get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch); // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); + float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in); + float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->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); + // Alt Hold State Machine Determination + if(!ap.auto_armed || !motors.get_interlock()) { + althold_state = AltHold_Disarmed; + } else if (ap.land_complete && (takeoff_state.running || (channel_throttle->control_in > get_takeoff_trigger_throttle()))){ + althold_state = AltHold_Takeoff; + } else if (ap.land_complete){ + althold_state = AltHold_Landed; + } else { + althold_state = AltHold_Flying; + } + + // Alt Hold State Machine + switch (althold_state) { + + case AltHold_Disarmed: + +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif // HELI_FRAME + pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); + break; + + case AltHold_Takeoff: + + // get takeoff adjusted pilot and takeoff climb rates + takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); - // check for take-off - if (ap.land_complete && (takeoff_state.running || (channel_throttle->control_in > get_takeoff_trigger_throttle()))) { if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } @@ -62,13 +79,37 @@ void Copter::althold_run() set_land_complete(false); // clear i term when we're taking off set_throttle_takeoff(); - } - // reset target lean angles and heading while landed - if (ap.land_complete) { + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + // body-frame rate controller is run directly from 100hz loop + + // call throttle controller + // ToDo: Should we really run surface_tracking during takeoff? + // if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) { + // if sonar is ok, use surface tracking + // target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); + // } + + // call position controller + pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); + pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); + pos_control.update_z_controller(); + break; + + case AltHold_Landed: + +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt); +#endif pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); - }else{ + break; + + case AltHold_Flying: // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // body-frame rate controller is run directly from 100hz loop @@ -81,7 +122,7 @@ void Copter::althold_run() // call position controller pos_control.set_alt_target_from_climb_rate(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 2feb75b663..e116d8df1b 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -202,6 +202,14 @@ enum RTLState { RTL_Land }; +// Alt_Hold states +enum AltHoldModeState { + AltHold_Disarmed, + AltHold_Takeoff, + AltHold_Flying, + AltHold_Landed +}; + // Flip states enum FlipState { Flip_Start,