From 277755a719bba1e4bc20cbbf0690b2bb2c570f10 Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Wed, 5 Jul 2017 09:49:13 +0200
Subject: [PATCH] Separate 3D state checks from mixTable() function for
 readability

---
 src/main/flight/mixer.c | 36 ++++++++++++++++++++++++++++++------
 src/main/flight/mixer.h |  8 ++++++++
 2 files changed, 38 insertions(+), 6 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index bba155a497..af1f85fce0 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -497,6 +497,23 @@ void stopPwmAllMotors(void)
     delayMicroseconds(1500);
 }
 
+throttle3Dstate_e get3DState(const uint16_t throttlePrevious)
+{
+    throttle3Dstate_e throttle3Dstate;
+
+    if((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) {
+        throttle3Dstate = INVERTED;
+    } else if(rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) {
+        throttle3Dstate = NORMAL;
+    } else if((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) {
+        throttle3Dstate = INVERTED_TO_DEADBAND;
+    } else {
+        throttle3Dstate = NORMAL_TO_DEADBAND;
+    }
+
+    return throttle3Dstate;
+}
+
 void mixTable(pidProfile_t *pidProfile)
 {
     // Scale roll/pitch/yaw uniformly to fit within throttle range
@@ -510,26 +527,33 @@ void mixTable(pidProfile_t *pidProfile)
     if (feature(FEATURE_3D)) {
         if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
 
-        if ((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Out of band handling
+        throttle3Dstate_e flight3DState = get3DState(throttlePrevious);
+
+        switch(flight3DState) {
+        case(INVERTED):
             motorOutputMax = deadbandMotor3dLow;
             motorOutputMin = motorOutputLow;
             throttlePrevious = rcCommand[THROTTLE];
             throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
             currentThrottleInputRange = rcCommandThrottleRange3dLow;
-            if (isMotorProtocolDshot()) mixerInversion = true;
-        } else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // Positive handling
+            if(isMotorProtocolDshot()) mixerInversion = true;
+            break;
+        case(NORMAL):
             motorOutputMax = motorOutputHigh;
             motorOutputMin = deadbandMotor3dHigh;
             throttlePrevious = rcCommand[THROTTLE];
             throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
             currentThrottleInputRange = rcCommandThrottleRange3dHigh;
-        } else if ((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)))  { // Deadband handling from negative to positive
+            break;
+        case(INVERTED_TO_DEADBAND):
             motorOutputMax = deadbandMotor3dLow;
             motorOutputMin = motorOutputLow;
             throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
             currentThrottleInputRange = rcCommandThrottleRange3dLow;
-            if (isMotorProtocolDshot()) mixerInversion = true;
-        } else {  // Deadband handling from positive to negative
+            if(isMotorProtocolDshot()) mixerInversion = true;
+            break;
+        default:
+        case(NORMAL_TO_DEADBAND):
             motorOutputMax = motorOutputHigh;
             motorOutputMin = deadbandMotor3dHigh;
             throttle = 0;
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 64535fd7c4..5bdfee7612 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -63,6 +63,14 @@ typedef enum mixerMode
     MIXER_QUADX_1234 = 26
 } mixerMode_e;
 
+typedef enum {
+    DEADBAND,
+    NORMAL,
+    INVERTED,
+    NORMAL_TO_DEADBAND,
+    INVERTED_TO_DEADBAND
+} throttle3Dstate_e;
+
 // Custom mixer data per motor
 typedef struct motorMixer_s {
     float throttle;