Redo brushed flipoveraftercrash

src/main/drivers/pwm_output.c

@@ -26,16 +26,10 @@
 #include "platform.h"
 #include "drivers/time.h"
 
-#include "pg/pinio.h"
-#include "pg/piniobox.h"
-
-#include "interface/msp_box.h"
-
 #include "drivers/io.h"
 #include "pwm_output.h"
 #include "timer.h"
 #include "drivers/pwm_output.h"
-#include "config/feature.h"
 
 static FAST_RAM_ZERO_INIT pwmWriteFn *pwmWrite;
 static FAST_RAM_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
@@ -68,6 +62,10 @@ static pwmOutputPort_t beeperPwm;
 static uint16_t freqBeep = 0;
 #endif
 
+#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH
+IO_t ioBrushedReverse;
+#endif
+
 static bool pwmMotorsEnabled = false;
 static bool isDshot = false;
 #ifdef USE_DSHOT_DMAR
@@ -137,16 +135,8 @@ static void pwmWriteUnused(uint8_t index, float value) {
     UNUSED(value);
 }
 
-FAST_CODE static void pwmWriteStandard(uint8_t index, float value) {
-    if(feature(FEATURE_3D)) {
-        if (lrintf(value) - 1500 > 0) {
-            pinioSet(0, 0);     // set to forward
-            value = (value - 1500) * 2 + 1000;
-        } else {
-            pinioSet(0, 1);     // set to backward
-            value = (1500 - value) * 2 + 1000;
-        }
-    }
+FAST_CODE static void pwmWriteStandard(uint8_t index, float value)
+{
     /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
     *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
 }
@@ -277,6 +267,12 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
     if (!isDshot) {
         pwmWrite = &pwmWriteStandard;
         pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
+
+#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH
+        ioBrushedReverse = IOGetByTag(motorConfig->reverseTag);
+        IOInit(ioBrushedReverse, OWNER_BRUSHED_REVERSE, 0);
+        IOConfigGPIO(ioBrushedReverse, IOCFG_OUT_PP);
+#endif
     }
     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
         const ioTag_t tag = motorConfig->ioTags[motorIndex];
@@ -555,3 +551,14 @@ void beeperPwmInit(const ioTag_t tag, uint16_t frequency) {
     }
 }
 #endif
+
+#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH
+void pwmWriteBrushedMotorReverse(bool onoffReverse)
+{
+    if (onoffReverse == true) {
+        IOHi(ioBrushedReverse);
+    } else {
+        IOLo(ioBrushedReverse);
+    }
+}
+#endif

src/main/drivers/pwm_output.h

@@ -180,6 +180,9 @@ typedef struct motorDevConfig_s {
     uint8_t  useUnsyncedPwm;
     uint8_t  useBurstDshot;
     ioTag_t  ioTags[MAX_SUPPORTED_MOTORS];
+#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH
+    ioTag_t  reverseTag;
+#endif
 } motorDevConfig_t;
 
 extern bool useBurstDshot;
@@ -225,6 +228,11 @@ void pwmWriteBeeper(bool onoffBeep);
 void pwmToggleBeeper(void);
 void beeperPwmInit(const ioTag_t tag, uint16_t frequency);
 #endif
+
+#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH
+void pwmWriteBrushedMotorReverse(bool onoffReverse);
+#endif
+
 void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion);
 
 void pwmWriteMotor(uint8_t index, float value);

src/main/drivers/resource.c

@@ -17,6 +17,7 @@
  *
  * If not, see <http://www.gnu.org/licenses/>.
  */
+#include "platform.h"
 
 #include "resource.h"
 
@@ -25,6 +26,9 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
     "PWM",
     "PPM",
     "MOTOR",
+#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH
+    "BRUSHED_REVERSE",
+#endif
     "SERVO",
     "LED",
     "ADC",

src/main/drivers/resource.h

@@ -17,6 +17,7 @@
  *
  * If not, see <http://www.gnu.org/licenses/>.
  */
+#include "platform.h"
 
 #pragma once
 
@@ -25,6 +26,9 @@ typedef enum {
     OWNER_PWMINPUT,
     OWNER_PPMINPUT,
     OWNER_MOTOR,
+#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH
+    OWNER_BRUSHED_REVERSE,
+#endif
     OWNER_SERVO,
     OWNER_LED,
     OWNER_ADC,

src/main/fc/config.c

@@ -179,7 +179,8 @@ static void validateAndFixConfig(void) {
         currentPidProfile->auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY;
     }
     if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
-        //  featureClear(FEATURE_3D);
+        featureClear(FEATURE_3D);
+
         if (motorConfig()->mincommand < 1000) {
             motorConfigMutable()->mincommand = 1000;
         }

src/main/fc/fc_core.c

@@ -337,6 +337,22 @@ void tryArm(void) {
             }
         }
 #endif
+
+#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH
+        if (isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) {
+            if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH))) {
+                flipOverAfterCrashMode = false;
+                pwmWriteBrushedMotorReverse(false);
+            } else {
+                flipOverAfterCrashMode = true;
+#ifdef USE_RUNAWAY_TAKEOFF
+                runawayTakeoffCheckDisabled = false;
+#endif
+                pwmWriteBrushedMotorReverse(true);
+            }
+        }
+#endif
+
         ENABLE_ARMING_FLAG(ARMED);
         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
         resetTryingToArm();

src/main/flight/mixer.c

@@ -36,10 +36,6 @@
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
 #include "pg/rx.h"
-#include "pg/pinio.h"
-#include "pg/piniobox.h"
-
-#include "interface/msp_box.h"
 
 #include "drivers/pwm_output.h"
 #include "drivers/pwm_esc_detect.h"
@@ -88,6 +84,9 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig) {
     motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
     motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
     motorConfig->dev.useUnsyncedPwm = true;
+#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH
+    motorConfig->dev.reverseTag = IO_TAG(BRUSHED_REVERSE_PIN);
+#endif
 #else
 #ifdef USE_BRUSHED_ESC_AUTODETECT
     if (hardwareMotorType == MOTOR_BRUSHED) {
@@ -628,11 +627,6 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) {
         motorRangeMax = motorOutputHigh;
         motorOutputMin = motorOutputLow;
         motorOutputRange = motorOutputHigh - motorOutputLow;
-        if (getBoxIdState(BOXUSER4)) {
-            controllerMix3DModeSign = -1;
-        } else {
-            controllerMix3DModeSign = 1;
-        }
     }
     throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
 }

src/main/interface/cli.c

@@ -3749,6 +3749,9 @@ const cliResourceValue_t resourceTable[] = {
 #if defined(USE_PWM)
     DEFA( OWNER_PWMINPUT,      PG_PWM_CONFIG, pwmConfig_t, ioTags[0], PWM_INPUT_PORT_COUNT ),
 #endif
+#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH
+    DEFS( OWNER_BRUSHED_REVERSE, PG_MOTOR_CONFIG, motorConfig_t, dev.reverseTag ),
+#endif
 #ifdef USE_RANGEFINDER_HCSR04
     DEFS( OWNER_SONAR_TRIGGER, PG_SONAR_CONFIG, sonarConfig_t, triggerTag ),
     DEFS( OWNER_SONAR_ECHO,    PG_SONAR_CONFIG, sonarConfig_t, echoTag ),

src/main/interface/msp_box.c

@@ -90,7 +90,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { BOXUSER1, "USER1", 40 },
     { BOXUSER2, "USER2", 41 },
     { BOXUSER3, "USER3", 42 },
-    { BOXUSER4, "FLIP OVER AFTER CRASH BRUSHED", 43 },
+    { BOXUSER4, "USER4", 43 },
     { BOXPIDAUDIO, "PID AUDIO", 44 },
     { BOXPARALYZE, "PARALYZE", 45 },
     { BOXGPSRESCUE, "GPS RESCUE", 46 },
@@ -205,6 +205,12 @@ void initActiveBoxIds(void) {
     if (isMotorProtocolDshot()) {
         BME(BOXFLIPOVERAFTERCRASH);
     }
+#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH
+    else {
+        BME(BOXFLIPOVERAFTERCRASH);
+    }
+#endif
+
     if (feature(FEATURE_SERVO_TILT)) {
         BME(BOXCAMSTAB);
     }