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

DSHOT - Use cycle counting instead of recording timestamp in dshot motor_DMA_IRQHandler.

Browse source 
It turns out that two calls to micros() and the calculation of
directionChangeDurationUs took 581 cycles, vs 396 cycles without the
calls to micros() and deferred calculation of the duration which is only
needed in the CLI.

This brings the time down from around 7 microseconds to 5.5 microseconds
on an F3 at 72Mhz.

This makes the difference between 100% invalid telemetry and 4% invalid
telemetry on the first motor on the F3.

Squashed commits:
* Remove the forward declaration for `pwmDshotSetDirectionInput` and make
it static.
* Remove unneeded forward declaration of `motor_DMA_IRQHandler`.
* Remove duplication in DMA IRQ Handler.
  Doesn't affect resulting code but improves readability.
* Use an inline function to read DWT->CYCCNT.
* Remove unneeded forward declarations from cli.c now that the correct
header is included.
* Update DWT unlock method.
This commit is contained in:
Dominic Clifton 2019-08-28 11:21:06 +02:00
parent 6f4c2bb48c
commit d016aa2fc4
8 changed files with 76 additions and 33 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -66,6 +66,7 @@ bool cliMode = false;
#include "drivers/dshot.h"
#include "drivers/dshot_command.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/pwm_output_dshot_shared.h"
#include "drivers/camera_control.h"
#include "drivers/compass/compass.h"
#include "drivers/display.h"
@ -274,12 +275,6 @@ 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[GCR_TELEMETRY_INPUT_LEN];
extern uint32_t setDirectionMicros;
#endif
typedef enum dumpFlags_e {
DUMP_MASTER = (1 << 0),
DUMP_PROFILE = (1 << 1),
@ -5840,8 +5835,10 @@ static void cliDshotTelemetryInfo(char *cmdline)
if (useDshotTelemetry) {
cliPrintLinef("Dshot reads: %u", readDoneCount);
cliPrintLinef("Dshot invalid pkts: %u", dshotInvalidPacketCount);
extern uint32_t setDirectionMicros;
cliPrintLinef("Dshot irq micros: %u", setDirectionMicros);
uint32_t directionChangeCycles = dshotDMAHandlerCycleCounters.changeDirectionCompletedAt - dshotDMAHandlerCycleCounters.irqAt;
uint32_t directionChangeDurationUs = clockCyclesToMicros(directionChangeCycles);
cliPrintLinef("Dshot directionChange cycles: %u, micros: %u", directionChangeCycles, directionChangeDurationUs);
cliPrintLinef("Dshot packet decode micros: %u", decodePacketDurationUs);
cliPrintLinefeed();
#ifdef USE_DSHOT_TELEMETRY_STATS

10
src/main/drivers/dma.h
View file

@ -108,8 +108,9 @@ typedef enum {
#define DEFINE_DMA_IRQ_HANDLER(d, s, i) void DMA ## d ## _Stream ## s ## _IRQHandler(void) {\
const uint8_t index = DMA_IDENTIFIER_TO_INDEX(i); \
if (dmaDescriptors[index].irqHandlerCallback)\
dmaDescriptors[index].irqHandlerCallback(&dmaDescriptors[index]);\
dmaCallbackHandlerFuncPtr handler = dmaDescriptors[index].irqHandlerCallback; \
if (handler) \
handler(&dmaDescriptors[index]); \
}
#define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) d->dma->HIFCR = (flag << (d->flagsShift - 32)); else d->dma->LIFCR = (flag << d->flagsShift)
@ -169,8 +170,9 @@ typedef enum {
#define DEFINE_DMA_IRQ_HANDLER(d, c, i) void DMA ## d ## _Channel ## c ## _IRQHandler(void) {\
const uint8_t index = DMA_IDENTIFIER_TO_INDEX(i); \
if (dmaDescriptors[index].irqHandlerCallback)\
dmaDescriptors[index].irqHandlerCallback(&dmaDescriptors[index]);\
dmaCallbackHandlerFuncPtr handler = dmaDescriptors[index].irqHandlerCallback; \
if (handler) \
handler(&dmaDescriptors[index]); \
}
#define DMA_CLEAR_FLAG(d, flag) d->dma->IFCR = (flag << d->flagsShift)

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

@ -35,6 +35,7 @@
#include "drivers/rcc.h"
#include "drivers/time.h"
#include "drivers/timer.h"
#include "drivers/system.h"
#if defined(STM32F4)
#include "stm32f4xx.h"
#elif defined(STM32F3)
@ -63,8 +64,6 @@ void dshotEnableChannels(uint8_t motorCount)
#endif
static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor);
FAST_CODE void pwmDshotSetDirectionOutput(
motorDmaOutput_t * const motor
#ifndef USE_DSHOT_TELEMETRY
@ -121,7 +120,7 @@ FAST_CODE void pwmDshotSetDirectionOutput(
#ifdef USE_DSHOT_TELEMETRY
FAST_CODE void pwmDshotSetDirectionInput(
FAST_CODE static void pwmDshotSetDirectionInput(
motorDmaOutput_t * const motor
)
{
@ -189,7 +188,7 @@ 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_TELEMETRY
uint32_t irqStart = micros();
dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
#endif
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
@ -208,7 +207,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
xDMA_SetCurrDataCounter(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
xDMA_Cmd(motor->dmaRef, ENABLE);
TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, ENABLE);
setDirectionMicros = micros() - irqStart;
dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter();
}
#endif
DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);

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

@ -41,6 +41,7 @@
#include "drivers/rcc.h"
#include "drivers/time.h"
#include "drivers/timer.h"
#include "drivers/system.h"
#include "pwm_output.h"
@ -66,8 +67,6 @@ void dshotEnableChannels(uint8_t motorCount)
#endif
static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor);
void pwmDshotSetDirectionOutput(
motorDmaOutput_t * const motor
#ifndef USE_DSHOT_TELEMETRY
@ -99,7 +98,7 @@ void pwmDshotSetDirectionOutput(
}
#ifdef USE_DSHOT_TELEMETRY
void pwmDshotSetDirectionInput(
static void pwmDshotSetDirectionInput(
motorDmaOutput_t * const motor
)
{
@ -161,7 +160,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
if (!motor->isInput) {
#ifdef USE_DSHOT_TELEMETRY
uint32_t irqStartUs = micros();
dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
#endif
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
@ -180,7 +179,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
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() - irqStartUs;
dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter();
}
#endif
}

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

@ -80,8 +80,10 @@ uint32_t readDoneCount;
// TODO remove once debugging no longer needed
FAST_RAM_ZERO_INIT uint32_t dshotInvalidPacketCount;
FAST_RAM_ZERO_INIT uint32_t inputBuffer[GCR_TELEMETRY_INPUT_LEN];
FAST_RAM_ZERO_INIT uint32_t setDirectionMicros;
FAST_RAM_ZERO_INIT uint32_t decodePacketDurationUs;
FAST_RAM_ZERO_INIT uint32_t inputStampUs;
FAST_RAM_ZERO_INIT dshotDMAHandlerCycleCounters_t dshotDMAHandlerCycleCounters;
#endif
motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
@ -194,13 +196,13 @@ static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
csum = csum ^ (csum >> 4); // xor nibbles
if ((csum & 0xf) != 0xf) {
setDirectionMicros = micros() - start;
decodePacketDurationUs = micros() - start;
return 0xffff;
}
decodedValue >>= 4;
if (decodedValue == 0x0fff) {
setDirectionMicros = micros() - start;
decodePacketDurationUs = micros() - start;
return 0;
}
decodedValue = (decodedValue & 0x000001ff) << ((decodedValue & 0xfffffe00) >> 9);
@ -208,7 +210,7 @@ static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
return 0xffff;
}
uint32_t ret = (1000000 * 60 / 100 + decodedValue / 2) / decodedValue;
setDirectionMicros = micros() - start;
decodePacketDurationUs = micros() - start;
return ret;
}

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

@ -46,8 +46,16 @@ 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[GCR_TELEMETRY_INPUT_LEN];
FAST_RAM_ZERO_INIT extern uint32_t setDirectionMicros;
FAST_RAM_ZERO_INIT extern uint32_t decodePacketDurationUs;
FAST_RAM_ZERO_INIT extern uint32_t inputStampUs;
typedef struct dshotDMAHandlerCycleCounters_s {
uint32_t irqAt;
uint32_t changeDirectionCompletedAt;
} dshotDMAHandlerCycleCounters_t;
FAST_RAM_ZERO_INIT extern dshotDMAHandlerCycleCounters_t dshotDMAHandlerCycleCounters;
#endif
uint8_t getTimerIndex(TIM_TypeDef *timer);
@ -67,10 +75,6 @@ FAST_CODE void pwmDshotSetDirectionOutput(
#endif
);
#ifdef USE_DSHOT_TELEMETRY
FAST_CODE void pwmDshotSetDirectionInput(motorDmaOutput_t * const motor);
#endif
bool pwmStartDshotMotorUpdate(void);
#endif

41
src/main/drivers/system.c
View file

@ -32,6 +32,15 @@
#include "system.h"
#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
// See "RM CoreSight Architecture Specification"
// B2.3.10 "LSR and LAR, Software Lock Status Register and Software Lock Access Register"
// "E1.2.11 LAR, Lock Access Register"
#define DWT_LAR_UNLOCK_VALUE 0xC5ACCE55
#endif
// cycles per microsecond
static uint32_t usTicks = 0;
// current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
@ -39,16 +48,34 @@ static volatile uint32_t sysTickUptime = 0;
static volatile uint32_t sysTickValStamp = 0;
// cached value of RCC->CSR
uint32_t cachedRccCsrValue;
static uint32_t cpuClockFrequency = 0;
void cycleCounterInit(void)
{
#if defined(USE_HAL_DRIVER)
usTicks = HAL_RCC_GetSysClockFreq() / 1000000;
cpuClockFrequency = HAL_RCC_GetSysClockFreq();
#else
RCC_ClocksTypeDef clocks;
RCC_GetClocksFreq(&clocks);
usTicks = clocks.SYSCLK_Frequency / 1000000;
cpuClockFrequency = clocks.SYSCLK_Frequency;
#endif
usTicks = cpuClockFrequency / 1000000;
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
#if defined(DWT_LAR_UNLOCK_VALUE)
#if defined(STM32F7) || defined(STM32H7)
DWT->LAR = DWT_LAR_UNLOCK_VALUE;
#elif defined(STM32F3) || defined(STM32F4)
// Note: DWT_Type does not contain LAR member.
#define DWT_LAR
__O uint32_t *DWTLAR = (uint32_t *)(DWT_BASE + 0x0FB0);
*(DWTLAR) = DWT_LAR_UNLOCK_VALUE;
#endif
#endif
DWT->CYCCNT = 0;
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}
// SysTick
@ -116,6 +143,16 @@ uint32_t micros(void)
return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks;
}
inline uint32_t getCycleCounter(void)
{
return DWT->CYCCNT;
}
uint32_t clockCyclesToMicros(uint32_t clockCycles)
{
return clockCycles / usTicks;
}
// Return system uptime in milliseconds (rollover in 49 days)
uint32_t millis(void)
{

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

@ -64,10 +64,13 @@ void systemReset(void);
void systemResetToBootloader(bootloaderRequestType_e requestType);
bool isMPUSoftReset(void);
void cycleCounterInit(void);
uint32_t clockCyclesToMicros(uint32_t clockCycles);
uint32_t getCycleCounter(void);
#if defined(STM32H7)
void systemCheckResetReason(void);
#endif
void initialiseMemorySections(void);
void enableGPIOPowerUsageAndNoiseReductions(void);