From b23511626243f63d6c760d6c5c77770946aed362 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Thu, 27 Jan 2022 15:36:21 +0000
Subject: [PATCH] Copter: mode_turtle: check motors armed and interlocked

---
 ArduCopter/mode_turtle.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/mode_turtle.cpp b/ArduCopter/mode_turtle.cpp
index 7324934de8..552bcad266 100644
--- a/ArduCopter/mode_turtle.cpp
+++ b/ArduCopter/mode_turtle.cpp
@@ -142,6 +142,9 @@ void ModeTurtle::run()
 // actually write values to the motors
 void ModeTurtle::output_to_motors()
 {
+    // check if motor are allowed to spin
+    const bool allow_output = motors->armed() && motors->get_interlock();
+
     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
         if (!motors->is_motor_enabled(i)) {
             continue;
@@ -149,7 +152,7 @@ void ModeTurtle::output_to_motors()
 
         const Vector2f output{motors->get_roll_factor(i), motors->get_pitch_factor(i)};
         // if output aligns with input then use this motor
-        if ((motors_input - output).length() > 0.5) {
+        if (!allow_output || (motors_input - output).length() > 0.5) {
             motors->rc_write(i, motors->get_pwm_output_min());
             continue;
         }