mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-18 22:05:17 +03:00
avoid dshot telemetry collisions
actually use calculated deadtime fix whitespace and return value fix ws Address review feedback fix ws
This commit is contained in:
parent
8984a7195f
commit
5a759c56ef
7 changed files with 82 additions and 51 deletions
|
@ -224,9 +224,10 @@ bool pwmAreMotorsEnabled(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_DSHOT_TELEMETRY
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
static void pwmStartWriteUnused(uint8_t motorCount)
|
static bool pwmStartWriteUnused(uint8_t motorCount)
|
||||||
{
|
{
|
||||||
UNUSED(motorCount);
|
UNUSED(motorCount);
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -253,9 +254,9 @@ void pwmCompleteMotorUpdate(uint8_t motorCount)
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_DSHOT_TELEMETRY
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
void pwmStartMotorUpdate(uint8_t motorCount)
|
bool pwmStartMotorUpdate(uint8_t motorCount)
|
||||||
{
|
{
|
||||||
pwmStartWrite(motorCount);
|
return pwmStartWrite(motorCount);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -521,7 +522,9 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
|
||||||
delayMicroseconds(DSHOT_COMMAND_DELAY_US);
|
delayMicroseconds(DSHOT_COMMAND_DELAY_US);
|
||||||
|
|
||||||
#ifdef USE_DSHOT_TELEMETRY
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
pwmStartDshotMotorUpdate(motorCount);
|
timeUs_t currentTimeUs = micros();
|
||||||
|
while (!pwmStartDshotMotorUpdate(motorCount) &&
|
||||||
|
cmpTimeUs(micros(), currentTimeUs) < 1000);
|
||||||
#endif
|
#endif
|
||||||
for (uint8_t i = 0; i < motorCount; i++) {
|
for (uint8_t i = 0; i < motorCount; i++) {
|
||||||
if ((i == index) || (index == ALL_MOTORS)) {
|
if ((i == index) || (index == ALL_MOTORS)) {
|
||||||
|
|
|
@ -22,6 +22,8 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "common/time.h"
|
||||||
|
|
||||||
#include "drivers/io_types.h"
|
#include "drivers/io_types.h"
|
||||||
#include "drivers/pwm_output_counts.h"
|
#include "drivers/pwm_output_counts.h"
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
|
@ -112,6 +114,8 @@ typedef enum {
|
||||||
#define PROSHOT_BASE_SYMBOL 24 // 1uS
|
#define PROSHOT_BASE_SYMBOL 24 // 1uS
|
||||||
#define PROSHOT_BIT_WIDTH 3
|
#define PROSHOT_BIT_WIDTH 3
|
||||||
#define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS
|
#define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS
|
||||||
|
|
||||||
|
#define DSHOT_TELEMETRY_DEADTIME_US (2 * 30 + 10) // 2 * 30uS to switch lines plus 10us grace period
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
@ -128,6 +132,7 @@ typedef struct {
|
||||||
#endif
|
#endif
|
||||||
uint16_t dmaBurstLength;
|
uint16_t dmaBurstLength;
|
||||||
uint32_t dmaBurstBuffer[DSHOT_DMA_BUFFER_SIZE * 4];
|
uint32_t dmaBurstBuffer[DSHOT_DMA_BUFFER_SIZE * 4];
|
||||||
|
timeUs_t inputDirectionStampUs;
|
||||||
#endif
|
#endif
|
||||||
uint16_t timerDmaSources;
|
uint16_t timerDmaSources;
|
||||||
} motorDmaTimer_t;
|
} motorDmaTimer_t;
|
||||||
|
@ -146,6 +151,7 @@ typedef struct {
|
||||||
volatile bool isInput;
|
volatile bool isInput;
|
||||||
volatile bool hasTelemetry;
|
volatile bool hasTelemetry;
|
||||||
uint16_t dshotTelemetryValue;
|
uint16_t dshotTelemetryValue;
|
||||||
|
timeDelta_t dshotTelemetryDeadtimeUs;
|
||||||
bool dshotTelemetryActive;
|
bool dshotTelemetryActive;
|
||||||
#ifdef USE_HAL_DRIVER
|
#ifdef USE_HAL_DRIVER
|
||||||
LL_TIM_OC_InitTypeDef ocInitStruct;
|
LL_TIM_OC_InitTypeDef ocInitStruct;
|
||||||
|
@ -183,7 +189,7 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
|
||||||
struct timerHardware_s;
|
struct timerHardware_s;
|
||||||
typedef void pwmWriteFn(uint8_t index, float value); // function pointer used to write motors
|
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 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 bool pwmStartWriteFn(uint8_t motorCount); // function pointer used before motors are written
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
volatile timCCR_t *ccr;
|
volatile timCCR_t *ccr;
|
||||||
|
@ -244,7 +250,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
|
||||||
void pwmWriteDshotInt(uint8_t index, uint16_t value);
|
void pwmWriteDshotInt(uint8_t index, uint16_t value);
|
||||||
void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
|
void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
|
||||||
#ifdef USE_DSHOT_TELEMETRY
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
void pwmStartDshotMotorUpdate(uint8_t motorCount);
|
bool pwmStartDshotMotorUpdate(uint8_t motorCount);
|
||||||
#endif
|
#endif
|
||||||
void pwmCompleteDshotMotorUpdate(uint8_t motorCount);
|
void pwmCompleteDshotMotorUpdate(uint8_t motorCount);
|
||||||
|
|
||||||
|
@ -268,7 +274,7 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware,
|
||||||
void pwmWriteMotor(uint8_t index, float value);
|
void pwmWriteMotor(uint8_t index, float value);
|
||||||
void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
|
void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
|
||||||
void pwmCompleteMotorUpdate(uint8_t motorCount);
|
void pwmCompleteMotorUpdate(uint8_t motorCount);
|
||||||
void pwmStartMotorUpdate(uint8_t motorCount);
|
bool pwmStartMotorUpdate(uint8_t motorCount);
|
||||||
|
|
||||||
void pwmWriteServo(uint8_t index, float value);
|
void pwmWriteServo(uint8_t index, float value);
|
||||||
|
|
||||||
|
|
|
@ -96,8 +96,9 @@ FAST_CODE void pwmDshotSetDirectionOutput(
|
||||||
DMA_DeInit(dmaRef);
|
DMA_DeInit(dmaRef);
|
||||||
|
|
||||||
#ifdef USE_DSHOT_TELEMETRY
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
motor->isInput = !output;
|
|
||||||
if (!output) {
|
if (!output) {
|
||||||
|
motor->isInput = true;
|
||||||
|
motor->timer->inputDirectionStampUs = micros();
|
||||||
TIM_ICInit(timer, &motor->icInitStruct);
|
TIM_ICInit(timer, &motor->icInitStruct);
|
||||||
|
|
||||||
#if defined(STM32F3)
|
#if defined(STM32F3)
|
||||||
|
@ -111,6 +112,9 @@ FAST_CODE void pwmDshotSetDirectionOutput(
|
||||||
UNUSED(output);
|
UNUSED(output);
|
||||||
#endif
|
#endif
|
||||||
{
|
{
|
||||||
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
|
motor->isInput = false;
|
||||||
|
#endif
|
||||||
timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Disable);
|
timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Disable);
|
||||||
timerOCInit(timer, timerHardware->channel, pOcInit);
|
timerOCInit(timer, timerHardware->channel, pOcInit);
|
||||||
timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable);
|
timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable);
|
||||||
|
@ -388,6 +392,9 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
|
||||||
|
|
||||||
#ifdef USE_DSHOT_TELEMETRY
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
motor->dmaInputLen = motor->useProshot ? PROSHOT_TELEMETRY_INPUT_LEN : DSHOT_TELEMETRY_INPUT_LEN;
|
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);
|
||||||
pwmDshotSetDirectionOutput(motor, true);
|
pwmDshotSetDirectionOutput(motor, true);
|
||||||
#else
|
#else
|
||||||
pwmDshotSetDirectionOutput(motor, true, &OCINIT, &DMAINIT);
|
pwmDshotSetDirectionOutput(motor, true, &OCINIT, &DMAINIT);
|
||||||
|
|
|
@ -98,8 +98,9 @@ void pwmDshotSetDirectionOutput(
|
||||||
LL_EX_DMA_DeInit(motor->dmaRef);
|
LL_EX_DMA_DeInit(motor->dmaRef);
|
||||||
|
|
||||||
#ifdef USE_DSHOT_TELEMETRY
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
motor->isInput = !output;
|
|
||||||
if (!output) {
|
if (!output) {
|
||||||
|
motor->isInput = true;
|
||||||
|
motor->timer->inputDirectionStampUs = micros();
|
||||||
LL_TIM_IC_Init(timer, motor->llChannel, &motor->icInitStruct);
|
LL_TIM_IC_Init(timer, motor->llChannel, &motor->icInitStruct);
|
||||||
motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
|
motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
|
||||||
} else
|
} else
|
||||||
|
@ -107,6 +108,9 @@ void pwmDshotSetDirectionOutput(
|
||||||
UNUSED(output);
|
UNUSED(output);
|
||||||
#endif
|
#endif
|
||||||
{
|
{
|
||||||
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
|
motor->isInput = false;
|
||||||
|
#endif
|
||||||
LL_TIM_OC_DisablePreload(timer, motor->llChannel);
|
LL_TIM_OC_DisablePreload(timer, motor->llChannel);
|
||||||
LL_TIM_OC_Init(timer, motor->llChannel, pOcInit);
|
LL_TIM_OC_Init(timer, motor->llChannel, pOcInit);
|
||||||
LL_TIM_OC_EnablePreload(timer, motor->llChannel);
|
LL_TIM_OC_EnablePreload(timer, motor->llChannel);
|
||||||
|
@ -351,6 +355,9 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
|
||||||
|
|
||||||
#ifdef USE_DSHOT_TELEMETRY
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
motor->dmaInputLen = motor->useProshot ? PROSHOT_TELEMETRY_INPUT_LEN : DSHOT_TELEMETRY_INPUT_LEN;
|
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);
|
||||||
pwmDshotSetDirectionOutput(motor, true);
|
pwmDshotSetDirectionOutput(motor, true);
|
||||||
#else
|
#else
|
||||||
pwmDshotSetDirectionOutput(motor, true, &OCINIT, &DMAINIT);
|
pwmDshotSetDirectionOutput(motor, true, &OCINIT, &DMAINIT);
|
||||||
|
|
|
@ -203,48 +203,54 @@ FAST_CODE void pwmDshotSetDirectionOutput(
|
||||||
|
|
||||||
#ifdef USE_DSHOT_TELEMETRY
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
|
|
||||||
void pwmStartDshotMotorUpdate(uint8_t motorCount)
|
bool pwmStartDshotMotorUpdate(uint8_t motorCount)
|
||||||
{
|
{
|
||||||
if (useDshotTelemetry) {
|
if (!useDshotTelemetry) {
|
||||||
for (int i = 0; i < motorCount; i++) {
|
return true;
|
||||||
if (dmaMotors[i].hasTelemetry) {
|
|
||||||
#ifdef STM32F7
|
|
||||||
uint32_t edges = LL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef);
|
|
||||||
#else
|
|
||||||
uint32_t edges = DMA_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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (value != 0xffff) {
|
|
||||||
dmaMotors[i].dshotTelemetryValue = value;
|
|
||||||
dmaMotors[i].dshotTelemetryActive = true;
|
|
||||||
if (i < 4) {
|
|
||||||
DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
dshotInvalidPacketCount++;
|
|
||||||
if (i == 0) {
|
|
||||||
memcpy(inputBuffer,dmaMotors[i].dmaBuffer,sizeof(inputBuffer));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
dmaMotors[i].hasTelemetry = false;
|
|
||||||
} else {
|
|
||||||
#ifdef STM32F7
|
|
||||||
LL_EX_TIM_DisableIT(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource);
|
|
||||||
#else
|
|
||||||
TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
pwmDshotSetDirectionOutput(&dmaMotors[i], true);
|
|
||||||
}
|
|
||||||
dshotEnableChannels(motorCount);
|
|
||||||
}
|
}
|
||||||
|
for (int i = 0; i < motorCount; i++) {
|
||||||
|
if (dmaMotors[i].hasTelemetry) {
|
||||||
|
#ifdef STM32F7
|
||||||
|
uint32_t edges = LL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef);
|
||||||
|
#else
|
||||||
|
uint32_t edges = DMA_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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (value != 0xffff) {
|
||||||
|
dmaMotors[i].dshotTelemetryValue = value;
|
||||||
|
dmaMotors[i].dshotTelemetryActive = true;
|
||||||
|
if (i < 4) {
|
||||||
|
DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
dshotInvalidPacketCount++;
|
||||||
|
if (i == 0) {
|
||||||
|
memcpy(inputBuffer,dmaMotors[i].dmaBuffer,sizeof(inputBuffer));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
dmaMotors[i].hasTelemetry = false;
|
||||||
|
} else {
|
||||||
|
timeDelta_t usSinceInput = cmpTimeUs(micros(), dmaMotors[i].timer->inputDirectionStampUs);
|
||||||
|
if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
#ifdef STM32F7
|
||||||
|
LL_EX_TIM_DisableIT(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource);
|
||||||
|
#else
|
||||||
|
TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
pwmDshotSetDirectionOutput(&dmaMotors[i], true);
|
||||||
|
}
|
||||||
|
dshotEnableChannels(motorCount);
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isDshotMotorTelemetryActive(uint8_t motorIndex)
|
bool isDshotMotorTelemetryActive(uint8_t motorIndex)
|
||||||
|
|
|
@ -66,7 +66,7 @@ FAST_CODE void pwmDshotSetDirectionOutput(
|
||||||
#endif
|
#endif
|
||||||
);
|
);
|
||||||
|
|
||||||
void pwmStartDshotMotorUpdate(uint8_t motorCount);
|
bool pwmStartDshotMotorUpdate(uint8_t motorCount);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -518,7 +518,9 @@ void writeMotors(void)
|
||||||
{
|
{
|
||||||
if (pwmAreMotorsEnabled()) {
|
if (pwmAreMotorsEnabled()) {
|
||||||
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
|
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
|
||||||
pwmStartMotorUpdate(motorCount);
|
if (!pwmStartMotorUpdate(motorCount)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
for (int i = 0; i < motorCount; i++) {
|
for (int i = 0; i < motorCount; i++) {
|
||||||
pwmWriteMotor(i, motor[i]);
|
pwmWriteMotor(i, motor[i]);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue