From 72cff92727c9ed5c103590eefe6fbf73cf7864b7 Mon Sep 17 00:00:00 2001 From: Gordon Mak Date: Thu, 17 Sep 2020 11:08:51 +0800 Subject: [PATCH 1/2] Undo part of commit 80059b2eaeb0f5c24e5a86fad3bfd1c25b03c636 [support for NewBeeDrone FC] This change does not use macro definitions to segment the code, which may affect the underlying risk. --- src/main/drivers/pwm_output.c | 15 --------------- src/main/fc/config.c | 2 +- src/main/flight/mixer.c | 10 +--------- src/main/interface/msp_box.c | 2 +- 4 files changed, 3 insertions(+), 26 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index bb05fed21a..41a5ab9c0f 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]; @@ -153,15 +147,6 @@ static void pwmWriteUnused(uint8_t index, float 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; - } -} /* 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); } diff --git a/src/main/fc/config.c b/src/main/fc/config.c index ade415469a..b474964598 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -203,7 +203,7 @@ static void validateAndFixConfig(void) } 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/flight/mixer.c b/src/main/flight/mixer.c index dcb194b400..1b4057482d 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" @@ -644,11 +640,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) motorRangeMax = motorOutputHigh; motorOutputMin = motorOutputLow; motorOutputRange = motorOutputHigh - motorOutputLow; - if (getBoxIdState(BOXUSER4)) { - motorOutputMixSign = -1; - } else { - motorOutputMixSign = 1; - } + motorOutputMixSign = 1; } throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c index e0d478a41e..f600a976d8 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 }, From 2c5990f7a5e54497f437a46db9941950fec6bfd3 Mon Sep 17 00:00:00 2001 From: Gordon Mak Date: Mon, 21 Sep 2020 14:43:29 +0800 Subject: [PATCH 2/2] Added brushed motor flip over after crash feature - Use a general io to control motor forward and reverse (need special esc) - Added this pin to cli resource when macro activated --- src/main/drivers/pwm_output.c | 21 +++++++++++++++++++++ src/main/drivers/pwm_output.h | 8 ++++++++ src/main/drivers/resource.c | 4 ++++ src/main/drivers/resource.h | 4 ++++ src/main/fc/fc_core.c | 15 +++++++++++++++ src/main/flight/mixer.c | 3 +++ src/main/interface/cli.c | 3 +++ src/main/interface/msp_box.c | 5 +++++ 8 files changed, 63 insertions(+) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 41a5ab9c0f..42c3e74fb7 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -62,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 @@ -294,6 +298,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++) { @@ -624,3 +634,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/fc_core.c b/src/main/fc/fc_core.c index 50410bf861..a827b10d02 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -372,6 +372,21 @@ 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); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 1b4057482d..9876c66c8e 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -85,6 +85,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) { diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index 19c614696f..a54803a902 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -4206,6 +4206,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 f600a976d8..2b02ba2959 100644 --- a/src/main/interface/msp_box.c +++ b/src/main/interface/msp_box.c @@ -223,6 +223,11 @@ void initActiveBoxIds(void) if (isMotorProtocolDshot()) { BME(BOXFLIPOVERAFTERCRASH); } +#ifdef USE_BRUSHED_FLIPOVERAFTERCRASH + else { + BME(BOXFLIPOVERAFTERCRASH); + } +#endif if (feature(FEATURE_SERVO_TILT)) { BME(BOXCAMSTAB);