3d on a mode switch

src/main/fc/fc_core.c

@@ -106,7 +106,6 @@ enum {
 int16_t magHold;
 #endif
 
-static bool reverseMotors = false;
 static bool flipOverAfterCrashMode = false;
 
 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
@@ -728,10 +727,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     }
 }
 
-bool isMotorsReversed(void)
-{
-    return reverseMotors;
-}
 
 bool isFlipOverAfterCrashMode(void)
 {

src/main/fc/fc_core.h

@@ -47,5 +47,4 @@ void updateArmingStatus(void);
 void updateRcCommands(void);
 
 void taskMainPidLoop(timeUs_t currentTimeUs);
-bool isMotorsReversed(void);
 bool isFlipOverAfterCrashMode(void);

src/main/fc/fc_msp_box.c

@@ -79,6 +79,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { BOXFLIPOVERAFTERCRASH, "FLIP OVER AFTER CRASH", 35 },
     { BOXPREARM, "PREARM", 36 },
     { BOXBEEPGPSCOUNT, "BEEP GPS SATELLITE COUNT", 37 },
+    { BOX3DONASWITCH, "3D ON A SWITCH", 38 },
+
 };
 
 // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@@ -216,6 +218,7 @@ void initActiveBoxIds(void)
 
     if (feature(FEATURE_3D)) {
         BME(BOX3DDISABLE);
+        BME(BOX3DONASWITCH);
     }
 
     if (isMotorProtocolDshot()) {

src/main/fc/fc_rc.c

@@ -49,6 +49,7 @@
 
 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
 static float throttlePIDAttenuation;
+static bool reverseMotors = false;
 
 float getSetpointRate(int axis)
 {
@@ -327,6 +328,18 @@ void updateRcCommands(void)
         rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
     }
 
+    if (feature(FEATURE_3D) && isModeActivationConditionPresent(BOX3DONASWITCH) && !failsafeIsActive()) {
+        if (IS_RC_MODE_ACTIVE(BOX3DONASWITCH)) {
+            reverseMotors = true;
+            fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
+            rcCommand[THROTTLE] = rxConfig()->midrc - qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
+        }
+        else {
+            reverseMotors = false;
+            fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
+            rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
+        }
+    }
     if (FLIGHT_MODE(HEADFREE_MODE)) {
         static t_fp_vector_def  rcCommandBuff;
 
@@ -351,3 +364,8 @@ void resetYawAxis(void)
     rcCommand[YAW] = 0;
     setpointRate[YAW] = 0;
 }
+
+bool isMotorsReversed(void)
+{
+    return reverseMotors;
+}

src/main/fc/fc_rc.h

@@ -24,3 +24,5 @@ float getThrottlePIDAttenuation(void);
 void updateRcCommands(void);
 void resetYawAxis(void);
 void generateThrottleCurve(void);
+bool isMotorsReversed(void);
+

src/main/fc/rc_controls.c

@@ -108,9 +108,13 @@ bool areSticksInApModePosition(uint16_t ap_mode)
 
 throttleStatus_e calculateThrottleStatus(void)
 {
-    if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLE)) {
+    if (feature(FEATURE_3D)) {
-        if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)))
+        if (IS_RC_MODE_ACTIVE(BOX3DDISABLE) || isModeActivationConditionPresent(BOX3DONASWITCH)) {
+            if (rcData[THROTTLE] < rxConfig()->mincheck)
+                return THROTTLE_LOW;
+        } else if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) {
             return THROTTLE_LOW;
+        }
     } else {
         if (rcData[THROTTLE] < rxConfig()->mincheck)
             return THROTTLE_LOW;

src/main/fc/rc_modes.h

@@ -60,6 +60,7 @@ typedef enum {
     BOXFLIPOVERAFTERCRASH,
     BOXPREARM,
     BOXBEEPGPSCOUNT,
+    BOX3DONASWITCH,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/flight/mixer.c

@@ -559,7 +559,9 @@ void calculateThrottleAndCurrentMotorEndpoints(void)
             rcThrottlePrevious = rcCommand[THROTTLE];
             throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh;
             currentThrottleInputRange = rcCommandThrottleRange3dHigh;
-        } else if((rcThrottlePrevious <= rcCommand3dDeadBandLow)) {
+        } else if((rcThrottlePrevious <= rcCommand3dDeadBandLow &&
+                !isModeActivationConditionPresent(BOX3DONASWITCH)) || 
+                isMotorsReversed()) {
             // INVERTED_TO_DEADBAND
             motorRangeMin = motorOutputLow;
             motorRangeMax = deadbandMotor3dLow;
@@ -676,11 +678,6 @@ void mixTable(uint8_t vbatPidCompensation)
         constrainf(axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
     float scaledAxisPidYaw =
         constrainf(axisPID_P[FD_YAW] + axisPID_I[FD_YAW], -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING;
-    if (isMotorsReversed()) {
-        scaledAxisPidRoll = -scaledAxisPidRoll;
-        scaledAxisPidPitch = -scaledAxisPidPitch;
-        scaledAxisPidYaw = -scaledAxisPidYaw;
-    }
     if (!mixerConfig()->yaw_motors_reversed) {
         scaledAxisPidYaw = -scaledAxisPidYaw;
     }