Merge pull request #3318 from mikeller/fixup_dshot

Fixup Dshot / Proshot implementation.

src/main/drivers/pwm_output.c

@@ -31,11 +31,13 @@
 #define MULTISHOT_5US_PW    (MULTISHOT_TIMER_MHZ * 5)
 #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
 
-#define DSHOT_MAX_COMMAND 47
+static pwmWriteFunc *pwmWrite;
-
-static pwmWriteFuncPtr pwmWritePtr;
 static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
-static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
+static pwmCompleteWriteFunc *pwmCompleteWrite = NULL;
+
+#ifdef USE_DSHOT
+loadDmaBufferFunc *loadDmaBuffer;
+#endif
 
 #ifdef USE_SERVOS
 static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
@@ -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,10 +159,37 @@ 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) {
-        pwmWritePtr(index, value);
+        pwmWrite(index, value);
     }    
 }
 
@@ -182,7 +212,7 @@ void pwmDisableMotors(void)
 void pwmEnableMotors(void)
 {
     /* check motors can be enabled */
-    pwmMotorsEnabled = (pwmWritePtr != pwmWriteUnused);
+    pwmMotorsEnabled = (pwmWrite != &pwmWriteUnused);
 }
 
 bool pwmAreMotorsEnabled(void)
@@ -209,7 +239,7 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
 
 void pwmCompleteMotorUpdate(uint8_t motorCount)
 {
-    pwmCompleteWritePtr(motorCount);
+    pwmCompleteWrite(motorCount);
 }
 
 void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
@@ -218,53 +248,54 @@ 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:
     case PWM_TYPE_ONESHOT125:
         timerMhzCounter = ONESHOT125_TIMER_MHZ;
-        pwmWritePtr = pwmWriteOneShot125;
+        pwmWrite = &pwmWriteOneShot125;
         break;
     case PWM_TYPE_ONESHOT42:
         timerMhzCounter = ONESHOT42_TIMER_MHZ;
-        pwmWritePtr = pwmWriteOneShot42;
+        pwmWrite = &pwmWriteOneShot42;
         break;
     case PWM_TYPE_MULTISHOT:
         timerMhzCounter = MULTISHOT_TIMER_MHZ;
-        pwmWritePtr = pwmWriteMultiShot;
+        pwmWrite = &pwmWriteMultiShot;
         break;
     case PWM_TYPE_BRUSHED:
         timerMhzCounter = PWM_BRUSHED_TIMER_MHZ;
-        pwmWritePtr = pwmWriteBrushed;
+        pwmWrite = &pwmWriteBrushed;
         useUnsyncedPwm = true;
         idlePulse = 0;
         break;
     case PWM_TYPE_STANDARD:
         timerMhzCounter = PWM_TIMER_MHZ;
-        pwmWritePtr = pwmWriteStandard;
+        pwmWrite = &pwmWriteStandard;
         useUnsyncedPwm = true;
         idlePulse = 0;
         break;
 #ifdef USE_DSHOT
     case PWM_TYPE_PROSHOT1000:
-        pwmWritePtr = pwmWriteProShot;
+        pwmWrite = &pwmWriteDigital;
-        pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
+        loadDmaBuffer = &loadDmaBufferProshot;
+        pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate;
         isDigital = true;
         break;
     case PWM_TYPE_DSHOT1200:
     case PWM_TYPE_DSHOT600:
     case PWM_TYPE_DSHOT300:
     case PWM_TYPE_DSHOT150:
-        pwmWritePtr = pwmWriteDshot;
+        pwmWrite = &pwmWriteDigital;
-        pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
+        loadDmaBuffer = &loadDmaBufferDshot;
+        pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate;
         isDigital = true;
         break;
 #endif
     }
 
     if (!isDigital) {
-        pwmCompleteWritePtr = useUnsyncedPwm ? pwmCompleteWriteUnused : pwmCompleteOneshotMotorUpdate;
+        pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
     }
 
     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
@@ -273,8 +304,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
 
         if (timerHardware == NULL) {
             /* not enough motors initialised for the mixer or a break in the motors */
-            pwmWritePtr = pwmWriteUnused;
+            pwmWrite = &pwmWriteUnused;
-            pwmCompleteWritePtr = pwmCompleteWriteUnused;
+            pwmCompleteWrite = &pwmCompleteWriteUnused;
             /* TODO: block arming and add reason system cannot arm */
             return;
         }
@@ -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,
@@ -131,8 +133,8 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
 extern bool pwmMotorsEnabled;
 
 struct timerHardware_s;
-typedef void(*pwmWriteFuncPtr)(uint8_t index, float value);  // function pointer used to write motors
+typedef void pwmWriteFunc(uint8_t index, float value);  // function pointer used to write motors
-typedef void(*pwmCompleteWriteFuncPtr)(uint8_t motorCount);   // function pointer used after motors are written
+typedef void pwmCompleteWriteFunc(uint8_t motorCount);   // function pointer used after motors are written
 
 typedef struct {
     volatile timCCR_t *ccr;
@@ -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 loadDmaBufferFunc(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 loadDmaBufferFunc *loadDmaBuffer;
+
 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 = loadDmaBuffer(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 = loadDmaBuffer(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: