diff --git a/src/main/build/debug.c b/src/main/build/debug.c index bbe22206d3..e7177cdfe8 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -78,4 +78,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "ANTI_GRAVITY", "DYN_LPF", "RX_SPEKTRUM_SPI", + "DSHOT_TELEMETRY", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 0cc50a66f1..291cef84f0 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -96,6 +96,7 @@ typedef enum { DEBUG_ANTI_GRAVITY, DEBUG_DYN_LPF, DEBUG_RX_SPEKTRUM_SPI, + DEBUG_RPM_TELEMETRY, DEBUG_COUNT } debugType_e; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index ddd1e5b0c0..3d7edab960 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -34,6 +34,9 @@ static FAST_RAM_ZERO_INIT pwmWriteFn *pwmWrite; static FAST_RAM_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; static FAST_RAM_ZERO_INIT pwmCompleteWriteFn *pwmCompleteWrite = NULL; +#ifdef USE_DSHOT_TELEMETRY +static FAST_RAM_ZERO_INIT pwmStartWriteFn *pwmStartWrite = NULL; +#endif #ifdef USE_DSHOT FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer; @@ -67,6 +70,9 @@ static bool isDshot = false; #ifdef USE_DSHOT_DMAR FAST_RAM_ZERO_INIT bool useBurstDshot = false; #endif +#ifdef USE_DSHOT_TELEMETRY +FAST_RAM_ZERO_INIT bool useDshotTelemetry = false; +#endif static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) { @@ -163,6 +169,8 @@ static FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uin dmaBuffer[i * stride] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first packet <<= 1; } + dmaBuffer[16 * stride] = 0; + dmaBuffer[17 * stride] = 0; return DSHOT_DMA_BUFFER_SIZE; } @@ -173,6 +181,8 @@ static uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t pa dmaBuffer[i * stride] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first packet <<= 4; // Shift 4 bits } + dmaBuffer[4 * stride] = 0; + dmaBuffer[5 * stride] = 0; return PROSHOT_DMA_BUFFER_SIZE; } @@ -210,6 +220,13 @@ bool pwmAreMotorsEnabled(void) return pwmMotorsEnabled; } +#ifdef USE_DSHOT_TELEMETRY +static void pwmStartWriteUnused(uint8_t motorCount) +{ + UNUSED(motorCount); +} +#endif + static void pwmCompleteWriteUnused(uint8_t motorCount) { UNUSED(motorCount); @@ -232,6 +249,13 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) pwmCompleteWrite(motorCount); } +#ifdef USE_DSHOT_TELEMETRY +void pwmStartMotorUpdate(uint8_t motorCount) +{ + pwmStartWrite(motorCount); +} +#endif + void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) { memset(motors, 0, sizeof(motors)); @@ -270,6 +294,10 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 pwmWrite = &pwmWriteDshot; loadDmaBuffer = &loadDmaBufferProshot; pwmCompleteWrite = &pwmCompleteDshotMotorUpdate; +#ifdef USE_DSHOT_TELEMETRY + pwmStartWrite = &pwmStartDshotMotorUpdate; + useDshotTelemetry = motorConfig->useDshotTelemetry; +#endif isDshot = true; break; case PWM_TYPE_DSHOT1200: @@ -279,6 +307,10 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 pwmWrite = &pwmWriteDshot; loadDmaBuffer = &loadDmaBufferDshot; pwmCompleteWrite = &pwmCompleteDshotMotorUpdate; +#ifdef USE_DSHOT_TELEMETRY + pwmStartWrite = &pwmStartDshotMotorUpdate; + useDshotTelemetry = motorConfig->useDshotTelemetry; +#endif isDshot = true; #ifdef USE_DSHOT_DMAR if (motorConfig->useBurstDshot) { @@ -292,6 +324,9 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 if (!isDshot) { pwmWrite = &pwmWriteStandard; pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate; +#ifdef USE_DSHOT_TELEMETRY + pwmStartWrite = pwmStartWriteUnused; +#endif } for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { @@ -430,6 +465,8 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo case DSHOT_CMD_SAVE_SETTINGS: case DSHOT_CMD_SPIN_DIRECTION_NORMAL: case DSHOT_CMD_SPIN_DIRECTION_REVERSED: + case DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE: + case DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY: repeats = 10; break; case DSHOT_CMD_BEACON1: @@ -448,6 +485,9 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo for (; repeats; repeats--) { delayMicroseconds(DSHOT_COMMAND_DELAY_US); +#ifdef USE_DSHOT_TELEMETRY + pwmStartDshotMotorUpdate(motorCount); +#endif for (uint8_t i = 0; i < motorCount; i++) { if ((i == index) || (index == ALL_MOTORS)) { motorDmaOutput_t *const motor = getMotorDmaOutput(i); diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 7b42bdb9a1..74e9777a96 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -28,8 +28,9 @@ #define ALL_MOTORS 255 - #define DSHOT_MAX_COMMAND 47 +#define DSHOT_TELEMETRY_INPUT_LEN 32 +#define PROSHOT_TELEMETRY_INPUT_LEN 8 /* DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings @@ -66,6 +67,8 @@ typedef enum { DSHOT_CMD_LED3_OFF, // BLHeli32 only DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF = 30, // KISS audio Stream mode on/Off DSHOT_CMD_SILENT_MODE_ON_OFF = 31, // KISS silent Mode on/Off + DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE = 32, + DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY = 33, DSHOT_CMD_MAX = 47 } dshotCommands_e; @@ -100,7 +103,7 @@ typedef enum { #define MOTOR_BIT_0 7 #define MOTOR_BIT_1 14 -#define MOTOR_BITLENGTH 19 +#define MOTOR_BITLENGTH 20 #define MOTOR_PROSHOT1000_HZ MHZ_TO_HZ(24) #define PROSHOT_BASE_SYMBOL 24 // 1uS @@ -133,14 +136,35 @@ typedef struct { #ifdef USE_DSHOT uint16_t timerDmaSource; bool configured; + uint8_t output; + uint8_t index; +#ifdef USE_DSHOT_TELEMETRY + bool useProshot; + volatile bool isInput; + volatile bool hasTelemetry; + uint16_t dshotTelemetryValue; + TIM_OCInitTypeDef ocInitStruct; + TIM_ICInitTypeDef icInitStruct; + DMA_InitTypeDef dmaInitStruct; + uint8_t dmaInputLen; +#ifdef STM32F3 + DMA_Channel_TypeDef *dmaRef; +#else + DMA_Stream_TypeDef *dmaRef; +#endif +#endif #endif motorDmaTimer_t *timer; volatile bool requestTelemetry; +#ifdef USE_DSHOT_TELEMETRY + uint32_t dmaBuffer[DSHOT_TELEMETRY_INPUT_LEN]; +#else #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) uint32_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; #else uint8_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; #endif +#endif } motorDmaOutput_t; motorDmaOutput_t *getMotorDmaOutput(uint8_t index); @@ -148,6 +172,7 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index); struct timerHardware_s; typedef void pwmWriteFn(uint8_t index, float value); // function pointer used to write motors typedef void pwmCompleteWriteFn(uint8_t motorCount); // function pointer used after motors are written +typedef void pwmStartWriteFn(uint8_t motorCount); // function pointer used before motors are written typedef struct { volatile timCCR_t *ccr; @@ -170,10 +195,15 @@ typedef struct motorDevConfig_s { uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation uint8_t useUnsyncedPwm; uint8_t useBurstDshot; + uint8_t useDshotTelemetry; ioTag_t ioTags[MAX_SUPPORTED_MOTORS]; } motorDevConfig_t; extern bool useBurstDshot; +#ifdef USE_DSHOT_TELEMETRY +extern bool useDshotTelemetry; +extern uint32_t dshotInvalidPacketCount; +#endif void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, uint8_t motorCount); @@ -202,12 +232,16 @@ void pwmWriteDshotCommandControl(uint8_t index); void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking); void pwmWriteDshotInt(uint8_t index, uint16_t value); void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); +#ifdef USE_DSHOT_TELEMETRY +void pwmStartDshotMotorUpdate(uint8_t motorCount); +#endif void pwmCompleteDshotMotorUpdate(uint8_t motorCount); bool pwmDshotCommandIsQueued(void); bool pwmDshotCommandIsProcessing(void); uint8_t pwmGetDshotCommand(uint8_t index); bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount); +uint16_t getDshotTelemetry(uint8_t index); #endif @@ -221,6 +255,7 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, void pwmWriteMotor(uint8_t index, float value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteMotorUpdate(uint8_t motorCount); +void pwmStartMotorUpdate(uint8_t motorCount); void pwmWriteServo(uint8_t index, float value); diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index ee81dffb60..bba1ff6b0a 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -41,10 +41,25 @@ #include "dma.h" #include "rcc.h" +// TODO remove once debugging no longer needed +#ifdef USE_DSHOT_TELEMETRY +#include +#endif + static uint8_t dmaMotorTimerCount = 0; static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; +#ifdef USE_DSHOT_TELEMETRY +//FAST_RAM_ZERO_INIT bool dshotTelemetryEnabled; +uint32_t readDoneCount; + +// TODO remove once debugging no longer needed +uint32_t dshotInvalidPacketCount; +uint32_t inputBuffer[DSHOT_TELEMETRY_INPUT_LEN]; +uint32_t setDirectionMicros; +#endif + motorDmaOutput_t *getMotorDmaOutput(uint8_t index) { return &dmaMotors[index]; @@ -96,6 +111,196 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value) } } +#ifdef USE_DSHOT_TELEMETRY + +static void processInputIrq(motorDmaOutput_t * const motor) +{ + motor->hasTelemetry = true; + DMA_Cmd(motor->timerHardware->dmaRef, DISABLE); + TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE); + readDoneCount++; +} + +static uint16_t decodeDshotPacket(uint32_t buffer[]) +{ + uint32_t value = 0; + for (int i = 1; i < DSHOT_TELEMETRY_INPUT_LEN; i += 2) { + int diff = buffer[i] - buffer[i-1]; + value <<= 1; + if (diff > 0) { + if (diff >= 11) value |= 1; + } else { + if (diff >= -9) value |= 1; + } + } + + uint32_t csum = value; + csum = csum ^ (csum >> 8); // xor bytes + csum = csum ^ (csum >> 4); // xor nibbles + + if (csum & 0xf) { + return 0xffff; + } + return value >> 4; +} + +static uint16_t decodeProshotPacket(uint32_t buffer[]) +{ + uint32_t value = 0; + for (int i = 1; i < PROSHOT_TELEMETRY_INPUT_LEN; i += 2) { + const int proshotModulo = MOTOR_NIBBLE_LENGTH_PROSHOT; + int diff = ((buffer[i] + proshotModulo - buffer[i-1]) % proshotModulo) - PROSHOT_BASE_SYMBOL; + int nible; + if (diff < 0) { + nible = 0; + } else { + nible = (diff + PROSHOT_BIT_WIDTH / 2) / PROSHOT_BIT_WIDTH; + } + value <<= 4; + value |= nible; + } + + uint32_t csum = value; + csum = csum ^ (csum >> 8); // xor bytes + csum = csum ^ (csum >> 4); // xor nibbles + + if (csum & 0xf) { + return 0xffff; + } + return value >> 4; +} + +#endif + +static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor); + +inline FAST_CODE static void pwmDshotSetDirectionOutput( + motorDmaOutput_t * const motor, bool output +#ifndef USE_DSHOT_TELEMETRY + ,TIM_OCInitTypeDef *pOcInit, DMA_InitTypeDef* pDmaInit +#endif +) +{ +#if defined(STM32F4) || defined(STM32F7) + typedef DMA_Stream_TypeDef dmaStream_t; +#else + typedef DMA_Channel_TypeDef dmaStream_t; +#endif + +#ifdef USE_DSHOT_TELEMETRY + TIM_OCInitTypeDef* pOcInit = &motor->ocInitStruct; + DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct; +#endif + + const timerHardware_t * const timerHardware = motor->timerHardware; + TIM_TypeDef *timer = timerHardware->tim; + +#ifndef USE_DSHOT_TELEMETRY + dmaStream_t *dmaRef; + +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { + dmaRef = timerHardware->dmaTimUPRef; + } else +#endif + { + dmaRef = timerHardware->dmaRef; + } +#else + dmaStream_t *dmaRef = motor->dmaRef; +#endif + + DMA_DeInit(dmaRef); + +#ifdef USE_DSHOT_TELEMETRY + motor->isInput = !output; + if (!output) { + TIM_ICInit(timer, &motor->icInitStruct); + +#if defined(STM32F3) + motor->dmaInitStruct.DMA_DIR = DMA_DIR_PeripheralSRC; + motor->dmaInitStruct.DMA_M2M = DMA_M2M_Disable; +#elif defined(STM32F4) + motor->dmaInitStruct.DMA_DIR = DMA_DIR_PeripheralToMemory; +#endif + } else +#else + UNUSED(output); +#endif + { + timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Disable); + timerOCInit(timer, timerHardware->channel, pOcInit); + timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable); + +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { +#if defined(STM32F3) + pDmaInit->DMA_DIR = DMA_DIR_PeripheralDST; +#else + pDmaInit->DMA_DIR = DMA_DIR_MemoryToPeripheral; +#endif + } else +#endif + { +#if defined(STM32F3) + pDmaInit->DMA_DIR = DMA_DIR_PeripheralDST; + pDmaInit->DMA_M2M = DMA_M2M_Disable; +#elif defined(STM32F4) + pDmaInit->DMA_DIR = DMA_DIR_MemoryToPeripheral; +#endif + } + } + + DMA_Init(dmaRef, pDmaInit); + DMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE); +} + +#ifdef USE_DSHOT_TELEMETRY + +void pwmStartDshotMotorUpdate(uint8_t motorCount) +{ + if (useDshotTelemetry) { + for (int i = 0; i < motorCount; i++) { + if (dmaMotors[i].hasTelemetry) { + uint16_t value = dmaMotors[i].useProshot ? + decodeProshotPacket( + dmaMotors[i].dmaBuffer ) : + decodeDshotPacket( + dmaMotors[i].dmaBuffer ); + if (value != 0xffff) { + dmaMotors[i].dshotTelemetryValue = value; + if (i < 4) { + DEBUG_SET(DEBUG_RPM_TELEMETRY, i, value); + } + } else { + dshotInvalidPacketCount++; + if (i == 0) { + memcpy(inputBuffer,dmaMotors[i].dmaBuffer,sizeof(inputBuffer)); + } + } + dmaMotors[i].hasTelemetry = false; + } else { + TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE); + } + pwmDshotSetDirectionOutput(&dmaMotors[i], true); + } + for (int i = 0; i < motorCount; i++) { + if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) { + TIM_CCxNCmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerHardware->channel, TIM_CCxN_Enable); + } else { + TIM_CCxCmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerHardware->channel, TIM_CCx_Enable); + } + } + } +} + +uint16_t getDshotTelemetry(uint8_t index) +{ + return dmaMotors[index].dshotTelemetryValue; +} + +#endif + void pwmCompleteDshotMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); @@ -128,18 +333,34 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) { if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam]; - -#ifdef USE_DSHOT_DMAR - if (useBurstDshot) { - DMA_Cmd(motor->timerHardware->dmaTimUPRef, DISABLE); - TIM_DMACmd(motor->timerHardware->tim, TIM_DMA_Update, DISABLE); +#ifdef USE_DSHOT_TELEMETRY + uint32_t irqStart = micros(); + if (motor->isInput) { + processInputIrq(motor); } else #endif { - DMA_Cmd(motor->timerHardware->dmaRef, DISABLE); - TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE); - } +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { + DMA_Cmd(motor->timerHardware->dmaTimUPRef, DISABLE); + TIM_DMACmd(motor->timerHardware->tim, TIM_DMA_Update, DISABLE); + } else +#endif + { + DMA_Cmd(motor->timerHardware->dmaRef, DISABLE); + TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE); + } +#ifdef USE_DSHOT_TELEMETRY + if (useDshotTelemetry) { + pwmDshotSetDirectionOutput(motor, false); + DMA_SetCurrDataCounter(motor->dmaRef, motor->dmaInputLen); + DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); + TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, ENABLE); + setDirectionMicros = micros() - irqStart; + } +#endif + } DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); } } @@ -152,6 +373,16 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m typedef DMA_Channel_TypeDef dmaStream_t; #endif +#ifdef USE_DSHOT_TELEMETRY +#define OCINIT motor->ocInitStruct +#define DMAINIT motor->dmaInitStruct +#else + TIM_OCInitTypeDef ocInitStruct; + DMA_InitTypeDef dmaInitStruct; +#define OCINIT ocInitStruct +#define DMAINIT dmaInitStruct +#endif + dmaStream_t *dmaRef; #ifdef USE_DSHOT_DMAR @@ -167,10 +398,10 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m return; } - TIM_OCInitTypeDef TIM_OCInitStructure; - DMA_InitTypeDef DMA_InitStructure; - motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; +#ifdef USE_DSHOT_TELEMETRY + motor->useProshot = (pwmProtocolType == PWM_TYPE_PROSHOT1000); +#endif motor->timerHardware = timerHardware; TIM_TypeDef *timer = timerHardware->tim; @@ -183,7 +414,17 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m const uint8_t timerIndex = getTimerIndex(timer); const bool configureTimer = (timerIndex == dmaMotorTimerCount-1); - IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction); + uint8_t pupMode = 0; +#ifdef USE_DSHOT_TELEMETRY + if (!useDshotTelemetry) { + pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP; + } else +#endif + { + pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PuPd_UP : GPIO_PuPd_DOWN; + } + + IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, pupMode), timerHardware->alternateFunction); if (configureTimer) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; @@ -193,42 +434,37 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m TIM_Cmd(timer, DISABLE); TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1); - TIM_TimeBaseStructure.TIM_Period = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH; + TIM_TimeBaseStructure.TIM_Period = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); } - TIM_OCStructInit(&TIM_OCInitStructure); - TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + TIM_OCStructInit(&OCINIT); + OCINIT.TIM_OCMode = TIM_OCMode_PWM1; if (output & TIMER_OUTPUT_N_CHANNEL) { - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; - TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset; - TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; + OCINIT.TIM_OutputNState = TIM_OutputNState_Enable; + OCINIT.TIM_OCNIdleState = TIM_OCNIdleState_Reset; + OCINIT.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High; } else { - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; - TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; + OCINIT.TIM_OutputState = TIM_OutputState_Enable; + OCINIT.TIM_OCIdleState = TIM_OCIdleState_Set; + OCINIT.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High; } - TIM_OCInitStructure.TIM_Pulse = 0; + OCINIT.TIM_Pulse = 0; - timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure); - timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable); - - if (output & TIMER_OUTPUT_N_CHANNEL) { - TIM_CCxNCmd(timer, timerHardware->channel, TIM_CCxN_Enable); - } else { - TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable); - } - - if (configureTimer) { - TIM_CtrlPWMOutputs(timer, ENABLE); - TIM_ARRPreloadConfig(timer, ENABLE); - TIM_Cmd(timer, ENABLE); - } +#ifdef USE_DSHOT_TELEMETRY + TIM_ICStructInit(&motor->icInitStruct); + motor->icInitStruct.TIM_ICSelection = TIM_ICSelection_DirectTI; + motor->icInitStruct.TIM_ICPolarity = TIM_ICPolarity_BothEdge; + motor->icInitStruct.TIM_ICPrescaler = TIM_ICPSC_DIV1; + motor->icInitStruct.TIM_Channel = timerHardware->channel; + motor->icInitStruct.TIM_ICFilter = 0; //2; +#endif motor->timer = &dmaMotorTimers[timerIndex]; + motor->index = motorIndex; #ifdef USE_DSHOT_DMAR if (useBurstDshot) { @@ -247,67 +483,88 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m DMA_Cmd(dmaRef, DISABLE); DMA_DeInit(dmaRef); - DMA_StructInit(&DMA_InitStructure); + DMA_StructInit(&DMAINIT); #ifdef USE_DSHOT_DMAR if (useBurstDshot) { dmaInit(timerHardware->dmaTimUPIrqHandler, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim)); - dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); #if defined(STM32F3) - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)motor->timer->dmaBurstBuffer; - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; + DMAINIT.DMA_MemoryBaseAddr = (uint32_t)motor->timer->dmaBurstBuffer; + DMAINIT.DMA_DIR = DMA_DIR_PeripheralDST; #else - DMA_InitStructure.DMA_Channel = timerHardware->dmaTimUPChannel; - DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)motor->timer->dmaBurstBuffer; - DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; - DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Enable; - DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; - DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; - DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMAINIT.DMA_Channel = timerHardware->dmaTimUPChannel; + DMAINIT.DMA_Memory0BaseAddr = (uint32_t)motor->timer->dmaBurstBuffer; + DMAINIT.DMA_DIR = DMA_DIR_MemoryToPeripheral; + DMAINIT.DMA_FIFOMode = DMA_FIFOMode_Enable; + DMAINIT.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; + DMAINIT.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMAINIT.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; #endif - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&timerHardware->tim->DMAR; - DMA_InitStructure.DMA_BufferSize = (pwmProtocolType == PWM_TYPE_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; - DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; - DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMAINIT.DMA_PeripheralBaseAddr = (uint32_t)&timerHardware->tim->DMAR; + DMAINIT.DMA_BufferSize = (pwmProtocolType == PWM_TYPE_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX + DMAINIT.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMAINIT.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMAINIT.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; + DMAINIT.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMAINIT.DMA_Mode = DMA_Mode_Normal; + DMAINIT.DMA_Priority = DMA_Priority_High; } else #endif { dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); - dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); #if defined(STM32F3) - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)motor->dmaBuffer; - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; + DMAINIT.DMA_MemoryBaseAddr = (uint32_t)motor->dmaBuffer; + DMAINIT.DMA_DIR = DMA_DIR_PeripheralDST; + DMAINIT.DMA_M2M = DMA_M2M_Disable; #elif defined(STM32F4) - DMA_InitStructure.DMA_Channel = timerHardware->dmaChannel; - DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)motor->dmaBuffer; - DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; - DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Enable; - DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull; - DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; - DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMAINIT.DMA_Channel = timerHardware->dmaChannel; + DMAINIT.DMA_Memory0BaseAddr = (uint32_t)motor->dmaBuffer; + DMAINIT.DMA_DIR = DMA_DIR_MemoryToPeripheral; + DMAINIT.DMA_FIFOMode = DMA_FIFOMode_Enable; + DMAINIT.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull; + DMAINIT.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMAINIT.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; #endif - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware); - 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; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; - DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; - DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMAINIT.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware); + DMAINIT.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMAINIT.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMAINIT.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; + DMAINIT.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; + DMAINIT.DMA_Mode = DMA_Mode_Normal; + DMAINIT.DMA_Priority = DMA_Priority_High; } // XXX Consolidate common settings in the next refactor - DMA_Init(dmaRef, &DMA_InitStructure); - DMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE); +#ifdef USE_DSHOT_TELEMETRY + motor->dmaRef = dmaRef; + motor->dmaInputLen = motor->useProshot ? PROSHOT_TELEMETRY_INPUT_LEN : DSHOT_TELEMETRY_INPUT_LEN; + pwmDshotSetDirectionOutput(motor, true); +#else + pwmDshotSetDirectionOutput(motor, true, &OCINIT, &DMAINIT); +#endif +#ifdef USE_DSHOT_DMAR + if (useBurstDshot) { + dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(2, 1), motor->index); + } else +#endif + { + dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(2, 1), motor->index); + } + TIM_Cmd(timer, ENABLE); + if (output & TIMER_OUTPUT_N_CHANNEL) { + TIM_CCxNCmd(timer, timerHardware->channel, TIM_CCxN_Enable); + } else { + TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable); + } + if (configureTimer) { + TIM_ARRPreloadConfig(timer, ENABLE); + TIM_CtrlPWMOutputs(timer, ENABLE); + TIM_Cmd(timer, ENABLE); + } motor->configured = true; } diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index 6c62baff06..8b2b2a3af1 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -181,7 +181,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m LL_TIM_DisableCounter(timer); init.Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1); - init.Autoreload = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH; + init.Autoreload = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1; init.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; init.RepetitionCounter = 0; init.CounterMode = LL_TIM_COUNTERMODE_UP; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e31f72cfe4..3d95fadc24 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -405,6 +405,12 @@ static void validateAndFixConfig(void) #endif #endif +#if defined(USE_DSHOT_TELEMETRY) + if (motorConfig()->dev.useBurstDshot && motorConfig()->dev.useDshotTelemetry) { + motorConfigMutable()->dev.useDshotTelemetry = false; + } +#endif + #if defined(TARGET_VALIDATECONFIG) targetValidateConfiguration(); #endif diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 6f01bf5d45..f816eb3735 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -382,6 +382,14 @@ void tryArm(void) const timeUs_t currentTimeUs = micros(); #ifdef USE_DSHOT +#ifdef USE_DSHOT_TELEMETRY + pwmWriteDshotCommand( + 255, getMotorCount(), + motorConfig()->dev.useDshotTelemetry ? + DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY : + DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, true); +#endif + if (currentTimeUs - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) { if (tryingToArm == ARMING_DELAYED_DISARMED) { if (IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 608a3f7a7b..1541a745d8 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -86,6 +86,7 @@ #include "fc/tasks.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "fc/dispatch.h" #include "interface/cli.h" #include "interface/msp.h" @@ -205,6 +206,26 @@ static IO_t busSwitchResetPin = IO_NONE; } #endif +#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) +void activateDshotTelemetry(struct dispatchEntry_s* self) +{ + UNUSED(self); + if (!ARMING_FLAG(ARMED)) + { + pwmWriteDshotCommand( + 255, getMotorCount(), + motorConfig()->dev.useDshotTelemetry ? + DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY : + DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, true); + } +} + +dispatchEntry_t activateDshotTelemetryEntry = +{ + activateDshotTelemetry, 0, NULL, false +}; +#endif + void init(void) { #ifdef USE_ITCM_RAM @@ -748,7 +769,14 @@ void init(void) setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME); +// TODO: potentially delete when feature is stable. Activation when arming is enough for flight. +#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) + dispatchEnable(); + dispatchAdd(&activateDshotTelemetryEntry, 5000000); +#endif + fcTasksInit(); systemState |= SYSTEM_STATE_READY; + } diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d7841d40e0..47db2a71ea 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -507,6 +507,9 @@ void mixerResetDisarmedMotors(void) void writeMotors(void) { if (pwmAreMotorsEnabled()) { +#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) + pwmStartMotorUpdate(motorCount); +#endif for (int i = 0; i < motorCount; i++) { pwmWriteMotor(i, motor[i]); } diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index 29773c789b..71d7f93334 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -239,6 +239,14 @@ static const char * const *sensorHardwareNames[] = { }; #endif // USE_SENSOR_NAMES +#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) +extern uint32_t readDoneCount; +extern uint32_t inputBuffer[DSHOT_TELEMETRY_INPUT_LEN]; +extern uint32_t setDirectionMicros; +#endif + + + static void backupPgConfig(const pgRegistry_t *pg) { memcpy(pg->copy, pg->address, pg->size); @@ -3712,6 +3720,28 @@ static void cliStatus(char *cmdline) cliPrintf(" %s", armingDisableFlagNames[bitpos]); } cliPrintLinefeed(); +#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) + if (useDshotTelemetry) { + cliPrintLinef("Dshot reads: %u", readDoneCount); + cliPrintLinef("Dshot invalid pkts: %u", dshotInvalidPacketCount); + extern uint32_t setDirectionMicros; + cliPrintLinef("Dshot irq micros: %u", setDirectionMicros); + for (int i = 0; idev.motorPwmProtocol == PWM_TYPE_PROSHOT1000); + int modulo = proshot ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH; + int len = proshot ? 8 : DSHOT_TELEMETRY_INPUT_LEN; + for (int i=0; i