From c932bd73131987192abb1259ca481d1e28a55c10 Mon Sep 17 00:00:00 2001 From: DieHertz Date: Tue, 27 Dec 2016 13:25:37 +0300 Subject: [PATCH] Added PWM inversion to motor config --- src/main/drivers/pwm_output.c | 14 ++++++++------ src/main/drivers/pwm_output.h | 2 +- src/main/drivers/pwm_output_stm32f3xx.c | 8 ++++---- src/main/drivers/pwm_output_stm32f4xx.c | 8 ++++---- src/main/drivers/pwm_output_stm32f7xx.c | 12 ++++++++---- src/main/fc/cli.c | 1 + src/main/fc/fc_msp.c | 7 ++++++- src/main/io/motors.h | 3 ++- 8 files changed, 34 insertions(+), 21 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 2c7628988..f0ce496c6 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -82,7 +82,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 #endif } -static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value) +static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, uint8_t inversion) { #if defined(USE_HAL_DRIVER) TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim); @@ -90,7 +90,8 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard #endif configTimeBase(timerHardware->tim, period, mhz); - pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output); + pwmOCConfig(timerHardware->tim, timerHardware->channel, value, + inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); #if defined(USE_HAL_DRIVER) HAL_TIM_PWM_Start(Handle, timerHardware->channel); @@ -256,7 +257,8 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot #ifdef USE_DSHOT if (isDigital) { - pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol); + pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol, + motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); motors[motorIndex].enabled = true; continue; } @@ -271,9 +273,9 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot if (useUnsyncedPwm) { const uint32_t hz = timerMhzCounter * 1000000; - pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse); + pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse, motorConfig->motorPwmInversion); } else { - pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0); + pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion); } motors[motorIndex].enabled = true; } @@ -335,7 +337,7 @@ void servoInit(const servoConfig_t *servoConfig) break; } - pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse); + pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0); servos[servoIndex].enabled = true; } } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index a6ad7f38b..c86488987 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -118,7 +118,7 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn #ifdef USE_DSHOT uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); void pwmWriteDigital(uint8_t index, uint16_t value); -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType); +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); void pwmCompleteDigitalMotorUpdate(uint8_t motorCount); #endif diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index 7a624c363..6a4f3260d 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -105,7 +105,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) } } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; @@ -139,14 +139,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + if (output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; + TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } TIM_OCInitStructure.TIM_Pulse = 0; diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 55a48bcba..0f4b80f45 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -102,7 +102,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) } } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; @@ -136,14 +136,14 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; - if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + if (output & TIMER_OUTPUT_N_CHANNEL) { TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; + TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low; } else { TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } TIM_OCInitStructure.TIM_Pulse = 0; diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index 30ccd983e..58e4f37bf 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -96,7 +96,7 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) UNUSED(motorCount); } -void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) +void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; motor->timerHardware = timerHardware; @@ -174,9 +174,13 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t /* PWM1 Mode configuration: Channel1 */ TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1; - TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH; - TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET; - TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; + if (output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW; + TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; + } else { + TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH; + TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET; + } TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; TIM_OCInitStructure.Pulse = 0; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 218772e86..d8b5ad980 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -524,6 +524,7 @@ const clivalue_t valueTable[] = { { "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } }, { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &motorConfig()->motorPwmRate, .config.minmax = { 200, 32000 } }, + { "motor_pwm_inversion", VAR_UINT8 | MASTER_VALUE, &motorConfig()->motorPwmInversion, .config.lookup = { TABLE_OFF_ON } }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, { "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } }, diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 88ea65d1f..c80b6db8b 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1143,6 +1143,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, gyroConfig()->gyro_use_32khz); //!!TODO gyro_isr_update to be added pending decision //sbufWriteU8(dst, gyroConfig()->gyro_isr_update); + sbufWriteU8(dst, motorConfig()->motorPwmInversion); break; case MSP_FILTER_CONFIG : @@ -1479,7 +1480,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED); #endif motorConfig()->motorPwmRate = sbufReadU16(src); - if (dataSize > 7) { + if (sbufBytesRemaining(src) >= 2) { motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f; } if (sbufBytesRemaining(src)) { @@ -1490,6 +1491,10 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) gyroConfig()->gyro_isr_update = sbufReadU8(src); }*/ validateAndFixGyroConfig(); + + if (sbufBytesRemaining(src)) { + motorConfig()->motorPwmInversion = sbufReadU8(src); + } break; case MSP_SET_FILTER_CONFIG: diff --git a/src/main/io/motors.h b/src/main/io/motors.h index 8486d507d..82517b2e9 100644 --- a/src/main/io/motors.h +++ b/src/main/io/motors.h @@ -24,8 +24,9 @@ typedef struct motorConfig_s { uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs - uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) + uint16_t motorPwmRate; // The update rate of motor outputs uint8_t motorPwmProtocol; // Pwm Protocol + uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation uint8_t useUnsyncedPwm; float digitalIdleOffsetPercent; ioTag_t ioTags[MAX_SUPPORTED_MOTORS];