From 65ed77e8034a0071b20a3d2eca31b45637096e86 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 23 Aug 2014 23:39:36 +0900
Subject: [PATCH] Copter: ensure motors don't stop during flip

---
 ArduCopter/control_flip.pde | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/ArduCopter/control_flip.pde b/ArduCopter/control_flip.pde
index f1ac639c0..c0f2309c0 100644
--- a/ArduCopter/control_flip.pde
+++ b/ArduCopter/control_flip.pde
@@ -141,7 +141,9 @@ static void flip_run()
         // between 45deg ~ -90deg request 400deg/sec roll
         attitude_control.rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0);
         // decrease throttle
-        throttle_out -= FLIP_THR_DEC;
+        if (throttle_out >= g.throttle_min) {
+            throttle_out = max(throttle_out - FLIP_THR_DEC, g.throttle_min);
+        }
 
         // beyond -90deg move on to recovery
         if ((flip_angle < 4500) && (flip_angle > -9000)) {
@@ -153,7 +155,9 @@ static void flip_run()
         // between 45deg ~ -90deg request 400deg/sec pitch
         attitude_control.rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
         // decrease throttle
-        throttle_out -= FLIP_THR_DEC;
+        if (throttle_out >= g.throttle_min) {
+            throttle_out = max(throttle_out - FLIP_THR_DEC, g.throttle_min);
+        }
 
         // check roll for inversion
         if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) {
@@ -165,7 +169,9 @@ static void flip_run()
         // between 45deg ~ -90deg request 400deg/sec pitch
         attitude_control.rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
         // decrease throttle
-        throttle_out -= FLIP_THR_DEC;
+        if (throttle_out >= g.throttle_min) {
+            throttle_out = max(throttle_out - FLIP_THR_DEC, g.throttle_min);
+        }
 
         // check roll for inversion
         if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) {
-- 
GitLab