From 38cc5a817f1bb812130edf0b11db9515c2507db1 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 28 Feb 2019 18:03:23 +0900 Subject: [PATCH] Copter: consolidate mode state decisions bnsgeyer and rmackay9 contributed to these changes make_safe_shut_down waits for spool down before disarming remove use of attitude_control::set_throttle_out_unstabilized to consolidate logic between multicopters and tradhelis --- ArduCopter/defines.h | 24 - ArduCopter/mode.cpp | 93 +++- ArduCopter/mode.h | 12 + ArduCopter/mode_acro.cpp | 35 +- ArduCopter/mode_althold.cpp | 78 +--- ArduCopter/mode_auto.cpp | 48 +- ArduCopter/mode_brake.cpp | 19 +- ArduCopter/mode_circle.cpp | 46 +- ArduCopter/mode_drift.cpp | 42 +- ArduCopter/mode_flip.cpp | 6 +- ArduCopter/mode_flowhold.cpp | 120 ++--- ArduCopter/mode_follow.cpp | 9 +- ArduCopter/mode_guided.cpp | 84 ++-- ArduCopter/mode_land.cpp | 80 ++-- ArduCopter/mode_loiter.cpp | 57 +-- ArduCopter/mode_poshold.cpp | 809 ++++++++++++++++++---------------- ArduCopter/mode_rtl.cpp | 47 +- ArduCopter/mode_sport.cpp | 63 +-- ArduCopter/mode_stabilize.cpp | 44 +- ArduCopter/mode_throw.cpp | 6 +- 20 files changed, 842 insertions(+), 880 deletions(-) diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 0ca9b9d3b3..824710c825 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -176,30 +176,6 @@ enum SmartRTLState { SmartRTL_Land }; -// Alt_Hold states -enum AltHoldModeState { - AltHold_MotorStopped, - AltHold_Takeoff, - AltHold_Flying, - AltHold_Landed -}; - -// Loiter states -enum LoiterModeState { - Loiter_MotorStopped, - Loiter_Takeoff, - Loiter_Flying, - Loiter_Landed -}; - -// Sport states -enum SportModeState { - Sport_MotorStopped, - Sport_Takeoff, - Sport_Flying, - Sport_Landed -}; - // Flip states enum FlipState { Flip_Start, diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index d7732b37f6..859b62379c 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -396,14 +396,43 @@ void Copter::Mode::zero_throttle_and_relax_ac(bool spool_up) } else { motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); } -#if FRAME_CONFIG == HELI_FRAME - // Helicopters always stabilize roll/pitch/yaw attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt); -#else - // multicopters do not stabilize roll/pitch/yaw when disarmed - attitude_control->set_throttle_out_unstabilized(0.0f, true, copter.g.throttle_filt); -#endif +} + +void Copter::Mode::zero_throttle_and_hold_attitude() +{ + // run attitude controller + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); + attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt); +} + +void Copter::Mode::make_safe_shut_down() +{ + // command aircraft to initiate the shutdown process + motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + switch (motors->get_spool_mode()) { + + case AP_Motors::SHUT_DOWN: + case AP_Motors::GROUND_IDLE: + // relax controllers during idle states + attitude_control->reset_rate_controller_I_terms(); + attitude_control->set_yaw_target_to_current_heading(); + break; + + default: + // while transitioning though active states continue to operate normally + break; + } + + // we may need to move this out + wp_nav->wp_and_spline_init(); + // we may need to move this out + loiter_nav->init_target(); + pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->update_z_controller(); + // we may need to move this out + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); } /* @@ -596,6 +625,58 @@ float Copter::Mode::get_pilot_desired_throttle() const return throttle_out; } +Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_climb_rate_cms) +{ + // Alt Hold State Machine Determination + if (!motors->armed()) { + // the aircraft should moved to a shut down state + motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + + // transition through states as aircraft spools down + switch (motors->get_spool_mode()) { + + case AP_Motors::SHUT_DOWN: + return AltHold_MotorStopped; + + case AP_Motors::DESIRED_GROUND_IDLE: + return AltHold_Landed_Ground_Idle; + + default: + return AltHold_Landed_Pre_Takeoff; + } + + } else if (takeoff.running() || takeoff.triggered(target_climb_rate_cms)) { + // the aircraft is currently landed or taking off, asking for a positive climb rate and in THROTTLE_UNLIMITED + // the aircraft should progress through the take off procedure + return AltHold_Takeoff; + + } else if (!ap.auto_armed || ap.land_complete) { + // the aircraft is armed and landed + if (target_climb_rate_cms < 0.0f && !ap.using_interlock) { + // the aircraft should move to a ground idle state + motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + + } else { + // the aircraft should prepare for imminent take off + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + } + + if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + // the aircraft is waiting in ground idle + return AltHold_Landed_Ground_Idle; + + } else { + // the aircraft can leave the ground at any time + return AltHold_Landed_Pre_Takeoff; + } + + } else { + // the aircraft is in a flying state + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + return AltHold_Flying; + } +} + // pass-through functions to reduce code churn on conversion; // these are candidates for moving into the Mode base // class. diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 365b0bf6c5..c23338ca5a 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -116,6 +116,8 @@ protected: // helper functions void zero_throttle_and_relax_ac(bool spool_up = false); + void zero_throttle_and_hold_attitude(); + void make_safe_shut_down(); // functions to control landing // in modes that support landing @@ -126,6 +128,16 @@ protected: // return expected input throttle setting to hover: virtual float throttle_hover() const; + // Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport + enum AltHoldModeState { + AltHold_MotorStopped, + AltHold_Takeoff, + AltHold_Landed_Ground_Idle, + AltHold_Landed_Pre_Takeoff, + AltHold_Flying + }; + AltHoldModeState get_alt_hold_state(float target_climb_rate_cms); + // convenience references to avoid code churn in conversion: Parameters &g; ParametersG2 &g2; diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index 35499eaf9e..ac690454c0 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -10,21 +10,34 @@ void Copter::ModeAcro::run() { + // convert the input to the desired body frame rate float target_roll, target_pitch, target_yaw; + get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); - // if not armed set throttle to zero and exit immediately - if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); - return; + if (!motors->armed()) { + // Motors should be Stopped + motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + } else if (ap.throttle_zero) { + // Attempting to Land + motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + } else { + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } - // clear landing flag - set_land_complete(false); - - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // convert the input to the desired body frame rate - get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); + if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) { + // Motors Stopped + attitude_control->set_yaw_target_to_current_heading(); + attitude_control->reset_rate_controller_I_terms(); + } else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + // Landed + attitude_control->set_yaw_target_to_current_heading(); + attitude_control->reset_rate_controller_I_terms(); + } else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + // clear landing flag above zero throttle + if (!motors->limit.throttle_lower) { + set_land_complete(false); + } + } // run attitude controller attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 0bd49de4f5..8d08a11e89 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -21,7 +21,6 @@ bool Copter::ModeAltHold::init(bool ignore_checks) // should be called at 100hz or more void Copter::ModeAltHold::run() { - AltHoldModeState althold_state; float takeoff_climb_rate = 0.0f; // initialize vertical speeds and acceleration @@ -43,40 +42,30 @@ void Copter::ModeAltHold::run() target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); // Alt Hold State Machine Determination - if (!motors->armed() || !motors->get_interlock()) { - althold_state = AltHold_MotorStopped; - } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) { - althold_state = AltHold_Takeoff; - } else if (!ap.auto_armed || ap.land_complete) { - althold_state = AltHold_Landed; - } else { - althold_state = AltHold_Flying; - } + AltHoldModeState althold_state = get_alt_hold_state(target_climb_rate); // Alt Hold State Machine switch (althold_state) { case AltHold_MotorStopped: - motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); -#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); - if (ap.land_complete_maybe) { - pos_control->relax_alt_hold_controllers(0.0f); - } -#else pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero -#endif - pos_control->update_z_controller(); + break; + + case AltHold_Landed_Ground_Idle: + + attitude_control->reset_rate_controller_I_terms(); + attitude_control->set_yaw_target_to_current_heading(); + // FALLTHROUGH + + case AltHold_Landed_Pre_Takeoff: + + pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero break; case AltHold_Takeoff: - // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // initiate take-off if (!takeoff.running()) { @@ -93,38 +82,9 @@ void Copter::ModeAltHold::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - - // call position controller + // set position controller targets 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 AltHold_Landed: - -#if FRAME_CONFIG == HELI_FRAME - // helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot. - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - if (motors->init_targets_on_arming()) { - attitude_control->reset_rate_controller_I_terms(); - attitude_control->set_yaw_target_to_current_heading(); - } -#else - // 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_GROUND_IDLE); - } else { - 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(); -#endif - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, 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 AltHold_Flying: @@ -135,18 +95,20 @@ void Copter::ModeAltHold::run() copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max); #endif - // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - // adjust climb rate using rangefinder target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control->update_z_controller(); break; } + + // call attitude controller + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + + // call z-axis position controller + pos_control->update_z_controller(); + } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index d40ccbdcc8..5b2e93f514 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -729,16 +729,6 @@ void Copter::ModeAuto::takeoff_run() // called by auto_run at 100hz or more void Copter::ModeAuto::wp_run() { - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off - // (of course it would be better if people just used take-off) - zero_throttle_and_relax_ac(); - // clear i term when we're taking off - set_throttle_takeoff(); - return; - } - // process pilot's yaw input float target_yaw_rate = 0; if (!copter.failsafe.radio) { @@ -749,6 +739,12 @@ void Copter::ModeAuto::wp_run() } } + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); + return; + } + // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); @@ -772,13 +768,9 @@ void Copter::ModeAuto::wp_run() // called by auto_run at 100hz or more void Copter::ModeAuto::spline_run() { - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off - // (of course it would be better if people just used take-off) - zero_throttle_and_relax_ac(); - // clear i term when we're taking off - set_throttle_takeoff(); + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); return; } @@ -815,12 +807,14 @@ void Copter::ModeAuto::spline_run() // called by auto_run at 100hz or more void Copter::ModeAuto::land_run() { - // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); - // set target to current position - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); + // disarm when the landing detector says we've landed + if (ap.land_complete) { + copter.init_disarm_motors(); + } + + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); return; } @@ -872,9 +866,9 @@ void Copter::ModeAuto::nav_guided_run() // called by auto_run at 100hz or more void Copter::ModeAuto::loiter_run() { - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); return; } @@ -1515,7 +1509,7 @@ bool Copter::ModeAuto::verify_land() case LandStateType_Descending: // rely on THROTTLE_LAND mode to correctly update landing status - retval = ap.land_complete; + retval = ap.land_complete && (motors->get_spool_mode() == AP_Motors::GROUND_IDLE); break; default: diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 6b2bca08c4..7f2c69872e 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -31,27 +31,20 @@ bool Copter::ModeBrake::init(bool ignore_checks) // should be called at 100hz or more void Copter::ModeBrake::run() { - // if not auto armed set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE); - zero_throttle_and_relax_ac(); - pos_control->relax_alt_hold_controllers(0.0f); + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); return; } + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + // relax stop target if we might be landed if (ap.land_complete_maybe) { loiter_nav->soften_for_landing(); } - // if landed immediately disarm - if (ap.land_complete) { - copter.init_disarm_motors(); - } - - // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // run brake controller wp_nav->update_brake(); diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index b262e7d973..1952098be0 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -27,41 +27,30 @@ bool Copter::ModeCircle::init(bool ignore_checks) // should be called at 100hz or more void Copter::ModeCircle::run() { - float target_yaw_rate = 0; - float target_climb_rate = 0; - // initialize speeds and accelerations pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy()); pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration()); pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_max_accel_z(g.pilot_accel_z); - - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { - // To-Do: add some initialisation of position controllers - zero_throttle_and_relax_ac(); - pos_control->set_alt_target_to_current_alt(); - return; + + // get pilot's desired yaw rate (or zero if in radio failsafe) + float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if (!is_zero(target_yaw_rate)) { + pilot_yaw_override = true; } - // process pilot inputs - if (!copter.failsafe.radio) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - if (!is_zero(target_yaw_rate)) { - pilot_yaw_override = true; - } + // get pilot desired climb rate (or zero if in radio failsafe) + float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + // adjust climb rate using rangefinder + if (copter.rangefinder_alt_ok()) { + // if rangefinder is ok, use surface tracking + target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); + } - // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - - // check for pilot requested take-off - if (ap.land_complete && target_climb_rate > 0) { - // indicate we are taking off - set_land_complete(false); - // clear i term when we're taking off - set_throttle_takeoff(); - } + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); + return; } // set motors to full range @@ -81,9 +70,6 @@ void Copter::ModeCircle::run() copter.circle_nav->get_yaw(), true); } - // adjust climb rate using rangefinder - target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); - // update altitude target and call position controller // protects heli's from inflight motor interlock disable if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) { diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 0e79ead597..69aed245a0 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -40,21 +40,9 @@ void Copter::ModeDrift::run() { static float braker = 0.0f; static float roll_input = 0.0f; - float target_roll, target_pitch; - float target_yaw_rate; - - // if landed and throttle at zero, set throttle to zero and exit immediately - if (!motors->armed() || !motors->get_interlock() || (ap.land_complete && ap.throttle_zero)) { - zero_throttle_and_relax_ac(copter.is_tradheli() && motors->get_interlock()); - return; - } - - // clear landing flag above zero throttle - if (!ap.throttle_zero) { - set_land_complete(false); - } // convert pilot input to lean angles + float target_roll, target_pitch; get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // Grab inertial velocity @@ -66,7 +54,7 @@ void Copter::ModeDrift::run() // gain sceduling for Yaw float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); - target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p; + float target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p; roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); @@ -90,8 +78,30 @@ void Copter::ModeDrift::run() braker = 0.0f; } - // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + if (!motors->armed()) { + // Motors should be Stopped + motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + } else if (ap.throttle_zero) { + // Attempting to Land + motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + } else { + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + } + + if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) { + // Motors Stopped + attitude_control->set_yaw_target_to_current_heading(); + attitude_control->reset_rate_controller_I_terms(); + } else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + // Landed + attitude_control->set_yaw_target_to_current_heading(); + attitude_control->reset_rate_controller_I_terms(); + } else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + // clear landing flag above zero throttle + if (!motors->limit.throttle_lower) { + set_land_complete(false); + } + } // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index 09c18c9945..3429958151 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -102,6 +102,9 @@ void Copter::ModeFlip::run() // get pilot's desired throttle float throttle_out = get_pilot_desired_throttle(); + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + // get corrected angle based on direction and axis of rotation // we flip the sign of flip_angle to minimize the code repetition int32_t flip_angle; @@ -209,9 +212,6 @@ void Copter::ModeFlip::run() break; } - // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // output pilot's throttle without angle boost attitude_control->set_throttle_out(throttle_out, false, g.throttle_filt); } diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 7d212a13c3..d18dbf89f7 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -217,7 +217,6 @@ void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stic // should be called at 100hz or more void Copter::ModeFlowHold::run() { - FlowHoldModeState flowhold_state; float takeoff_climb_rate = 0.0f; update_height_estimate(); @@ -241,15 +240,8 @@ void Copter::ModeFlowHold::run() // get pilot's desired yaw rate float target_yaw_rate = copter.get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in()); - if (!copter.motors->armed() || !copter.motors->get_interlock()) { - flowhold_state = FlowHold_MotorStopped; - } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) { - flowhold_state = FlowHold_Takeoff; - } else if (!copter.ap.auto_armed || copter.ap.land_complete) { - flowhold_state = FlowHold_Landed; - } else { - flowhold_state = FlowHold_Flying; - } + // Flow Hold State Machine Determination + AltHoldModeState flowhold_state = get_alt_hold_state(target_climb_rate); if (copter.optflow.healthy()) { const float filter_constant = 0.95; @@ -258,47 +250,19 @@ void Copter::ModeFlowHold::run() quality_filtered = 0; } - Vector2f bf_angles; - - // calculate alt-hold angles - int16_t roll_in = copter.channel_roll->get_control_in(); - int16_t pitch_in = copter.channel_pitch->get_control_in(); - float angle_max = copter.attitude_control->get_althold_lean_angle_max(); - get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y,angle_max, attitude_control->get_althold_lean_angle_max()); - - if (quality_filtered >= flow_min_quality && - AP_HAL::millis() - copter.arm_time_ms > 3000) { - // don't use for first 3s when we are just taking off - Vector2f flow_angles; - - flowhold_flow_to_angle(flow_angles, (roll_in != 0) || (pitch_in != 0)); - flow_angles.x = constrain_float(flow_angles.x, -angle_max/2, angle_max/2); - flow_angles.y = constrain_float(flow_angles.y, -angle_max/2, angle_max/2); - bf_angles += flow_angles; - } - bf_angles.x = constrain_float(bf_angles.x, -angle_max, angle_max); - bf_angles.y = constrain_float(bf_angles.y, -angle_max, angle_max); - // Flow Hold State Machine switch (flowhold_state) { - case FlowHold_MotorStopped: + case AltHold_MotorStopped: copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); - copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); -#if FRAME_CONFIG == HELI_FRAME - // force descent rate and call position controller - copter.pos_control->set_alt_target_from_climb_rate(-abs(copter.g.land_speed), copter.G_Dt, false); -#else copter.attitude_control->reset_rate_controller_I_terms(); copter.attitude_control->set_yaw_target_to_current_heading(); copter.pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero -#endif flow_pi_xy.reset_I(); - copter.pos_control->update_z_controller(); break; - case FlowHold_Takeoff: + case AltHold_Takeoff: // set motors to full range copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); @@ -317,56 +281,66 @@ void Copter::ModeFlowHold::run() // get avoidance adjusted climb rate target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate); - // call attitude controller - copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); - // call position controller copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, copter.G_Dt, false); copter.pos_control->add_takeoff_climb_rate(takeoff_climb_rate, copter.G_Dt); - copter.pos_control->update_z_controller(); break; - case FlowHold_Landed: -#if FRAME_CONFIG == HELI_FRAME - // helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot. - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); -#else - // 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) { - copter.motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); - } else { - copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - } -#endif - copter.attitude_control->reset_rate_controller_I_terms(); - copter.attitude_control->set_yaw_target_to_current_heading(); - copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); - copter.pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero - copter.pos_control->update_z_controller(); + case AltHold_Landed_Ground_Idle: + attitude_control->reset_rate_controller_I_terms(); + attitude_control->set_yaw_target_to_current_heading(); + // FALLTHROUGH + + case AltHold_Landed_Pre_Takeoff: + + pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero break; - case FlowHold_Flying: + case AltHold_Flying: copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); -#if AC_AVOID_ENABLED == ENABLED - // apply avoidance - copter.avoid.adjust_roll_pitch(bf_angles.x, bf_angles.y, copter.aparm.angle_max); -#endif - - // call attitude controller - copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); - // adjust climb rate using rangefinder target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate, copter.pos_control->get_alt_target(), copter.G_Dt); // get avoidance adjusted climb rate target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate); - // call position controller - copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, copter.G_Dt, false); - copter.pos_control->update_z_controller(); + copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); break; } + + // flowhold attitude target calculations + Vector2f bf_angles; + + // calculate alt-hold angles + int16_t roll_in = copter.channel_roll->get_control_in(); + int16_t pitch_in = copter.channel_pitch->get_control_in(); + float angle_max = copter.attitude_control->get_althold_lean_angle_max(); + get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y, angle_max, attitude_control->get_althold_lean_angle_max()); + + if (quality_filtered >= flow_min_quality && + AP_HAL::millis() - copter.arm_time_ms > 3000) { + // don't use for first 3s when we are just taking off + Vector2f flow_angles; + + flowhold_flow_to_angle(flow_angles, (roll_in != 0) || (pitch_in != 0)); + flow_angles.x = constrain_float(flow_angles.x, -angle_max/2, angle_max/2); + flow_angles.y = constrain_float(flow_angles.y, -angle_max/2, angle_max/2); + bf_angles += flow_angles; + } + bf_angles.x = constrain_float(bf_angles.x, -angle_max, angle_max); + bf_angles.y = constrain_float(bf_angles.y, -angle_max, angle_max); + +#if AC_AVOID_ENABLED == ENABLED + // apply avoidance + copter.avoid.adjust_roll_pitch(bf_angles.x, bf_angles.y, copter.aparm.angle_max); +#endif + + // call attitude controller + copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); + + // call z-axis position controller + pos_control->update_z_controller(); } /* diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 2896d26ac0..b27bbb5fb2 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -26,12 +26,15 @@ bool Copter::ModeFollow::init(const bool ignore_checks) void Copter::ModeFollow::run() { - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); return; } + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + // re-use guided mode's velocity controller // Note: this is safe from interference from GCSs and companion computer's whose guided mode // position and velocity requests will be ignored while the vehicle is not in guided mode diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index f4f6c56304..1546451ad4 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -377,13 +377,9 @@ void Copter::ModeGuided::takeoff_run() void Copter::Mode::auto_takeoff_run() { - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - // initialise wpnav targets - wp_nav->shift_wp_origin_to_current_pos(); - zero_throttle_and_relax_ac(); - // clear i term when we're taking off - set_throttle_takeoff(); + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed) { + make_safe_shut_down(); return; } @@ -394,17 +390,12 @@ void Copter::Mode::auto_takeoff_run() target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } -#if FRAME_CONFIG == HELI_FRAME // aircraft stays in landed state until rotor speed runup has finished if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { set_land_complete(false); } else { - // initialise wpnav targets wp_nav->shift_wp_origin_to_current_pos(); } -#else - set_land_complete(false); -#endif // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); @@ -423,12 +414,6 @@ void Copter::Mode::auto_takeoff_run() // called from guided_run void Copter::ModeGuided::pos_control_run() { - // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { - zero_throttle_and_relax_ac(); - return; - } - // process pilot's yaw input float target_yaw_rate = 0; if (!copter.failsafe.radio) { @@ -439,6 +424,12 @@ void Copter::ModeGuided::pos_control_run() } } + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); + return; + } + // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); @@ -465,14 +456,6 @@ void Copter::ModeGuided::pos_control_run() // called from guided_run void Copter::ModeGuided::vel_control_run() { - // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { - // initialise velocity controller - pos_control->init_vel_controller_xyz(); - zero_throttle_and_relax_ac(); - return; - } - // process pilot's yaw input float target_yaw_rate = 0; if (!copter.failsafe.radio) { @@ -483,6 +466,12 @@ void Copter::ModeGuided::vel_control_run() } } + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); + return; + } + // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); @@ -519,15 +508,6 @@ void Copter::ModeGuided::vel_control_run() // called from guided_run void Copter::ModeGuided::posvel_control_run() { - // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { - // set target position and velocity to current position and velocity - pos_control->set_pos_target(inertial_nav.get_position()); - pos_control->set_desired_velocity(Vector3f(0,0,0)); - zero_throttle_and_relax_ac(); - return; - } - // process pilot's yaw input float target_yaw_rate = 0; @@ -539,6 +519,12 @@ void Copter::ModeGuided::posvel_control_run() } } + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); + return; + } + // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); @@ -587,16 +573,6 @@ void Copter::ModeGuided::posvel_control_run() // called from guided_run void Copter::ModeGuided::angle_control_run() { - // if not auto armed or motors not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) { -#if FRAME_CONFIG == HELI_FRAME - attitude_control->set_yaw_target_to_current_heading(); -#endif - zero_throttle_and_relax_ac(); - pos_control->relax_alt_hold_controllers(0.0f); - return; - } - // constrain desired lean angles float roll_in = guided_angle_state.roll_cd; float pitch_in = guided_angle_state.pitch_cd; @@ -627,6 +603,24 @@ void Copter::ModeGuided::angle_control_run() yaw_rate_in = 0.0f; } + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || (ap.land_complete && (guided_angle_state.climb_rate_cms <= 0.0f))) { + make_safe_shut_down(); + return; + } + + // TODO: use get_alt_hold_state + // landed with positive desired climb rate, takeoff + if (ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) { + zero_throttle_and_relax_ac(); + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + set_land_complete(false); + set_throttle_takeoff(); + } + return; + } + // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 8340abf63e..4f5c8d150b 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -53,29 +53,26 @@ void Copter::ModeLand::run() // should be called at 100hz or more void Copter::ModeLand::gps_run() { - // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); + // disarm when the landing detector says we've landed + if (ap.land_complete) { + copter.init_disarm_motors(); + } - // disarm when the landing detector says we've landed - if (ap.land_complete) { - copter.init_disarm_motors(); + // Land State Machine Determination + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); + } else { + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + + // pause before beginning land descent + if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { + land_pause = false; } - return; + + land_run_horizontal_control(); + land_run_vertical_control(land_pause); } - - // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // pause before beginning land descent - if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { - land_pause = false; - } - - land_run_horizontal_control(); - land_run_vertical_control(land_pause); } // land_nogps_run - runs the land controller @@ -106,37 +103,28 @@ void Copter::ModeLand::nogps_run() target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } - // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { -#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw - // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - attitude_control->set_throttle_out(0,false,g.throttle_filt); -#else - motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); - // multicopters do not stabilize roll/pitch/yaw when disarmed - attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt); -#endif - - // disarm when the landing detector says we've landed - if (ap.land_complete) { - copter.init_disarm_motors(); - } - return; + // disarm when the landing detector says we've landed + if (ap.land_complete) { + copter.init_disarm_motors(); } - // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + // Land State Machine Determination + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); + } else { + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + + // pause before beginning land descent + if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { + land_pause = false; + } + + land_run_vertical_control(land_pause); + } // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - - // pause before beginning land descent - if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { - land_pause = false; - } - - land_run_vertical_control(land_pause); } // do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 2b90f200e3..e575e586d1 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -74,8 +74,6 @@ void Copter::ModeLoiter::precision_loiter_xy() // should be called at 100hz or more void Copter::ModeLoiter::run() { - LoiterModeState loiter_state; - float target_roll, target_pitch; float target_yaw_rate = 0.0f; float target_climb_rate = 0.0f; @@ -113,42 +111,22 @@ void Copter::ModeLoiter::run() } // Loiter State Machine Determination - if (!motors->armed() || !motors->get_interlock()) { - loiter_state = Loiter_MotorStopped; - } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) { - loiter_state = Loiter_Takeoff; - } else if (!ap.auto_armed || ap.land_complete) { - loiter_state = Loiter_Landed; - } else { - loiter_state = Loiter_Flying; - } + AltHoldModeState loiter_state = get_alt_hold_state(target_climb_rate); // Loiter State Machine switch (loiter_state) { - case Loiter_MotorStopped: + case AltHold_MotorStopped: - motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); -#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); - if (ap.land_complete_maybe) { - pos_control->relax_alt_hold_controllers(0.0f); - } -#else - loiter_nav->init_target(); 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 - loiter_nav->update(); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); + loiter_nav->init_target(); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); pos_control->update_z_controller(); break; - case Loiter_Takeoff: - // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + case AltHold_Takeoff: // initiate take-off if (!takeoff.running()) { @@ -177,27 +155,21 @@ void Copter::ModeLoiter::run() pos_control->update_z_controller(); break; - case Loiter_Landed: -#if FRAME_CONFIG == HELI_FRAME - // helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot. - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); -#else - // 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_GROUND_IDLE); - } else { - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - } -#endif - loiter_nav->init_target(); + case AltHold_Landed_Ground_Idle: + attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0); + // FALLTHROUGH + + case AltHold_Landed_Pre_Takeoff: + + loiter_nav->init_target(); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); break; - case Loiter_Flying: + case AltHold_Flying: // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); @@ -220,7 +192,6 @@ void Copter::ModeLoiter::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->update_z_controller(); break; diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 927e1e4797..0d48505db5 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -23,6 +23,14 @@ #define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE 1800 // max angle required (in centi-degrees) after which the smooth stick release effect is applied #define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s +// PosHold states +enum PosHoldModeState { + PosHold_MotorStopped, + PosHold_Takeoff, + PosHold_Flying, + PosHold_Landed +}; + // mission state enumeration enum poshold_rp_mode { POSHOLD_PILOT_OVERRIDE=0, // pilot is controlling this axis (i.e. roll or pitch) @@ -34,8 +42,8 @@ enum poshold_rp_mode { }; static struct { - poshold_rp_mode roll_mode : 3; // roll mode: pilot override, brake or loiter - poshold_rp_mode pitch_mode : 3; // pitch mode: pilot override, brake or loiter + poshold_rp_mode roll_mode : 3; // roll mode: pilot override, brake or loiter + poshold_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 @@ -95,16 +103,16 @@ bool Copter::ModePosHold::init(bool ignore_checks) // if landed begin in loiter mode poshold.roll_mode = POSHOLD_LOITER; poshold.pitch_mode = POSHOLD_LOITER; - // set target to current position - // only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); }else{ // if not landed start in pilot override to avoid hard twitch poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; } + // initialise loiter + loiter_nav->clear_pilot_desired_acceleration(); + loiter_nav->init_target(); + // initialise wind_comp each time PosHold is switched on poshold.wind_comp_ef.zero(); poshold.wind_comp_roll = 0; @@ -118,14 +126,10 @@ bool Copter::ModePosHold::init(bool ignore_checks) // should be called at 100hz or more void Copter::ModePosHold::run() { - float target_roll, target_pitch; // pilot's roll and pitch angle inputs - float target_yaw_rate = 0; // pilot desired yaw rate in centi-degrees/sec - float target_climb_rate = 0; // pilot desired climb rate in centimeters/sec - float takeoff_climb_rate = 0.0f; // takeoff induced climb rate + float takeoff_climb_rate = 0.0f; float brake_to_loiter_mix; // mix of brake and loiter controls. 0 = fully brake controls, 1 = fully loiter controls float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls - float vel_fw, vel_right; // vehicle's current velocity in body-frame forward and right directions const Vector3f& vel = inertial_nav.get_velocity(); // initialize vertical speeds and acceleration @@ -133,406 +137,437 @@ void Copter::ModePosHold::run() pos_control->set_max_accel_z(g.pilot_accel_z); loiter_nav->clear_pilot_desired_acceleration(); - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - loiter_nav->init_target(); - zero_throttle_and_relax_ac(copter.is_tradheli() && motors->get_interlock()); - pos_control->relax_alt_hold_controllers(0.0f); - return; - } + // apply SIMPLE mode transform to pilot inputs + update_simple_mode(); - // process pilot inputs - if (!copter.failsafe.radio) { - // apply SIMPLE mode transform to pilot inputs - update_simple_mode(); + // convert pilot input to lean angles + float target_roll, target_pitch; + get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max()); - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + // get pilot's desired yaw rate + float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - // get pilot desired climb rate (for alt-hold mode and take-off) - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); - - // get takeoff adjusted pilot and takeoff climb rates - takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate); - - // check for take-off -#if FRAME_CONFIG == HELI_FRAME - // helicopters are held on the ground until rotor speed runup has finished - if (ap.land_complete && (takeoff.running() || (target_climb_rate > 0.0f && motors->rotor_runup_complete()))) { -#else - if (ap.land_complete && (takeoff.running() || target_climb_rate > 0.0f)) { -#endif - if (!takeoff.running()) { - takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); - } - - // indicate we are taking off - set_land_complete(false); - // clear i term when we're taking off - set_throttle_takeoff(); - } - } + // get pilot desired climb rate (for alt-hold mode and take-off) + float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); // relax loiter target if we might be landed if (ap.land_complete_maybe) { loiter_nav->soften_for_landing(); } - // if landed initialise loiter targets, set throttle to zero and exit - if (ap.land_complete) { -#if FRAME_CONFIG == HELI_FRAME - // helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot. - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); -#else - // 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_GROUND_IDLE); - } else { - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - } -#endif - loiter_nav->init_target(); + // Pos Hold State Machine Determination + AltHoldModeState poshold_state = get_alt_hold_state(target_climb_rate); + + // state machine + switch (poshold_state) { + + case AltHold_MotorStopped: + attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero - pos_control->update_z_controller(); - return; - }else{ - // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max()); + loiter_nav->init_target(); + loiter_nav->update(); - // convert inertial nav earth-frame velocities to body-frame - // To-Do: move this to AP_Math (or perhaps we already have a function to do this) - vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw(); - vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw(); - - // If not in LOITER, retrieve latest wind compensation lean angles related to current yaw - if (poshold.roll_mode != POSHOLD_LOITER || poshold.pitch_mode != POSHOLD_LOITER) - poshold_get_wind_comp_lean_angles(poshold.wind_comp_roll, poshold.wind_comp_pitch); + // set poshold state to pilot override + poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; + poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; + break; - // Roll state machine - // Each state (aka mode) is responsible for: - // 1. dealing with pilot input - // 2. calculating the final roll output to the attitude controller - // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state - switch (poshold.roll_mode) { + case AltHold_Takeoff: - case POSHOLD_PILOT_OVERRIDE: - // update pilot desired roll angle using latest radio input - // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); - - // switch to BRAKE mode for next iteration if no pilot input - if (is_zero(target_roll) && (fabsf(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) { - // initialise BRAKE mode - poshold.roll_mode = POSHOLD_BRAKE; // Set brake roll mode - poshold.brake_roll = 0; // initialise braking angle to zero - poshold.brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking - poshold.brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. - poshold.braking_time_updated_roll = false; // flag the braking time can be re-estimated - } - - // final lean angle should be pilot input plus wind compensation - poshold.roll = poshold.pilot_roll + poshold.wind_comp_roll; - break; - - case POSHOLD_BRAKE: - case POSHOLD_BRAKE_READY_TO_LOITER: - // calculate brake_roll angle to counter-act velocity - poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); - - // update braking time estimate - if (!poshold.braking_time_updated_roll) { - // check if brake angle is increasing - if (abs(poshold.brake_roll) >= poshold.brake_angle_max_roll) { - poshold.brake_angle_max_roll = abs(poshold.brake_roll); - } else { - // braking angle has started decreasing so re-estimate braking time - poshold.brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. - poshold.braking_time_updated_roll = true; - } - } - - // if velocity is very low reduce braking time to 0.5seconds - if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_roll > 50*LOOP_RATE_FACTOR)) { - poshold.brake_timeout_roll = 50*LOOP_RATE_FACTOR; - } - - // reduce braking timer - if (poshold.brake_timeout_roll > 0) { - poshold.brake_timeout_roll--; - } else { - // indicate that we are ready to move to Loiter. - // Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER - // logic for engaging loiter is handled below the roll and pitch mode switch statements - poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; - } - - // final lean angle is braking angle + wind compensation angle - poshold.roll = poshold.brake_roll + poshold.wind_comp_roll; - - // check for pilot input - if (!is_zero(target_roll)) { - // init transition to pilot override - poshold_roll_controller_to_pilot_override(); - } - break; - - case POSHOLD_BRAKE_TO_LOITER: - case POSHOLD_LOITER: - // these modes are combined roll-pitch modes and are handled below - break; - - case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: - // update pilot desired roll angle using latest radio input - // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); - - // count-down loiter to pilot timer - if (poshold.controller_to_pilot_timer_roll > 0) { - poshold.controller_to_pilot_timer_roll--; - } else { - // when timer runs out switch to full pilot override for next iteration - poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; - } - - // calculate controller_to_pilot mix ratio - controller_to_pilot_roll_mix = (float)poshold.controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; - - // mix final loiter lean angle and pilot desired lean angles - poshold.roll = poshold_mix_controls(controller_to_pilot_roll_mix, poshold.controller_final_roll, poshold.pilot_roll + poshold.wind_comp_roll); - break; + // initiate take-off + if (!takeoff.running()) { + takeoff.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(); } - // Pitch state machine - // Each state (aka mode) is responsible for: - // 1. dealing with pilot input - // 2. calculating the final pitch output to the attitude contpitcher - // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state - switch (poshold.pitch_mode) { - - case POSHOLD_PILOT_OVERRIDE: - // update pilot desired pitch angle using latest radio input - // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); - - // switch to BRAKE mode for next iteration if no pilot input - if (is_zero(target_pitch) && (fabsf(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) { - // initialise BRAKE mode - poshold.pitch_mode = POSHOLD_BRAKE; // set brake pitch mode - poshold.brake_pitch = 0; // initialise braking angle to zero - poshold.brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking - poshold.brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. - poshold.braking_time_updated_pitch = false; // flag the braking time can be re-estimated - } - - // final lean angle should be pilot input plus wind compensation - poshold.pitch = poshold.pilot_pitch + poshold.wind_comp_pitch; - break; - - case POSHOLD_BRAKE: - case POSHOLD_BRAKE_READY_TO_LOITER: - // calculate brake_pitch angle to counter-act velocity - poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); - - // update braking time estimate - if (!poshold.braking_time_updated_pitch) { - // check if brake angle is increasing - if (abs(poshold.brake_pitch) >= poshold.brake_angle_max_pitch) { - poshold.brake_angle_max_pitch = abs(poshold.brake_pitch); - } else { - // braking angle has started decreasing so re-estimate braking time - poshold.brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. - poshold.braking_time_updated_pitch = true; - } - } - - // if velocity is very low reduce braking time to 0.5seconds - if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) { - poshold.brake_timeout_pitch = 50*LOOP_RATE_FACTOR; - } - - // reduce braking timer - if (poshold.brake_timeout_pitch > 0) { - poshold.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 POSHOLD_BRAKE_READY_TO_LOITER - // logic for engaging loiter is handled below the pitch and pitch mode switch statements - poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; - } - - // final lean angle is braking angle + wind compensation angle - poshold.pitch = poshold.brake_pitch + poshold.wind_comp_pitch; - - // check for pilot input - if (!is_zero(target_pitch)) { - // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); - } - break; - - case POSHOLD_BRAKE_TO_LOITER: - case POSHOLD_LOITER: - // these modes are combined pitch-pitch modes and are handled below - break; - - case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: - // update pilot desired pitch angle using latest radio input - // this filters the input so that it returns to zero no faster than the brake-rate - poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); - - // count-down loiter to pilot timer - if (poshold.controller_to_pilot_timer_pitch > 0) { - poshold.controller_to_pilot_timer_pitch--; - } else { - // when timer runs out switch to full pilot override for next iteration - poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; - } - - // calculate controller_to_pilot mix ratio - controller_to_pilot_pitch_mix = (float)poshold.controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; - - // mix final loiter lean angle and pilot desired lean angles - poshold.pitch = poshold_mix_controls(controller_to_pilot_pitch_mix, poshold.controller_final_pitch, poshold.pilot_pitch + poshold.wind_comp_pitch); - break; - } - - // set motors to full range - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // - // Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER) - // - - // switch into LOITER mode when both roll and pitch are ready - if (poshold.roll_mode == POSHOLD_BRAKE_READY_TO_LOITER && poshold.pitch_mode == POSHOLD_BRAKE_READY_TO_LOITER) { - poshold.roll_mode = POSHOLD_BRAKE_TO_LOITER; - poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER; - poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER; - // init loiter controller - loiter_nav->init_target(inertial_nav.get_position()); - // set delay to start of wind compensation estimate updates - poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER; - } - - // roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes - if (poshold.roll_mode == POSHOLD_BRAKE_TO_LOITER || poshold.roll_mode == POSHOLD_LOITER) { - - // force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states) - poshold.pitch_mode = poshold.roll_mode; - - // handle combined roll+pitch mode - switch (poshold.roll_mode) { - case POSHOLD_BRAKE_TO_LOITER: - // reduce brake_to_loiter timer - if (poshold.brake_to_loiter_timer > 0) { - poshold.brake_to_loiter_timer--; - } else { - // progress to full loiter on next iteration - poshold.roll_mode = POSHOLD_LOITER; - poshold.pitch_mode = POSHOLD_LOITER; - } - - // calculate percentage mix of loiter and brake control - brake_to_loiter_mix = (float)poshold.brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER; - - // calculate brake_roll and pitch angles to counter-act velocity - poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); - poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); - - // run loiter controller - loiter_nav->update(); - - // calculate final roll and pitch output by mixing loiter and brake controls - poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, loiter_nav->get_roll()); - poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, loiter_nav->get_pitch()); - - // check for pilot input - if (!is_zero(target_roll) || !is_zero(target_pitch)) { - // if roll input switch to pilot override for roll - if (!is_zero(target_roll)) { - // init transition to pilot override - poshold_roll_controller_to_pilot_override(); - // switch pitch-mode to brake (but ready to go back to loiter anytime) - // no need to reset poshold.brake_pitch here as wind comp has not been updated since last brake_pitch computation - poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; - } - // if pitch input switch to pilot override for pitch - if (!is_zero(target_pitch)) { - // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); - if (is_zero(target_roll)) { - // switch roll-mode to brake (but ready to go back to loiter anytime) - // no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation - poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; - } - } - } - break; - - case POSHOLD_LOITER: - // run loiter controller - loiter_nav->update(); - - // set roll angle based on loiter controller outputs - poshold.roll = loiter_nav->get_roll(); - poshold.pitch = loiter_nav->get_pitch(); - - // update wind compensation estimate - poshold_update_wind_comp_estimate(); - - // check for pilot input - if (!is_zero(target_roll) || !is_zero(target_pitch)) { - // if roll input switch to pilot override for roll - if (!is_zero(target_roll)) { - // init transition to pilot override - poshold_roll_controller_to_pilot_override(); - // switch pitch-mode to brake (but ready to go back to loiter anytime) - poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; - // reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle - poshold.brake_pitch = 0; - } - // if pitch input switch to pilot override for pitch - if (!is_zero(target_pitch)) { - // init transition to pilot override - poshold_pitch_controller_to_pilot_override(); - // if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time) - if (is_zero(target_roll)) { - poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; - poshold.brake_roll = 0; - } - } - } - break; - - default: - // do nothing for uncombined roll and pitch modes - break; - } - } - - // constrain target pitch/roll angles - float angle_max = copter.aparm.angle_max; - poshold.roll = constrain_int16(poshold.roll, -angle_max, angle_max); - poshold.pitch = constrain_int16(poshold.pitch, -angle_max, angle_max); - - // update attitude controller targets - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate); - - // adjust climb rate using rangefinder - target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); + // get take-off adjusted pilot and takeoff climb rates + takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // update altitude target and call position controller + // init and update loiter although pilot is controlling lean angles + loiter_nav->init_target(); + loiter_nav->update(); + + // set position controller targets 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(); + + // set poshold state to pilot override + poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; + poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; + break; + + case AltHold_Landed_Ground_Idle: + + loiter_nav->init_target(); + loiter_nav->update(); + attitude_control->reset_rate_controller_I_terms(); + attitude_control->set_yaw_target_to_current_heading(); + // FALLTHROUGH + + case AltHold_Landed_Pre_Takeoff: + + pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + + // set poshold state to pilot override + poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; + poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; + break; + + case AltHold_Flying: + + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + +#if AC_AVOID_ENABLED == ENABLED + // apply avoidance + copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max); +#endif + + // adjust climb rate using rangefinder + if (copter.rangefinder_alt_ok()) { + // if rangefinder is ok, use surface tracking + target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); + } + + // get avoidance adjusted climb rate + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); + + pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + break; } + + // poshold specific behaviour to calculate desired roll, pitch angles + // convert inertial nav earth-frame velocities to body-frame + // To-Do: move this to AP_Math (or perhaps we already have a function to do this) + float vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw(); + float vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw(); + + // If not in LOITER, retrieve latest wind compensation lean angles related to current yaw + if (poshold.roll_mode != POSHOLD_LOITER || poshold.pitch_mode != POSHOLD_LOITER) { + poshold_get_wind_comp_lean_angles(poshold.wind_comp_roll, poshold.wind_comp_pitch); + } + + // Roll state machine + // Each state (aka mode) is responsible for: + // 1. dealing with pilot input + // 2. calculating the final roll output to the attitude controller + // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state + switch (poshold.roll_mode) { + + case POSHOLD_PILOT_OVERRIDE: + // update pilot desired roll angle using latest radio input + // this filters the input so that it returns to zero no faster than the brake-rate + poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); + + // switch to BRAKE mode for next iteration if no pilot input + if (is_zero(target_roll) && (fabsf(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) { + // initialise BRAKE mode + poshold.roll_mode = POSHOLD_BRAKE; // Set brake roll mode + poshold.brake_roll = 0; // initialise braking angle to zero + poshold.brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking + poshold.brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. + poshold.braking_time_updated_roll = false; // flag the braking time can be re-estimated + } + + // final lean angle should be pilot input plus wind compensation + poshold.roll = poshold.pilot_roll + poshold.wind_comp_roll; + break; + + case POSHOLD_BRAKE: + case POSHOLD_BRAKE_READY_TO_LOITER: + // calculate brake_roll angle to counter-act velocity + poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); + + // update braking time estimate + if (!poshold.braking_time_updated_roll) { + // check if brake angle is increasing + if (abs(poshold.brake_roll) >= poshold.brake_angle_max_roll) { + poshold.brake_angle_max_roll = abs(poshold.brake_roll); + } else { + // braking angle has started decreasing so re-estimate braking time + poshold.brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. + poshold.braking_time_updated_roll = true; + } + } + + // if velocity is very low reduce braking time to 0.5seconds + if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_roll > 50*LOOP_RATE_FACTOR)) { + poshold.brake_timeout_roll = 50*LOOP_RATE_FACTOR; + } + + // reduce braking timer + if (poshold.brake_timeout_roll > 0) { + poshold.brake_timeout_roll--; + } else { + // indicate that we are ready to move to Loiter. + // Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER + // logic for engaging loiter is handled below the roll and pitch mode switch statements + poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; + } + + // final lean angle is braking angle + wind compensation angle + poshold.roll = poshold.brake_roll + poshold.wind_comp_roll; + + // check for pilot input + if (!is_zero(target_roll)) { + // init transition to pilot override + poshold_roll_controller_to_pilot_override(); + } + break; + + case POSHOLD_BRAKE_TO_LOITER: + case POSHOLD_LOITER: + // these modes are combined roll-pitch modes and are handled below + break; + + case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: + // update pilot desired roll angle using latest radio input + // this filters the input so that it returns to zero no faster than the brake-rate + poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); + + // count-down loiter to pilot timer + if (poshold.controller_to_pilot_timer_roll > 0) { + poshold.controller_to_pilot_timer_roll--; + } else { + // when timer runs out switch to full pilot override for next iteration + poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; + } + + // calculate controller_to_pilot mix ratio + controller_to_pilot_roll_mix = (float)poshold.controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; + + // mix final loiter lean angle and pilot desired lean angles + poshold.roll = poshold_mix_controls(controller_to_pilot_roll_mix, poshold.controller_final_roll, poshold.pilot_roll + poshold.wind_comp_roll); + break; + } + + // Pitch state machine + // Each state (aka mode) is responsible for: + // 1. dealing with pilot input + // 2. calculating the final pitch output to the attitude contpitcher + // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state + switch (poshold.pitch_mode) { + + case POSHOLD_PILOT_OVERRIDE: + // update pilot desired pitch angle using latest radio input + // this filters the input so that it returns to zero no faster than the brake-rate + poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); + + // switch to BRAKE mode for next iteration if no pilot input + if (is_zero(target_pitch) && (fabsf(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) { + // initialise BRAKE mode + poshold.pitch_mode = POSHOLD_BRAKE; // set brake pitch mode + poshold.brake_pitch = 0; // initialise braking angle to zero + poshold.brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking + poshold.brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. + poshold.braking_time_updated_pitch = false; // flag the braking time can be re-estimated + } + + // final lean angle should be pilot input plus wind compensation + poshold.pitch = poshold.pilot_pitch + poshold.wind_comp_pitch; + break; + + case POSHOLD_BRAKE: + case POSHOLD_BRAKE_READY_TO_LOITER: + // calculate brake_pitch angle to counter-act velocity + poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); + + // update braking time estimate + if (!poshold.braking_time_updated_pitch) { + // check if brake angle is increasing + if (abs(poshold.brake_pitch) >= poshold.brake_angle_max_pitch) { + poshold.brake_angle_max_pitch = abs(poshold.brake_pitch); + } else { + // braking angle has started decreasing so re-estimate braking time + poshold.brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. + poshold.braking_time_updated_pitch = true; + } + } + + // if velocity is very low reduce braking time to 0.5seconds + if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) { + poshold.brake_timeout_pitch = 50*LOOP_RATE_FACTOR; + } + + // reduce braking timer + if (poshold.brake_timeout_pitch > 0) { + poshold.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 POSHOLD_BRAKE_READY_TO_LOITER + // logic for engaging loiter is handled below the pitch and pitch mode switch statements + poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; + } + + // final lean angle is braking angle + wind compensation angle + poshold.pitch = poshold.brake_pitch + poshold.wind_comp_pitch; + + // check for pilot input + if (!is_zero(target_pitch)) { + // init transition to pilot override + poshold_pitch_controller_to_pilot_override(); + } + break; + + case POSHOLD_BRAKE_TO_LOITER: + case POSHOLD_LOITER: + // these modes are combined pitch-pitch modes and are handled below + break; + + case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: + // update pilot desired pitch angle using latest radio input + // this filters the input so that it returns to zero no faster than the brake-rate + poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); + + // count-down loiter to pilot timer + if (poshold.controller_to_pilot_timer_pitch > 0) { + poshold.controller_to_pilot_timer_pitch--; + } else { + // when timer runs out switch to full pilot override for next iteration + poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; + } + + // calculate controller_to_pilot mix ratio + controller_to_pilot_pitch_mix = (float)poshold.controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; + + // mix final loiter lean angle and pilot desired lean angles + poshold.pitch = poshold_mix_controls(controller_to_pilot_pitch_mix, poshold.controller_final_pitch, poshold.pilot_pitch + poshold.wind_comp_pitch); + break; + } + + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + + // + // Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER) + // + + // switch into LOITER mode when both roll and pitch are ready + if (poshold.roll_mode == POSHOLD_BRAKE_READY_TO_LOITER && poshold.pitch_mode == POSHOLD_BRAKE_READY_TO_LOITER) { + poshold.roll_mode = POSHOLD_BRAKE_TO_LOITER; + poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER; + poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER; + // init loiter controller + loiter_nav->init_target(inertial_nav.get_position()); + // set delay to start of wind compensation estimate updates + poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER; + } + + // roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes + if (poshold.roll_mode == POSHOLD_BRAKE_TO_LOITER || poshold.roll_mode == POSHOLD_LOITER) { + + // force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states) + poshold.pitch_mode = poshold.roll_mode; + + // handle combined roll+pitch mode + switch (poshold.roll_mode) { + case POSHOLD_BRAKE_TO_LOITER: + // reduce brake_to_loiter timer + if (poshold.brake_to_loiter_timer > 0) { + poshold.brake_to_loiter_timer--; + } else { + // progress to full loiter on next iteration + poshold.roll_mode = POSHOLD_LOITER; + poshold.pitch_mode = POSHOLD_LOITER; + } + + // calculate percentage mix of loiter and brake control + brake_to_loiter_mix = (float)poshold.brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER; + + // calculate brake_roll and pitch angles to counter-act velocity + poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); + poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); + + // run loiter controller + loiter_nav->update(); + + // calculate final roll and pitch output by mixing loiter and brake controls + poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, loiter_nav->get_roll()); + poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, loiter_nav->get_pitch()); + + // check for pilot input + if (!is_zero(target_roll) || !is_zero(target_pitch)) { + // if roll input switch to pilot override for roll + if (!is_zero(target_roll)) { + // init transition to pilot override + poshold_roll_controller_to_pilot_override(); + // switch pitch-mode to brake (but ready to go back to loiter anytime) + // no need to reset poshold.brake_pitch here as wind comp has not been updated since last brake_pitch computation + poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; + } + // if pitch input switch to pilot override for pitch + if (!is_zero(target_pitch)) { + // init transition to pilot override + poshold_pitch_controller_to_pilot_override(); + if (is_zero(target_roll)) { + // switch roll-mode to brake (but ready to go back to loiter anytime) + // no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation + poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; + } + } + } + break; + + case POSHOLD_LOITER: + // run loiter controller + loiter_nav->update(); + + // set roll angle based on loiter controller outputs + poshold.roll = loiter_nav->get_roll(); + poshold.pitch = loiter_nav->get_pitch(); + + // update wind compensation estimate + poshold_update_wind_comp_estimate(); + + // check for pilot input + if (!is_zero(target_roll) || !is_zero(target_pitch)) { + // if roll input switch to pilot override for roll + if (!is_zero(target_roll)) { + // init transition to pilot override + poshold_roll_controller_to_pilot_override(); + // switch pitch-mode to brake (but ready to go back to loiter anytime) + poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; + // reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle + poshold.brake_pitch = 0; + } + // if pitch input switch to pilot override for pitch + if (!is_zero(target_pitch)) { + // init transition to pilot override + poshold_pitch_controller_to_pilot_override(); + // if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time) + if (is_zero(target_roll)) { + poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; + poshold.brake_roll = 0; + } + // if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time) + } + } + break; + + default: + // do nothing for uncombined roll and pitch modes + break; + } + } + + // constrain target pitch/roll angles + float angle_max = copter.aparm.angle_max; + poshold.roll = constrain_int16(poshold.roll, -angle_max, angle_max); + poshold.pitch = constrain_int16(poshold.pitch, -angle_max, angle_max); + + // call attitude controller + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate); + + // call z-axis position controller + pos_control->update_z_controller(); } // poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 10ebd4c2b5..c4a74296b8 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -127,10 +127,9 @@ void Copter::ModeRTL::return_start() // called by rtl_run at 100hz or more void Copter::ModeRTL::climb_return_run() { - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); - // To-Do: re-initialise wpnav targets + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); return; } @@ -185,10 +184,9 @@ void Copter::ModeRTL::loiterathome_start() // called by rtl_run at 100hz or more void Copter::ModeRTL::loiterathome_run() { - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); - // To-Do: re-initialise wpnav targets + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); return; } @@ -258,12 +256,9 @@ void Copter::ModeRTL::descent_run() float target_pitch = 0.0f; float target_yaw_rate = 0.0f; - // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); - // set target to current position - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); return; } @@ -358,20 +353,17 @@ bool Copter::ModeRTL::landing_gear_should_be_deployed() const // called by rtl_run at 100hz or more void Copter::ModeRTL::land_run(bool disarm_on_land) { - // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); - // set target to current position - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); + // check if we've completed this stage of RTL + _state_complete = ap.land_complete; - // disarm when the landing detector says we've landed - if (ap.land_complete && disarm_on_land) { - copter.init_disarm_motors(); - } + // disarm when the landing detector says we've landed + if (disarm_on_land && ap.land_complete) { + copter.init_disarm_motors(); + } - // check if we've completed this stage of RTL - _state_complete = ap.land_complete; + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + make_safe_shut_down(); return; } @@ -380,9 +372,6 @@ void Copter::ModeRTL::land_run(bool disarm_on_land) land_run_horizontal_control(); land_run_vertical_control(); - - // check if we've completed this stage of RTL - _state_complete = ap.land_complete; } void Copter::ModeRTL::build_path(bool terrain_following_allowed) diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 76a079812f..6277ba72ff 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -26,7 +26,6 @@ bool Copter::ModeSport::init(bool ignore_checks) // should be called at 100hz or more void Copter::ModeSport::run() { - SportModeState sport_state; float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration @@ -73,39 +72,20 @@ void Copter::ModeSport::run() float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); - // State Machine Determination - if (!motors->armed() || !motors->get_interlock()) { - sport_state = Sport_MotorStopped; - } else if (takeoff.running() || takeoff.triggered(target_climb_rate)) { - sport_state = Sport_Takeoff; - } else if (!ap.auto_armed || ap.land_complete) { - sport_state = Sport_Landed; - } else { - sport_state = Sport_Flying; - } + // Sport State Machine Determination + AltHoldModeState sport_state = get_alt_hold_state(target_climb_rate); // State Machine switch (sport_state) { - case Sport_MotorStopped: + case AltHold_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); + case AltHold_Takeoff: // initiate take-off if (!takeoff.running()) { @@ -122,38 +102,23 @@ void Copter::ModeSport::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // 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: -#if FRAME_CONFIG == HELI_FRAME - // helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot. - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); -#else - // 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_GROUND_IDLE); - } else { - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - } -#endif + case AltHold_Landed_Ground_Idle: 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); + // FALLTHROUGH + + case AltHold_Landed_Pre_Takeoff: + pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero - pos_control->update_z_controller(); break; - case Sport_Flying: + case AltHold_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); // adjust climb rate using rangefinder target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); @@ -161,11 +126,15 @@ void Copter::ModeSport::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - // call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control->update_z_controller(); break; } + + // call attitude controller + attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); + + // call z-axis position controller + pos_control->update_z_controller(); } #endif diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index 8e95376ee6..f06dd433f4 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -8,28 +8,40 @@ // should be called at 100hz or more void Copter::ModeStabilize::run() { - float target_roll, target_pitch; - float target_yaw_rate; - - // if not armed set throttle to zero and exit immediately - if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) { - zero_throttle_and_relax_ac(); - return; - } - - // clear landing flag - set_land_complete(false); - - motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - - // apply SIMPLE mode transform to pilot inputs + // apply simple mode transform to pilot inputs update_simple_mode(); // convert pilot input to lean angles + float target_roll, target_pitch; get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // 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()); + + if (!motors->armed()) { + // Motors should be Stopped + motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); + } else if (ap.throttle_zero) { + // Attempting to Land + motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + } else { + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + } + + if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) { + // Motors Stopped + attitude_control->set_yaw_target_to_current_heading(); + attitude_control->reset_rate_controller_I_terms(); + } else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) { + // Landed + attitude_control->set_yaw_target_to_current_heading(); + attitude_control->reset_rate_controller_I_terms(); + } else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) { + // clear landing flag above zero throttle + if (!motors->limit.throttle_lower) { + set_land_complete(false); + } + } // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 526538cbb5..782067d461 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -35,7 +35,7 @@ void Copter::ModeThrow::run() */ // Don't enter THROW mode if interlock will prevent motors running - if (!motors->armed() && motors->get_interlock()) { + if (!motors->armed()) { // state machine entry is always from a disarmed state stage = Throw_Disarmed; @@ -116,7 +116,7 @@ void Copter::ModeThrow::run() } // demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller - attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt); + attitude_control->set_throttle_out(0,true,g.throttle_filt); break; case Throw_Detecting: @@ -129,7 +129,7 @@ void Copter::ModeThrow::run() } // Hold throttle at zero during the throw and continually reset the attitude controller - attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt); + attitude_control->set_throttle_out(0,true,g.throttle_filt); // Play the waiting for throw tone sequence to alert the user AP_Notify::flags.waiting_for_throw = true;