diff --git a/src/main/config/config.h b/src/main/config/config.h index 78de360b61..4ee2fcd756 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -72,6 +72,7 @@ typedef enum { FEATURE_RX_SPI = 1 << 25, FEATURE_SOFTSPI = 1 << 26, FEATURE_PWM_SERVO_DRIVER = 1 << 27, + FEATURE_PWM_OUTPUT_ENABLE = 1 << 28, } features_e; typedef enum { diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index f3e49b48d6..fa1861c9b2 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -32,8 +32,8 @@ #include "pwm_rx.h" #include "pwm_mapping.h" -void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto); -void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); +void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto, bool enableOutput); +void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput); /* Configuration maps @@ -370,7 +370,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) } #endif - pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType); + pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType, init->enablePWMOutput); if (init->useFastPwm) { pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_FASTPWM | PWM_PF_OUTPUT_PROTOCOL_PWM; } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { @@ -392,7 +392,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) } #ifdef USE_SERVOS - pwmServoConfig(timerHardwarePtr, pwmIOConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse); + pwmServoConfig(timerHardwarePtr, pwmIOConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse, init->enablePWMOutput); pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO | PWM_PF_OUTPUT_PROTOCOL_PWM; pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.servoCount; diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 84497c6fca..c85e65826a 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -50,6 +50,7 @@ typedef struct sonarIOConfig_s { } sonarIOConfig_t; typedef struct drv_pwm_config_s { + bool enablePWMOutput; bool useParallelPWM; bool usePPM; bool useSerialRx; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 79b9434ca1..a86e81e700 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -109,7 +109,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 } } -static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value) +static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, bool enableOutput) { pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; @@ -117,7 +117,17 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 const IO_t io = IOGetByTag(timerHardware->tag); IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); - IOConfigGPIO(io, IOCFG_AF_PP); + + if (enableOutput) { + // If PWM outputs are enabled - configure as AF_PP - map to timer + // AF itself was configured by timerInit(); + IOConfigGPIO(io, IOCFG_AF_PP); + } + else { + // If PWM outputs are disabled - configure as GPIO and drive low + IOConfigGPIO(io, IOCFG_OUT_OD); + IOLo(io); + } pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output & TIMER_OUTPUT_INVERTED); if (timerHardware->output & TIMER_OUTPUT_ENABLED) { @@ -204,7 +214,7 @@ bool isMotorBrushed(uint16_t motorPwmRate) return (motorPwmRate > 500); } -void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto) +void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto, bool enableOutput) { uint32_t timerMhzCounter; pwmWriteFuncPtr pwmWritePtr; @@ -243,14 +253,14 @@ void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, ui } const uint32_t hz = timerMhzCounter * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse); + motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse, enableOutput); motors[motorIndex]->pwmWritePtr = pwmWritePtr; } #ifdef USE_SERVOS -void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse) +void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput) { - servos[servoIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse); + servos[servoIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse, enableOutput); } void pwmWriteServo(uint8_t index, uint16_t value) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index c77aeb547f..b342ed5d67 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -289,6 +289,8 @@ void init(void) pwm_params.idlePulse = 0; // brushed motors } + pwm_params.enablePWMOutput = feature(FEATURE_PWM_OUTPUT_ENABLE); + #ifndef SKIP_RX_PWM_PPM pwmRxInit(masterConfig.inputFilteringMode); #endif diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 110839b1d4..8a9b1aac40 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -219,7 +219,7 @@ static const char * const featureNames[] = { "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "UNUSED_2", "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", - "SUPEREXPO", "VTX", "RX_SPI", "SOFTSPI", "PWM_SERVO_DRIVER", NULL + "SUPEREXPO", "VTX", "RX_SPI", "SOFTSPI", "PWM_SERVO_DRIVER", "PWM_OUTPUT_ENABLE", NULL }; // sync this with rxFailsafeChannelMode_e