Copter: add turtle mode safety features.

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;
 };

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();