diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 91b6ec02ff..53b31bf4cc 100644 --- a/src/main/drivers/pwm_output.c +++ b/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 diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index ce10850f70..808ca383af 100644 --- a/src/main/drivers/pwm_output.h +++ b/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); diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index 91161e028f..d3f10eeeeb 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -17,6 +17,7 @@ * * If not, see . */ +#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", diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 8b8daaf6a8..df526a0f60 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -17,6 +17,7 @@ * * If not, see . */ +#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, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 5ccab8f8b5..e661ce0d76 100644 --- a/src/main/fc/config.c +++ b/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; } diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 355ccba92e..ac3c2901f9 100644 --- a/src/main/fc/fc_core.c +++ b/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(); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ef7431fe8f..93e7443921 100644 --- a/src/main/flight/mixer.c +++ b/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); } diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index 93231dbcde..049d4ccda7 100644 --- a/src/main/interface/cli.c +++ b/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 ), diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c index e8358a9db4..4511b63029 100644 --- a/src/main/interface/msp_box.c +++ b/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); }