diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 57baff28d4..6ed6135991 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1641,6 +1641,9 @@ protected: const char *name4() const override { return "TRTL"; } private: + void arm_motors(); + void disarm_motors(); + float motors_output; Vector2f motors_input; }; diff --git a/ArduCopter/mode_turtle.cpp b/ArduCopter/mode_turtle.cpp index 4c896ea925..2e0aef1c67 100644 --- a/ArduCopter/mode_turtle.cpp +++ b/ArduCopter/mode_turtle.cpp @@ -18,12 +18,29 @@ bool ModeTurtle::init(bool ignore_checks) return false; } - // do not enter the mode if sticks are not centered + // do not enter the mode if sticks are not centered or throttle is not at zero if (!is_zero(channel_pitch->norm_input_dz()) || !is_zero(channel_roll->norm_input_dz()) - || !is_zero(channel_yaw->norm_input_dz())) { + || !is_zero(channel_yaw->norm_input_dz()) + || !is_zero(channel_throttle->norm_input_dz())) { return false; } + + // turn on notify leds + AP_Notify::flags.esc_calibration = true; + + return true; +} + +void ModeTurtle::arm_motors() +{ + if (hal.util->get_soft_armed()) { + return; + } + + // stop the spoolup block activating + motors->set_spoolup_block(false); + // reverse the motors hal.rcout->disable_channel_mask_updates(); change_motor_direction(true); @@ -36,8 +53,6 @@ bool ModeTurtle::init(bool ignore_checks) // arm motors->armed(true); hal.util->set_soft_armed(true); - - return true; } bool ModeTurtle::allows_arming(AP_Arming::Method method) const @@ -47,6 +62,18 @@ bool ModeTurtle::allows_arming(AP_Arming::Method method) const void ModeTurtle::exit() { + disarm_motors(); + + // turn off notify leds + AP_Notify::flags.esc_calibration = false; +} + +void ModeTurtle::disarm_motors() +{ + if (!hal.util->get_soft_armed()) { + return; + } + // disarm motors->armed(false); hal.util->set_soft_armed(false); @@ -142,6 +169,14 @@ void ModeTurtle::run() // actually write values to the motors void ModeTurtle::output_to_motors() { + // throttle needs to be raised + if (is_zero(channel_throttle->norm_input_dz())) { + disarm_motors(); + return; + } + + arm_motors(); + // check if motor are allowed to spin const bool allow_output = motors->armed() && motors->get_interlock();