mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-14 20:10:18 +03:00
rebase
Reduce code by supporting only GCR, fix serial_4way, fix f7 dshot bidir fix ws and eliminate superfluous buffer use GCR constant instead of 32 decode optimization bump 4way prot version mumber bump if version
This commit is contained in:
Thorsten Laux
parent
383ba1cd8e
commit
835a5cac0e
8 changed files with 192 additions and 175 deletions
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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,8 +139,10 @@ FAST_CODE void pwmDshotSetDirectionOutput(
|
|||
}
|
||||
|
||||
xDMA_Init(dmaRef, pDmaInit);
|
||||
if (output) {
|
||||
xDMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void pwmCompleteDshotMotorUpdate(void)
|
||||
|
|
@ -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,11 +180,7 @@ 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);
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
if (useBurstDshot) {
|
||||
xDMA_Cmd(motor->timerHardware->dmaTimUPRef, DISABLE);
|
||||
|
|
@ -197,13 +195,12 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
|
|||
#ifdef USE_DSHOT_TELEMETRY
|
||||
if (useDshotTelemetry) {
|
||||
pwmDshotSetDirectionOutput(motor, false);
|
||||
xDMA_SetCurrDataCounter(motor->dmaRef, motor->dmaInputLen);
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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,8 +110,10 @@ void pwmDshotSetDirectionOutput(
|
|||
motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
|
||||
}
|
||||
xLL_EX_DMA_Init(motor->dmaRef, pDmaInit);
|
||||
if (output) {
|
||||
xLL_EX_DMA_EnableIT_TC(motor->dmaRef);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
FAST_CODE void pwmCompleteDshotMotorUpdate(void)
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
} else {
|
||||
if (diff >= -9) 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 {
|
||||
len = 21 - bits;
|
||||
}
|
||||
|
||||
uint32_t csum = value;
|
||||
value <<= len;
|
||||
value |= 1 << (len - 1);
|
||||
oldValue = buffer[i];
|
||||
bits += len;
|
||||
}
|
||||
if (bits != 21) {
|
||||
return 0xffff;
|
||||
}
|
||||
|
||||
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;
|
||||
if (decodedValue == 0x0fff) {
|
||||
setDirectionMicros = micros() - start;
|
||||
return 0;
|
||||
}
|
||||
value <<= 4;
|
||||
value |= (nibble & 0xf);
|
||||
}
|
||||
|
||||
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)
|
||||
NOINLINE bool pwmStartDshotMotorUpdate(void)
|
||||
{
|
||||
if (!useDshotTelemetry) {
|
||||
return true;
|
||||
|
|
@ -249,21 +259,31 @@ 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
|
||||
|
||||
#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 == 0) {
|
||||
if (dmaMotors[i].useProshot) {
|
||||
value = decodeProshotPacket(dmaMotors[i].dmaBuffer);
|
||||
} else {
|
||||
value = decodeDshotPacket(dmaMotors[i].dmaBuffer);
|
||||
}
|
||||
}
|
||||
|
||||
if (edges > MIN_GCR_EDGES) {
|
||||
readDoneCount++;
|
||||
value = decodeTelemetryPacket(dmaMotors[i].dmaBuffer, edges);
|
||||
|
||||
#ifdef USE_DSHOT_TELEMETRY_STATS
|
||||
bool validTelemetryPacket = false;
|
||||
#endif
|
||||
|
|
@ -282,23 +302,14 @@ bool pwmStartDshotMotorUpdate(void)
|
|||
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
|
||||
}
|
||||
pwmDshotSetDirectionOutput(&dmaMotors[i], true);
|
||||
}
|
||||
inputStampUs = 0;
|
||||
dshotEnableChannels(dshotPwmDevice.count);
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -31,10 +31,13 @@
|
|||
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
#include "drivers/buf_writer.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/dshot.h"
|
||||
#include "drivers/dshot_dpwm.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/light_led.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
|
|
@ -76,9 +79,9 @@
|
|||
// *** change to adapt Revision
|
||||
#define SERIAL_4WAY_VER_MAIN 20
|
||||
#define SERIAL_4WAY_VER_SUB_1 (uint8_t) 0
|
||||
#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 03
|
||||
#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 04
|
||||
|
||||
#define SERIAL_4WAY_PROTOCOL_VER 107
|
||||
#define SERIAL_4WAY_PROTOCOL_VER 108
|
||||
// *** end
|
||||
|
||||
#if (SERIAL_4WAY_VER_MAIN > 24)
|
||||
|
|
@ -139,7 +142,6 @@ uint8_t esc4wayInit(void)
|
|||
// StopPwmAllMotors();
|
||||
// XXX Review effect of motor refactor
|
||||
//pwmDisableMotors();
|
||||
motorDisable();
|
||||
escCount = 0;
|
||||
memset(&escHardware, 0, sizeof(escHardware));
|
||||
pwmOutputPort_t *pwmMotors = pwmGetMotors();
|
||||
|
|
@ -153,6 +155,7 @@ uint8_t esc4wayInit(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
motorDisable();
|
||||
return escCount;
|
||||
}
|
||||
|
||||
|
|
@ -566,9 +569,13 @@ void esc4wayProcess(serialPort_t *mspPort)
|
|||
|
||||
case cmd_DeviceReset:
|
||||
{
|
||||
bool rebootEsc = false;
|
||||
if (ParamBuf[0] < escCount) {
|
||||
// Channel may change here
|
||||
selected_esc = ParamBuf[0];
|
||||
if (ioMem.D_FLASH_ADDR_L == 1) {
|
||||
rebootEsc = true;
|
||||
}
|
||||
}
|
||||
else {
|
||||
ACK_OUT = ACK_I_INVALID_CHANNEL;
|
||||
|
|
@ -582,6 +589,14 @@ void esc4wayProcess(serialPort_t *mspPort)
|
|||
case imARM_BLB:
|
||||
{
|
||||
BL_SendCMDRunRestartBootloader(&DeviceInfo);
|
||||
if (rebootEsc) {
|
||||
ESC_OUTPUT;
|
||||
setEscLo(selected_esc);
|
||||
timeMs_t m = millis();
|
||||
while (millis() - m < 300);
|
||||
setEscHi(selected_esc);
|
||||
ESC_INPUT;
|
||||
}
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue