diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index a7da53c8b4..de9126c8f1 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -31,12 +31,14 @@ #define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) -#define DSHOT_MAX_COMMAND 47 - static pwmWriteFuncPtr pwmWritePtr; static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL; +#ifdef USE_DSHOT +loadDmaBufferFuncPtr loadDmaBufferPtr; +#endif + #ifdef USE_SERVOS static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS]; #endif @@ -47,6 +49,7 @@ static uint16_t freqBeep=0; #endif bool pwmMotorsEnabled = false; +bool isDigital = false; static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) { @@ -156,6 +159,33 @@ static void pwmWriteMultiShot(uint8_t index, float value) *motors[index].ccr = lrintf(((value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); } +#ifdef USE_DSHOT +static void pwmWriteDigital(uint8_t index, float value) +{ + pwmWriteDigitalInt(index, lrintf(value)); +} + +static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet) +{ + for (int i = 0; i < 16; i++) { + motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first + packet <<= 1; + } + + return DSHOT_DMA_BUFFER_SIZE; +} + +static uint8_t loadDmaBufferProshot(motorDmaOutput_t *const motor, uint16_t packet) +{ + 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 + } + + return PROSHOT_DMA_BUFFER_SIZE; +} +#endif + void pwmWriteMotor(uint8_t index, float value) { if (pwmMotorsEnabled) { @@ -218,7 +248,6 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 uint32_t timerMhzCounter = 0; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; - bool isDigital = false; switch (motorConfig->motorPwmProtocol) { default: @@ -248,7 +277,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 break; #ifdef USE_DSHOT case PWM_TYPE_PROSHOT1000: - pwmWritePtr = pwmWriteProShot; + pwmWritePtr = pwmWriteDigital; + loadDmaBufferPtr = loadDmaBufferProshot; pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; isDigital = true; break; @@ -256,7 +286,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: - pwmWritePtr = pwmWriteDshot; + pwmWritePtr = pwmWriteDigital; + loadDmaBufferPtr = loadDmaBufferDshot; pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; isDigital = true; break; @@ -343,7 +374,7 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) void pwmWriteDshotCommand(uint8_t index, uint8_t command) { - if (command <= DSHOT_MAX_COMMAND) { + if (isDigital && (command <= DSHOT_MAX_COMMAND)) { motorDmaOutput_t *const motor = getMotorDmaOutput(index); unsigned repeats; @@ -364,13 +395,32 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command) for (; repeats; repeats--) { motor->requestTelemetry = true; - pwmWritePtr(index, command); + pwmWriteDigitalInt(index, command); pwmCompleteMotorUpdate(0); delay(1); } } } + +uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value) +{ + 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; + + return packet; +} #endif #ifdef USE_SERVOS diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 1c3e792a49..dd8e642eb4 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -28,6 +28,8 @@ #define MAX_SUPPORTED_SERVOS 8 #endif +#define DSHOT_MAX_COMMAND 47 + typedef enum { DSHOT_CMD_MOTOR_STOP = 0, DSHOT_CMD_BEEP1, @@ -165,10 +167,15 @@ void servoDevInit(const servoDevConfig_t *servoDevConfig); void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); #ifdef USE_DSHOT +typedef uint8_t(*loadDmaBufferFuncPtr)(motorDmaOutput_t *const motor, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation + +uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value); + +extern loadDmaBufferFuncPtr loadDmaBufferPtr; + uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); void pwmWriteDshotCommand(uint8_t index, uint8_t command); -void pwmWriteProShot(uint8_t index, float value); -void pwmWriteDshot(uint8_t index, float value); +void pwmWriteDigitalInt(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 c75ac74a03..5a18ae0623 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -54,70 +54,19 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount-1; } -void pwmWriteDshot(uint8_t index, float value) +void pwmWriteDigitalInt(uint8_t index, uint16_t value) { - const uint16_t digitalValue = lrintf(value); - - motorDmaOutput_t * const motor = &dmaMotors[index]; + motorDmaOutput_t *const motor = &dmaMotors[index]; if (!motor->timerHardware || !motor->timerHardware->dmaRef) { return; } - uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0); - motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row + uint16_t packet = prepareDshotPacket(motor, value); - // 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; - } + uint8_t bufferSize = loadDmaBufferPtr(motor, packet); - DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, DSHOT_DMA_BUFFER_SIZE); - DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); -} - -void pwmWriteProShot(uint8_t index, float value) -{ - const uint16_t digitalValue = lrintf(value); - - motorDmaOutput_t * const motor = &dmaMotors[index]; - - if (!motor->timerHardware || !motor->timerHardware->dmaRef) { - return; - } - - uint16_t packet = (digitalValue << 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_SetCurrDataCounter(motor->timerHardware->dmaRef, bufferSize); DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); } diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index c2ff4737e1..a80c944290 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -49,85 +49,25 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount - 1; } -void pwmWriteDshot(uint8_t index, float value) +void pwmWriteDigitalInt(uint8_t index, uint16_t value) { - const uint16_t digitalValue = lrintf(value); - - motorDmaOutput_t * const motor = &dmaMotors[index]; + motorDmaOutput_t *const motor = &dmaMotors[index]; if (!motor->timerHardware || !motor->timerHardware->dmaRef) { return; } - uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0); - motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row + uint16_t packet = prepareDshotPacket(motor, value); - // 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; - } + uint8_t bufferSize = loadDmaBufferPtr(motor, packet); if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) { + if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize) != HAL_OK) { /* Starting PWM generation Error */ return; } } else { - 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, float value) -{ - const uint16_t digitalValue = lrintf(value); - - motorDmaOutput_t * const motor = &dmaMotors[index]; - - if (!motor->timerHardware || !motor->timerHardware->dmaRef) { - return; - } - - uint16_t packet = (digitalValue << 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) { + if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize) != HAL_OK) { /* Starting PWM generation Error */ return; } diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 818b2e9fb8..fa3382475a 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -339,6 +339,7 @@ bool mixerIsOutputSaturated(int axis, float errorRate) bool isMotorProtocolDshot(void) { #ifdef USE_DSHOT switch(motorConfig()->dev.motorPwmProtocol) { + case PWM_TYPE_PROSHOT1000: case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: