From 07dc9eaa5ee5558eacd6fe4ede65d2bc85dcc101 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 23 Oct 2016 00:14:57 +0200 Subject: [PATCH] Fix DSHOT CRC --- src/main/drivers/pwm_output_stm32f3xx.c | 34 ++++++++++++------------- src/main/drivers/pwm_output_stm32f4xx.c | 34 ++++++++++++------------- src/main/drivers/pwm_output_stm32f7xx.c | 34 ++++++++++++------------- 3 files changed, 48 insertions(+), 54 deletions(-) diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index f8bcfe74a2..5e4f52fb66 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -58,24 +58,22 @@ void pwmWriteDigital(uint8_t index, uint16_t value) { motorDmaOutput_t * const motor = &dmaMotors[index]; - motor->dmaBuffer[0] = (value & 0x400) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[1] = (value & 0x200) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[2] = (value & 0x100) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[3] = (value & 0x80) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[4] = (value & 0x40) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[5] = (value & 0x20) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[6] = (value & 0x10) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[7] = (value & 0x8) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[8] = (value & 0x4) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[9] = (value & 0x2) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[10] = (value & 0x1) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[11] = MOTOR_BIT_0; /* telemetry is always false for the moment */ - - /* check sum */ - motor->dmaBuffer[12] = (value & 0x400) ^ (value & 0x40) ^ (value & 0x4) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[13] = (value & 0x200) ^ (value & 0x20) ^ (value & 0x2) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[14] = (value & 0x100) ^ (value & 0x10) ^ (value & 0x1) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[15] = (value & 0x80) ^ (value & 0x8) ^ (0x0) ? MOTOR_BIT_1 : MOTOR_BIT_0; + uint16_t packet = (value << 1) | 0; // Here goes telemetry bit (false for now) + // 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 whole packet + for (int i = 0; i < 16; i++) { + motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first + packet <<= 1; + } DMA_SetCurrDataCounter(motor->timerHardware->dmaChannel, MOTOR_DMA_BUFFER_SIZE); DMA_Cmd(motor->timerHardware->dmaChannel, ENABLE); diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index af2f3672a4..cc3e71ac9d 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -58,24 +58,22 @@ void pwmWriteDigital(uint8_t index, uint16_t value) { motorDmaOutput_t * const motor = &dmaMotors[index]; - motor->dmaBuffer[0] = (value & 0x400) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[1] = (value & 0x200) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[2] = (value & 0x100) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[3] = (value & 0x80) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[4] = (value & 0x40) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[5] = (value & 0x20) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[6] = (value & 0x10) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[7] = (value & 0x8) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[8] = (value & 0x4) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[9] = (value & 0x2) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[10] = (value & 0x1) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[11] = MOTOR_BIT_0; /* telemetry is always false for the moment */ - - /* check sum */ - motor->dmaBuffer[12] = (value & 0x400) ^ (value & 0x40) ^ (value & 0x4) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[13] = (value & 0x200) ^ (value & 0x20) ^ (value & 0x2) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[14] = (value & 0x100) ^ (value & 0x10) ^ (value & 0x1) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[15] = (value & 0x80) ^ (value & 0x8) ^ (0x0) ? MOTOR_BIT_1 : MOTOR_BIT_0; + uint16_t packet = (value << 1) | 0; // Here goes telemetry bit (false for now) + // 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 whole packet + for (int i = 0; i < 16; i++) { + motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first + packet <<= 1; + } DMA_SetCurrDataCounter(motor->timerHardware->dmaStream, MOTOR_DMA_BUFFER_SIZE); DMA_Cmd(motor->timerHardware->dmaStream, ENABLE); diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index 5ff6c4fc9b..4d47bfc990 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -58,24 +58,22 @@ void pwmWriteDigital(uint8_t index, uint16_t value) { motorDmaOutput_t * const motor = &dmaMotors[index]; - motor->dmaBuffer[0] = (value & 0x400) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[1] = (value & 0x200) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[2] = (value & 0x100) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[3] = (value & 0x80) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[4] = (value & 0x40) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[5] = (value & 0x20) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[6] = (value & 0x10) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[7] = (value & 0x8) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[8] = (value & 0x4) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[9] = (value & 0x2) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[10] = (value & 0x1) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[11] = MOTOR_BIT_0; /* telemetry is always false for the moment */ - - /* check sum */ - motor->dmaBuffer[12] = (value & 0x400) ^ (value & 0x40) ^ (value & 0x4) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[13] = (value & 0x200) ^ (value & 0x20) ^ (value & 0x2) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[14] = (value & 0x100) ^ (value & 0x10) ^ (value & 0x1) ? MOTOR_BIT_1 : MOTOR_BIT_0; - motor->dmaBuffer[15] = (value & 0x80) ^ (value & 0x8) ^ (0x0) ? MOTOR_BIT_1 : MOTOR_BIT_0; + uint16_t packet = (value << 1) | 0; // Here goes telemetry bit (false for now) + // 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 whole packet + for (int i = 0; i < 16; i++) { + motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first + packet <<= 1; + } if( HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK) {