Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

Dshot bidir blhelis support (#8554)

Browse source 
Dshot bidir blhelis support
This commit is contained in:
Michael Keller 2019-08-04 18:29:09 +12:00 • committed by GitHub
commit 076be798f8
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 173 additions and 171 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/cli/cli.c
View file

@ -261,7 +261,7 @@ static const char * const *sensorHardwareNames[] = {
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
extern uint32_t readDoneCount;
extern uint32_t inputBuffer[DSHOT_TELEMETRY_INPUT_LEN];
extern uint32_t inputBuffer[GCR_TELEMETRY_INPUT_LEN];
extern uint32_t setDirectionMicros;
#endif
@ -5680,15 +5680,13 @@ static void cliDshotTelemetryInfo(char *cmdline)
}
cliPrintLinefeed();
const bool proshot = (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_PROSHOT1000);
const int modulo = proshot ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH;
const int len = proshot ? 8 : DSHOT_TELEMETRY_INPUT_LEN;
const int len = MAX_GCR_EDGES;
for (int i = 0; i < len; i++) {
cliPrintf("%u ", (int)inputBuffer[i]);
}
cliPrintLinefeed();
for (int i = 1; i < len; i+=2) {
cliPrintf("%u ", (int)(inputBuffer[i] + modulo - inputBuffer[i-1]) % modulo);
for (int i = 1; i < len; i++) {
cliPrintf("%u ", (int)(inputBuffer[i] - inputBuffer[i-1]));
}
cliPrintLinefeed();
} else {

6
src/main/drivers/dshot_dpwm.c
View file

@ -112,6 +112,12 @@ static void dshotPwmDisableMotors(void)
static bool dshotPwmEnableMotors(void)
{
for (int i = 0; i < dshotPwmDevice.count; i++) {
motorDmaOutput_t *motor = getMotorDmaOutput(i);
const IO_t motorIO = IOGetByTag(motor->timerHardware->tag);
IOConfigGPIOAF(motorIO, motor->iocfg, motor->timerHardware->alternateFunction);
}
// No special processing required
return true;
}

25
src/main/drivers/dshot_dpwm.h
View file

@ -36,7 +36,11 @@
#define PROSHOT_BIT_WIDTH 3
#define MOTOR_NIBBLE_LENGTH_PROSHOT (PROSHOT_BASE_SYMBOL * 4) // 4uS
#define DSHOT_TELEMETRY_DEADTIME_US (2 * 30 + 10) // 2 * 30uS to switch lines plus 10us grace period
#define DSHOT_TELEMETRY_DEADTIME_US (30 + 5) // 30 to switch lines and 5 to switch lines back
#define MIN_GCR_EDGES 7
#define MAX_GCR_EDGES 22
typedef uint8_t loadDmaBufferFn(uint32_t *dmaBuffer, int stride, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation
extern FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
@ -55,8 +59,7 @@ motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint1
#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
#define PROSHOT_DMA_BUFFER_SIZE 6 /* resolution + frame reset (2us) */
#define DSHOT_TELEMETRY_INPUT_LEN 32
#define PROSHOT_TELEMETRY_INPUT_LEN 8
#define GCR_TELEMETRY_INPUT_LEN MAX_GCR_EDGES
// For H7, DMA buffer is placed in a dedicated segment for coherency management
#if defined(STM32H7)
@ -64,7 +67,7 @@ motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint1
#elif defined(STM32F7)
#define DSHOT_DMA_BUFFER_ATTRIBUTE FAST_RAM_ZERO_INIT
#else
#define DSHOT_DMA_BUFFER_ATTRIBUTE // None
#define DSHOT_DMA_BUFFER_ATTRIBUTE
#endif
#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
@ -74,13 +77,14 @@ motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint1
#endif
#ifdef USE_DSHOT_TELEMETRY
STATIC_ASSERT(DSHOT_TELEMETRY_INPUT_LEN >= DSHOT_DMA_BUFFER_SIZE, dshotBufferSizeConstrait);
#define DSHOT_DMA_BUFFER_ALLOC_SIZE DSHOT_TELEMETRY_INPUT_LEN
STATIC_ASSERT(GCR_TELEMETRY_INPUT_LEN >= DSHOT_DMA_BUFFER_SIZE, dshotBufferSizeConstrait);
#define DSHOT_DMA_BUFFER_ALLOC_SIZE GCR_TELEMETRY_INPUT_LEN
#else
#define DSHOT_DMA_BUFFER_ALLOC_SIZE DSHOT_DMA_BUFFER_SIZE
#endif
extern DSHOT_DMA_BUFFER_UNIT dshotDmaBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_ALLOC_SIZE];
extern DSHOT_DMA_BUFFER_UNIT dshotDmaInputBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_ALLOC_SIZE];
#ifdef USE_DSHOT_DMAR
extern DSHOT_DMA_BUFFER_UNIT dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
@ -88,7 +92,9 @@ extern DSHOT_DMA_BUFFER_UNIT dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFE
typedef struct {
TIM_TypeDef *timer;
#if defined(USE_DSHOT) && defined(USE_DSHOT_DMAR)
#if defined(USE_DSHOT)
uint16_t outputPeriod;
#if defined(USE_DSHOT_DMAR)
#if defined(STM32F7) || defined(STM32H7)
TIM_HandleTypeDef timHandle;
DMA_HandleTypeDef hdma_tim;
@ -96,7 +102,7 @@ typedef struct {
dmaResource_t *dmaBurstRef;
uint16_t dmaBurstLength;
uint32_t *dmaBurstBuffer;
timeUs_t inputDirectionStampUs;
#endif
#endif
uint16_t timerDmaSources;
} motorDmaTimer_t;
@ -115,6 +121,7 @@ typedef struct motorDmaOutput_s {
#endif
uint8_t output;
uint8_t index;
uint32_t iocfg;
#if defined(USE_HAL_DRIVER) && defined(USE_FULL_LL_DRIVER)
LL_DMA_InitTypeDef dmaInitStruct;
@ -124,9 +131,7 @@ typedef struct motorDmaOutput_s {
#endif
#ifdef USE_DSHOT_TELEMETRY
bool useProshot;
volatile bool isInput;
volatile bool hasTelemetry;
uint16_t dshotTelemetryValue;
timeDelta_t dshotTelemetryDeadtimeUs;
bool dshotTelemetryActive;

81
src/main/drivers/pwm_output_dshot.c
View file

@ -50,14 +50,6 @@
#ifdef USE_DSHOT_TELEMETRY
static void processInputIrq(motorDmaOutput_t * const motor)
{
motor->hasTelemetry = true;
xDMA_Cmd(motor->dmaRef, DISABLE);
TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
readDoneCount++;
}
void dshotEnableChannels(uint8_t motorCount)
{
for (int i = 0; i < motorCount; i++) {
@ -101,7 +93,12 @@ FAST_CODE void pwmDshotSetDirectionOutput(
#ifdef USE_DSHOT_TELEMETRY
if (!output) {
motor->isInput = true;
motor->timer->inputDirectionStampUs = micros();
if (!inputStampUs) {
inputStampUs = micros();
}
TIM_ARRPreloadConfig(timer, ENABLE);
timer->ARR = 0xffffffff;
TIM_ICInit(timer, &motor->icInitStruct);
#if defined(STM32F3)
@ -142,7 +139,9 @@ FAST_CODE void pwmDshotSetDirectionOutput(
}
xDMA_Init(dmaRef, pDmaInit);
xDMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE);
if (output) {
xDMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE);
}
}
@ -165,6 +164,9 @@ void pwmCompleteDshotMotorUpdate(void)
} else
#endif
{
TIM_ARRPreloadConfig(dmaMotorTimers[i].timer, DISABLE);
dmaMotorTimers[i].timer->ARR = dmaMotorTimers[i].outputPeriod;
TIM_ARRPreloadConfig(dmaMotorTimers[i].timer, ENABLE);
TIM_SetCounter(dmaMotorTimers[i].timer, 0);
TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
dmaMotorTimers[i].timerDmaSources = 0;
@ -178,32 +180,27 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
#ifdef USE_DSHOT_TELEMETRY
uint32_t irqStart = micros();
if (motor->isInput) {
processInputIrq(motor);
#endif
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
xDMA_Cmd(motor->timerHardware->dmaTimUPRef, DISABLE);
TIM_DMACmd(motor->timerHardware->tim, TIM_DMA_Update, DISABLE);
} else
#endif
{
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
xDMA_Cmd(motor->timerHardware->dmaTimUPRef, DISABLE);
TIM_DMACmd(motor->timerHardware->tim, TIM_DMA_Update, DISABLE);
} else
#endif
{
xDMA_Cmd(motor->dmaRef, DISABLE);
TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
}
xDMA_Cmd(motor->dmaRef, DISABLE);
TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
}
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
pwmDshotSetDirectionOutput(motor, false);
xDMA_SetCurrDataCounter(motor->dmaRef, motor->dmaInputLen);
xDMA_Cmd(motor->dmaRef, ENABLE);
TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, ENABLE);
setDirectionMicros = micros() - irqStart;
}
#endif
if (useDshotTelemetry) {
pwmDshotSetDirectionOutput(motor, false);
xDMA_SetCurrDataCounter(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
xDMA_Cmd(motor->dmaRef, ENABLE);
TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, ENABLE);
setDirectionMicros = micros() - irqStart;
}
#endif
DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
}
}
@ -254,13 +251,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
}
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;
const IO_t motorIO = IOGetByTag(timerHardware->tag);
// Boolean configureTimer is always true when different channels of the same timer are processed in sequence,
// causing the timer and the associated DMA initialized more than once.
@ -269,6 +260,12 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
const uint8_t timerIndex = getTimerIndex(timer);
const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
motor->timer = &dmaMotorTimers[timerIndex];
motor->index = motorIndex;
motor->timerHardware = timerHardware;
const IO_t motorIO = IOGetByTag(timerHardware->tag);
uint8_t pupMode = 0;
pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP;
#ifdef USE_DSHOT_TELEMETRY
@ -277,7 +274,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
}
#endif
IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, pupMode), timerHardware->alternateFunction);
motor->iocfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, pupMode);
IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
if (configureTimer) {
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
@ -313,11 +311,9 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
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;
motor->icInitStruct.TIM_ICFilter = 2;
#endif
motor->timer = &dmaMotorTimers[timerIndex];
motor->index = motorIndex;
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
@ -393,10 +389,9 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
motor->dmaRef = dmaRef;
#ifdef USE_DSHOT_TELEMETRY
motor->dmaInputLen = motor->useProshot ? PROSHOT_TELEMETRY_INPUT_LEN : DSHOT_TELEMETRY_INPUT_LEN;
motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 *
( 2 + (motor->useProshot ? 4 * MOTOR_NIBBLE_LENGTH_PROSHOT : 16 * MOTOR_BITLENGTH))
/ getDshotHz(pwmProtocolType);
(16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType);
motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
pwmDshotSetDirectionOutput(motor, true);
#else
pwmDshotSetDirectionOutput(motor, true, &OCINIT, &DMAINIT);

64
src/main/drivers/pwm_output_dshot_hal.c
View file

@ -53,23 +53,6 @@
#ifdef USE_DSHOT_TELEMETRY
static void processInputIrq(motorDmaOutput_t * const motor)
{
motor->hasTelemetry = true;
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
xLL_EX_DMA_DisableResource(motor->timerHardware->dmaTimUPRef);
LL_TIM_DisableDMAReq_UPDATE(motor->timerHardware->tim);
} else
#endif
{
xLL_EX_DMA_DisableResource(motor->dmaRef);
LL_EX_TIM_DisableIT(motor->timerHardware->tim, motor->timerDmaSource);
}
readDoneCount++;
}
void dshotEnableChannels(uint8_t motorCount)
{
for (int i = 0; i < motorCount; i++) {
@ -83,7 +66,6 @@ void dshotEnableChannels(uint8_t motorCount)
#endif
static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor);
void pwmDshotSetDirectionOutput(
@ -106,7 +88,11 @@ void pwmDshotSetDirectionOutput(
#ifdef USE_DSHOT_TELEMETRY
if (!output) {
motor->isInput = true;
motor->timer->inputDirectionStampUs = micros();
if (!inputStampUs) {
inputStampUs = micros();
}
LL_TIM_EnableARRPreload(timer); // Only update the period once all channels are done
timer->ARR = 0xffffffff;
LL_TIM_IC_Init(timer, motor->llChannel, &motor->icInitStruct);
motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
} else
@ -124,7 +110,9 @@ void pwmDshotSetDirectionOutput(
motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
}
xLL_EX_DMA_Init(motor->dmaRef, pDmaInit);
xLL_EX_DMA_EnableIT_TC(motor->dmaRef);
if (output) {
xLL_EX_DMA_EnableIT_TC(motor->dmaRef);
}
}
@ -148,6 +136,9 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(void)
} else
#endif
{
LL_TIM_DisableARRPreload(dmaMotorTimers[i].timer);
dmaMotorTimers[i].timer->ARR = dmaMotorTimers[i].outputPeriod;
/* Reset timer counter */
LL_TIM_SetCounter(dmaMotorTimers[i].timer, 0);
/* Enable channel DMA requests */
@ -161,13 +152,10 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
{
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
if (!motor->isInput) {
#ifdef USE_DSHOT_TELEMETRY
uint32_t irqStart = micros();
if (motor->isInput) {
processInputIrq(motor);
} else
uint32_t irqStartUs = micros();
#endif
{
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
xLL_EX_DMA_DisableResource(motor->timerHardware->dmaTimUPRef);
@ -182,10 +170,10 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
pwmDshotSetDirectionOutput(motor, false);
xLL_EX_DMA_SetDataLength(motor->dmaRef, motor->dmaInputLen);
xLL_EX_DMA_SetDataLength(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
xLL_EX_DMA_EnableResource(motor->dmaRef);
LL_EX_TIM_EnableIT(motor->timerHardware->tim, motor->timerDmaSource);
setDirectionMicros = micros() - irqStart;
setDirectionMicros = micros() - irqStartUs;
}
#endif
}
@ -231,26 +219,27 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
}
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
#ifdef USE_DSHOT_TELEMETRY
motor->useProshot = (pwmProtocolType == PWM_TYPE_PROSHOT1000);
#endif
motor->timerHardware = timerHardware;
motor->dmaRef = dmaRef;
TIM_TypeDef *timer = timerHardware->tim;
const IO_t motorIO = IOGetByTag(timerHardware->tag);
const uint8_t timerIndex = getTimerIndex(timer);
const bool configureTimer = (timerIndex == dmaMotorTimerCount - 1);
motor->timer = &dmaMotorTimers[timerIndex];
motor->index = motorIndex;
const IO_t motorIO = IOGetByTag(timerHardware->tag);
uint8_t pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PULLDOWN : GPIO_PULLUP;
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
output ^= TIMER_OUTPUT_INVERTED;
}
#endif
motor->timerHardware = timerHardware;
IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, pupMode), timerHardware->alternateFunction);
motor->iocfg = IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, pupMode);
IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
if (configureTimer) {
LL_TIM_InitTypeDef init;
@ -284,7 +273,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
LL_TIM_IC_StructInit(&motor->icInitStruct);
motor->icInitStruct.ICPolarity = LL_TIM_IC_POLARITY_BOTHEDGE;
motor->icInitStruct.ICPrescaler = LL_TIM_ICPSC_DIV1;
motor->icInitStruct.ICFilter = 0; //2;
motor->icInitStruct.ICFilter = 2;
#endif
uint32_t channel = 0;
@ -295,8 +284,6 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
case TIM_CHANNEL_4: channel = LL_TIM_CHANNEL_CH4; break;
}
motor->llChannel = channel;
motor->timer = &dmaMotorTimers[timerIndex];
motor->index = motorIndex;
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
@ -355,10 +342,9 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
motor->dmaRef = dmaRef;
#ifdef USE_DSHOT_TELEMETRY
motor->dmaInputLen = motor->useProshot ? PROSHOT_TELEMETRY_INPUT_LEN : DSHOT_TELEMETRY_INPUT_LEN;
motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 *
( 2 + (motor->useProshot ? 4 * MOTOR_NIBBLE_LENGTH_PROSHOT : 16 * MOTOR_BITLENGTH))
/ getDshotHz(pwmProtocolType);
( 16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType);
motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
pwmDshotSetDirectionOutput(motor, true);
#else
pwmDshotSetDirectionOutput(motor, true, &OCINIT, &DMAINIT);

155
src/main/drivers/pwm_output_dshot_shared.c
View file

@ -79,8 +79,9 @@ uint32_t readDoneCount;
// TODO remove once debugging no longer needed
FAST_RAM_ZERO_INIT uint32_t dshotInvalidPacketCount;
FAST_RAM_ZERO_INIT uint32_t inputBuffer[DSHOT_TELEMETRY_INPUT_LEN];
FAST_RAM_ZERO_INIT uint32_t inputBuffer[GCR_TELEMETRY_INPUT_LEN];
FAST_RAM_ZERO_INIT uint32_t setDirectionMicros;
FAST_RAM_ZERO_INIT uint32_t inputStampUs;
#endif
motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
@ -152,56 +153,65 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
void dshotEnableChannels(uint8_t motorCount);
static uint16_t decodeDshotPacket(uint32_t buffer[])
static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
{
uint32_t start = micros();
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;
uint32_t oldValue = buffer[0];
int bits = 0;
int len;
for (uint32_t i = 1; i <= count; i++) {
if (i < count) {
int diff = buffer[i] - oldValue;
if (bits >= 21) {
break;
}
len = (diff + 8) / 16;
} else {
if (diff >= -9) value |= 1;
len = 21 - bits;
}
value <<= len;
value |= 1 << (len - 1);
oldValue = buffer[i];
bits += len;
}
if (bits != 21) {
return 0xffff;
}
uint32_t csum = value;
static const uint32_t decode[32] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15,
0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 };
uint32_t decodedValue = decode[value & 0x1f];
decodedValue |= decode[(value >> 5) & 0x1f] << 4;
decodedValue |= decode[(value >> 10) & 0x1f] << 8;
decodedValue |= decode[(value >> 15) & 0x1f] << 12;
uint32_t csum = decodedValue;
csum = csum ^ (csum >> 8); // xor bytes
csum = csum ^ (csum >> 4); // xor nibbles
if ((csum & 0xf) != 0xf) {
setDirectionMicros = micros() - start;
return 0xffff;
}
return value >> 4;
}
decodedValue >>= 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 nibble;
if (diff < 0) {
nibble = 0;
} else {
nibble = (diff + PROSHOT_BIT_WIDTH / 2) / PROSHOT_BIT_WIDTH;
}
value <<= 4;
value |= (nibble & 0xf);
if (decodedValue == 0x0fff) {
setDirectionMicros = micros() - start;
return 0;
}
uint32_t csum = value;
csum = csum ^ (csum >> 8); // xor bytes
csum = csum ^ (csum >> 4); // xor nibbles
if ((csum & 0xf) != 0xf) {
decodedValue = (decodedValue & 0x000001ff) << ((decodedValue & 0xfffffe00) >> 9);
if (!decodedValue) {
return 0xffff;
}
return value >> 4;
uint32_t ret = (1000000 * 60 / 100 + decodedValue / 2) / decodedValue;
setDirectionMicros = micros() - start;
return ret;
}
uint16_t getDshotTelemetry(uint8_t index)
{
return dmaMotors[index].dshotTelemetryValue;
@ -241,7 +251,7 @@ void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool pac
}
#endif // USE_DSHOT_TELEMETRY_STATS
bool pwmStartDshotMotorUpdate(void)
FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
{
if (!useDshotTelemetry) {
return true;
@ -249,56 +259,57 @@ bool pwmStartDshotMotorUpdate(void)
#ifdef USE_DSHOT_TELEMETRY_STATS
const timeMs_t currentTimeMs = millis();
#endif
const timeUs_t currentUs = micros();
for (int i = 0; i < dshotPwmDevice.count; i++) {
if (dmaMotors[i].hasTelemetry) {
timeDelta_t usSinceInput = cmpTimeUs(currentUs, inputStampUs);
if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) {
return false;
}
if (dmaMotors[i].isInput) {
#ifdef USE_FULL_LL_DRIVER
uint32_t edges = xLL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef);
uint32_t edges = GCR_TELEMETRY_INPUT_LEN - xLL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef);
#else
uint32_t edges = xDMA_GetCurrDataCounter(dmaMotors[i].dmaRef);
uint32_t edges = GCR_TELEMETRY_INPUT_LEN - xDMA_GetCurrDataCounter(dmaMotors[i].dmaRef);
#endif
uint16_t value = 0xffff;
if (edges == 0) {
if (dmaMotors[i].useProshot) {
value = decodeProshotPacket(dmaMotors[i].dmaBuffer);
} else {
value = decodeDshotPacket(dmaMotors[i].dmaBuffer);
}
}
#ifdef USE_DSHOT_TELEMETRY_STATS
bool validTelemetryPacket = false;
#endif
if (value != 0xffff) {
dmaMotors[i].dshotTelemetryValue = value;
dmaMotors[i].dshotTelemetryActive = true;
if (i < 4) {
DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value);
}
#ifdef USE_DSHOT_TELEMETRY_STATS
validTelemetryPacket = true;
#endif
} else {
dshotInvalidPacketCount++;
if (i == 0) {
memcpy(inputBuffer,dmaMotors[i].dmaBuffer,sizeof(inputBuffer));
}
}
dmaMotors[i].hasTelemetry = false;
#ifdef USE_DSHOT_TELEMETRY_STATS
updateDshotTelemetryQuality(&dshotTelemetryQuality[i], validTelemetryPacket, currentTimeMs);
#endif
} else {
timeDelta_t usSinceInput = cmpTimeUs(micros(), dmaMotors[i].timer->inputDirectionStampUs);
if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) {
return false;
}
#ifdef USE_FULL_LL_DRIVER
LL_EX_TIM_DisableIT(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource);
#else
TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE);
#endif
uint16_t value = 0xffff;
if (edges > MIN_GCR_EDGES) {
readDoneCount++;
value = decodeTelemetryPacket(dmaMotors[i].dmaBuffer, edges);
#ifdef USE_DSHOT_TELEMETRY_STATS
bool validTelemetryPacket = false;
#endif
if (value != 0xffff) {
dmaMotors[i].dshotTelemetryValue = value;
dmaMotors[i].dshotTelemetryActive = true;
if (i < 4) {
DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value);
}
#ifdef USE_DSHOT_TELEMETRY_STATS
validTelemetryPacket = true;
#endif
} else {
dshotInvalidPacketCount++;
if (i == 0) {
memcpy(inputBuffer,dmaMotors[i].dmaBuffer,sizeof(inputBuffer));
}
}
#ifdef USE_DSHOT_TELEMETRY_STATS
updateDshotTelemetryQuality(&dshotTelemetryQuality[i], validTelemetryPacket, currentTimeMs);
}
#endif
}
pwmDshotSetDirectionOutput(&dmaMotors[i], true);
}
inputStampUs = 0;
dshotEnableChannels(dshotPwmDevice.count);
return true;
}

3
src/main/drivers/pwm_output_dshot_shared.h
View file

@ -45,8 +45,9 @@ extern uint32_t readDoneCount;
// TODO remove once debugging no longer needed
FAST_RAM_ZERO_INIT extern uint32_t dshotInvalidPacketCount;
FAST_RAM_ZERO_INIT extern uint32_t inputBuffer[DSHOT_TELEMETRY_INPUT_LEN];
FAST_RAM_ZERO_INIT extern uint32_t inputBuffer[GCR_TELEMETRY_INPUT_LEN];
FAST_RAM_ZERO_INIT extern uint32_t setDirectionMicros;
FAST_RAM_ZERO_INIT extern uint32_t inputStampUs;
#endif
uint8_t getTimerIndex(TIM_TypeDef *timer);