Fixup Dshot / Proshot implementation.

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

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 pwmWriteDigitalInt(uint8_t index, uint16_t value);
-void pwmWriteDshot(uint8_t index, float value);
 void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
 void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
 #endif

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);
+    uint16_t packet = prepareDshotPacket(motor, value);
-    motor->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
 
-    // compute checksum
+    uint8_t bufferSize = loadDmaBufferPtr(motor, packet);
-    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->dmaRef, DSHOT_DMA_BUFFER_SIZE);
+    DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, bufferSize);
-    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_Cmd(motor->timerHardware->dmaRef, ENABLE);
 }
 

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);
+    uint16_t packet = prepareDshotPacket(motor, value);
-    motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
 
-    // compute checksum
+    uint8_t bufferSize = loadDmaBufferPtr(motor, packet);
-    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 (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) {
+        if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize) != 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) {
             /* Starting PWM generation Error */
             return;
         }

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: