diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index af3519628e..969d498f18 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -184,6 +184,7 @@ public: // pause and resume a mode virtual bool pause() { return false; }; virtual bool resume() { return false; }; + void make_safe_ground_handling(bool force_throttle_unlimited = false); // true if weathervaning is allowed in the current mode #if WEATHERVANE_ENABLED == ENABLED @@ -196,7 +197,6 @@ protected: bool is_disarmed_or_landed() const; void zero_throttle_and_relax_ac(bool spool_up = false); void zero_throttle_and_hold_attitude(); - void make_safe_ground_handling(bool force_throttle_unlimited = false); // Return stopping point as a location with above origin alt frame Location get_stopping_point() const; diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 0a7da76b07..d7d0d2e470 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -38,30 +38,20 @@ void AutoTune::run() // apply SIMPLE mode transform to pilot inputs copter.update_simple_mode(); - // reset target lean angles and heading while landed - if (copter.ap.land_complete) { - // we are landed, shut down - float target_climb_rate = get_pilot_desired_climb_rate_cms(); - - // 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::DesiredSpoolState::GROUND_IDLE); - } else { - copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - } - copter.attitude_control->reset_rate_controller_I_terms_smoothly(); - copter.attitude_control->reset_yaw_target_and_rate(); - - float target_roll, target_pitch, target_yaw_rate; - get_pilot_desired_rp_yrate_cd(target_roll, target_pitch, target_yaw_rate); - - copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - copter.pos_control->relax_z_controller(0.0f); - copter.pos_control->update_z_controller(); - } else { - // run autotune mode - AC_AutoTune::run(); + // disarm when the landing detector says we've landed and spool state is ground idle + if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { + copter.arming.disarm(AP_Arming::Method::LANDED); } + + // if not armed set throttle to zero and exit immediately + if (copter.ap.land_complete) { + copter.flightmode->make_safe_ground_handling(); + return; + } + + // run autotune mode + AC_AutoTune::run(); + }