From b4d2388103b5f62676faafd4286f2359c269fbd7 Mon Sep 17 00:00:00 2001 From: TonyBazz Date: Sun, 28 May 2017 02:30:33 +1000 Subject: [PATCH] Some simple changes to allow for Proshot (Another digital protocol). --- src/main/drivers/pwm_output.c | 31 +++++++++----- src/main/drivers/pwm_output.h | 16 +++++-- src/main/drivers/pwm_output_dshot.c | 45 ++++++++++++++++---- src/main/drivers/pwm_output_dshot_hal.c | 55 +++++++++++++++++++++---- 4 files changed, 114 insertions(+), 33 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 1624598a5..1b14d37a0 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -158,7 +158,9 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value) void pwmWriteMotor(uint8_t index, uint16_t value) { - pwmWritePtr(index, value); + if (pwmMotorsEnabled) { + pwmWritePtr(index, value); + } } void pwmShutdownPulsesForAllMotors(uint8_t motorCount) @@ -245,11 +247,16 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 idlePulse = 0; break; #ifdef USE_DSHOT + case PWM_TYPE_PROSHOT1000: + pwmWritePtr = pwmWriteProShot; + pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; + isDigital = true; + break; case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: - pwmWritePtr = pwmWriteDigital; + pwmWritePtr = pwmWriteDshot; pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; isDigital = true; break; @@ -320,15 +327,17 @@ pwmOutputPort_t *pwmGetMotors(void) uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) { switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT1200): - return MOTOR_DSHOT1200_MHZ * 1000000; - case(PWM_TYPE_DSHOT600): - return MOTOR_DSHOT600_MHZ * 1000000; - case(PWM_TYPE_DSHOT300): - return MOTOR_DSHOT300_MHZ * 1000000; - default: - case(PWM_TYPE_DSHOT150): - return MOTOR_DSHOT150_MHZ * 1000000; + case(PWM_TYPE_PROSHOT1000): + return MOTOR_PROSHOT1000_MHZ * 1000000; + case(PWM_TYPE_DSHOT1200): + return MOTOR_DSHOT1200_MHZ * 1000000; + case(PWM_TYPE_DSHOT600): + return MOTOR_DSHOT600_MHZ * 1000000; + case(PWM_TYPE_DSHOT300): + return MOTOR_DSHOT300_MHZ * 1000000; + default: + case(PWM_TYPE_DSHOT150): + return MOTOR_DSHOT150_MHZ * 1000000; } } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index f4249f4ad..d211246cb 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -39,6 +39,7 @@ typedef enum { PWM_TYPE_DSHOT300, PWM_TYPE_DSHOT600, PWM_TYPE_DSHOT1200, + PWM_TYPE_PROSHOT1000, #endif PWM_TYPE_MAX } motorPwmProtocolTypes_e; @@ -56,6 +57,11 @@ typedef enum { #define MOTOR_BIT_0 7 #define MOTOR_BIT_1 14 #define MOTOR_BITLENGTH 19 + +#define MOTOR_PROSHOT1000_MHZ 24 +#define PROSHOT_BASE_SYMBOL 24 // 1uS +#define PROSHOT_BIT_WIDTH 3 +#define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS #endif #if defined(STM32F40_41xxx) // must be multiples of timer clock @@ -75,7 +81,8 @@ typedef enum { #define PWM_BRUSHED_TIMER_MHZ 24 #endif -#define MOTOR_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ +#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ +#define PROSHOT_DMA_BUFFER_SIZE 6/* resolution + frame reset (2us) */ typedef struct { TIM_TypeDef *timer; @@ -89,9 +96,9 @@ typedef struct { uint16_t timerDmaSource; volatile bool requestTelemetry; #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) - uint32_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE]; + uint32_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; #else - uint8_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE]; + uint8_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; #endif #if defined(STM32F7) TIM_HandleTypeDef TimHandle; @@ -140,7 +147,8 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn #ifdef USE_DSHOT uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); void pwmWriteDshotCommand(uint8_t index, uint8_t command); -void pwmWriteDigital(uint8_t index, uint16_t value); +void pwmWriteProShot(uint8_t index, uint16_t value); +void pwmWriteDshot(uint8_t index, uint16_t value); 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_dshot.c b/src/main/drivers/pwm_output_dshot.c index 38a4c1ccd..35fbad1ec 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -54,13 +54,8 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount-1; } -void pwmWriteDigital(uint8_t index, uint16_t value) +void pwmWriteDshot(uint8_t index, uint16_t value) { - - if (!pwmMotorsEnabled) { - return; - } - motorDmaOutput_t * const motor = &dmaMotors[index]; if (!motor->timerHardware || !motor->timerHardware->dmaRef) { @@ -86,7 +81,39 @@ void pwmWriteDigital(uint8_t index, uint16_t value) packet <<= 1; } - DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, MOTOR_DMA_BUFFER_SIZE); + DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, DSHOT_DMA_BUFFER_SIZE); + DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); +} + +void pwmWriteProShot(uint8_t index, uint16_t value) +{ + motorDmaOutput_t * const motor = &dmaMotors[index]; + + if (!motor->timerHardware || !motor->timerHardware->dmaRef) { + return; + } + + uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); + motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row + + // compute checksum + int csum = 0; + int csum_data = packet; + for (int i = 0; i < 3; i++) { + csum ^= csum_data; // xor data by nibbles + csum_data >>= 4; + } + csum &= 0xf; + // append checksum + packet = (packet << 4) | csum; + + // generate pulses for Proshot + for (int i = 0; i < 4; i++) { + motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first + packet <<= 4; // Shift 4 bits + } + + DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, PROSHOT_DMA_BUFFER_SIZE); DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); } @@ -139,7 +166,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t TIM_Cmd(timer, DISABLE); TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)((timerClock(timer) / getDshotHz(pwmProtocolType)) - 1); - TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH; + TIM_TimeBaseStructure.TIM_Period = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; @@ -205,7 +232,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; #endif DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware); - DMA_InitStructure.DMA_BufferSize = MOTOR_DMA_BUFFER_SIZE; + DMA_InitStructure.DMA_BufferSize = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index 01083add6..c1a1137f3 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -49,12 +49,8 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount - 1; } -void pwmWriteDigital(uint8_t index, uint16_t value) +void pwmWriteDshot(uint8_t index, uint16_t value) { - if (!pwmMotorsEnabled) { - return; - } - motorDmaOutput_t * const motor = &dmaMotors[index]; if (!motor->timerHardware || !motor->timerHardware->dmaRef) { @@ -81,12 +77,53 @@ void pwmWriteDigital(uint8_t index, uint16_t value) } if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK) { + if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) { /* Starting PWM generation Error */ return; } } else { - if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK) { + if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) { + /* Starting PWM generation Error */ + return; + } + } +} + +void pwmWriteProShot(uint8_t index, uint16_t value) +{ + motorDmaOutput_t * const motor = &dmaMotors[index]; + + if (!motor->timerHardware || !motor->timerHardware->dmaRef) { + return; + } + + uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); + motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row + + // compute checksum + int csum = 0; + int csum_data = packet; + for (int i = 0; i < 3; i++) { + csum ^= csum_data; // xor data by nibbles + csum_data >>= 4; + } + csum &= 0xf; + // append checksum + packet = (packet << 4) | csum; + + // generate pulses for Proshot + for (int i = 0; i < 4; i++) { + motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first + packet <<= 4; // Shift 4 bits + } + + if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { + if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) { + /* Starting PWM generation Error */ + return; + } + } else { + if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) { /* Starting PWM generation Error */ return; } @@ -122,8 +159,8 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t RCC_ClockCmd(timerRCC(timer), ENABLE); motor->TimHandle.Instance = timerHardware->tim; - motor->TimHandle.Init.Prescaler = (timerClock(timer) / getDshotHz(pwmProtocolType)) - 1;; - motor->TimHandle.Init.Period = MOTOR_BITLENGTH; + motor->TimHandle.Init.Prescaler = (timerClock(timer) / getDshotHz(pwmProtocolType)) - 1; + motor->TimHandle.Init.Period = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH; motor->TimHandle.Init.RepetitionCounter = 0; motor->TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; motor->TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;