Merge pull request #3162 from TonyBazz/master

ProShot Implementation

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):
+    case(PWM_TYPE_PROSHOT1000):
-            return MOTOR_DSHOT1200_MHZ * 1000000;
+        return MOTOR_PROSHOT1000_MHZ * 1000000;
-        case(PWM_TYPE_DSHOT600):
+    case(PWM_TYPE_DSHOT1200):
-            return MOTOR_DSHOT600_MHZ * 1000000;
+        return MOTOR_DSHOT1200_MHZ * 1000000;
-        case(PWM_TYPE_DSHOT300):
+    case(PWM_TYPE_DSHOT600):
-            return MOTOR_DSHOT300_MHZ * 1000000;
+        return MOTOR_DSHOT600_MHZ * 1000000;
-        default:
+    case(PWM_TYPE_DSHOT300):
-        case(PWM_TYPE_DSHOT150):
+        return MOTOR_DSHOT300_MHZ * 1000000;
-            return MOTOR_DSHOT150_MHZ * 1000000;
+    default:
+    case(PWM_TYPE_DSHOT150):
+        return MOTOR_DSHOT150_MHZ * 1000000;
     }
 }
 

src/main/drivers/pwm_output.h

@@ -59,6 +59,7 @@ typedef enum {
     PWM_TYPE_DSHOT300,
     PWM_TYPE_DSHOT600,
     PWM_TYPE_DSHOT1200,
+    PWM_TYPE_PROSHOT1000,
 #endif
     PWM_TYPE_MAX
 } motorPwmProtocolTypes_e;
@@ -76,6 +77,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
@@ -95,7 +101,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;
@@ -109,9 +116,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;
@@ -160,7 +167,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

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;

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.Prescaler = (timerClock(timer) / getDshotHz(pwmProtocolType)) - 1;
-    motor->TimHandle.Init.Period = MOTOR_BITLENGTH;
+    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;