Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 11:59:58 +03:00
Code Issues Wiki Activity

Merge pull request #7264 from joelucid/dshot_telemetry

Browse source 
Dshot telemetry
This commit is contained in:
Michael Keller 2019-01-08 01:04:22 +13:00 • committed by GitHub
commit 00e0248988
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
14 changed files with 498 additions and 79 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.c
View file

@ -78,4 +78,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"ANTI_GRAVITY",
"DYN_LPF",
"RX_SPEKTRUM_SPI",
"DSHOT_TELEMETRY",
};

1
src/main/build/debug.h
View file

@ -96,6 +96,7 @@ typedef enum {
DEBUG_ANTI_GRAVITY,
DEBUG_DYN_LPF,
DEBUG_RX_SPEKTRUM_SPI,
DEBUG_RPM_TELEMETRY,
DEBUG_COUNT
} debugType_e;

40
src/main/drivers/pwm_output.c
View file

@ -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);

39
src/main/drivers/pwm_output.h
View file

@ -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);

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

@ -41,10 +41,25 @@
#include "dma.h"
#include "rcc.h"
// TODO remove once debugging no longer needed
#ifdef USE_DSHOT_TELEMETRY
#include <string.h>
#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;
}

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

@ -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;

6
src/main/fc/config.c
View file

@ -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

8
src/main/fc/core.c
View file

@ -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)) {

28
src/main/fc/init.c
View file

@ -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;
}

3
src/main/flight/mixer.c
View file

@ -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]);
}

30
src/main/interface/cli.c
View file

@ -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; i<getMotorCount(); i++) {
cliPrintLinef( "Dshot RPM Motor %u: %u", i, (int)getDshotTelemetry(i));
}
bool proshot = (motorConfig()->dev.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<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);
}
cliPrintLinefeed();
}
#endif
}
#if defined(USE_TASK_STATISTICS)

3
src/main/interface/settings.c
View file

@ -685,6 +685,9 @@ const clivalue_t valueTable[] = {
{ "dshot_idle_value", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 2000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, digitalIdleOffsetValue) },
#ifdef USE_DSHOT_DMAR
{ "dshot_burst", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useBurstDshot) },
#ifdef USE_DSHOT_TELEMETRY
{ "dshot_bidir", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useDshotTelemetry) },
#endif
#endif
#endif
{ "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useUnsyncedPwm) },

5
src/main/target/NOX/target.c
View file

@ -31,7 +31,12 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), // T2C3
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0), // T2C2(1,6,3), T5C2(1,4,6)
// Use T3C2 instead of T1C1N to enable DSHOT telemetry
#ifdef USE_DSHOT_TELEMETRY
DEF_TIM(TIM3, CH2, PA7, TIM_USE_MOTOR, 0, 0), // T1C1N(2,3,6), T3C2(1,5,5)
#else
DEF_TIM(TIM1, CH1N, PA7, TIM_USE_MOTOR, 0, 0), // T1C1N(2,3,6), T3C2(1,5,5)
#endif
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), // T4C3(1,7,2), T10C1(X)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // T1C3N(2,6,6), T3C4(1,7,5)

2
src/main/target/common_pre.h
View file

@ -53,6 +53,7 @@
#define USE_FAST_RAM
#endif
#define USE_DSHOT
#define USE_DSHOT_TELEMETRY
#define I2C3_OVERCLOCK true
#define USE_GYRO_DATA_ANALYSE
#define USE_ADC
@ -243,3 +244,4 @@
#define USE_ESCSERIAL_SIMONK
#define USE_SERIAL_4WAY_SK_BOOTLOADER
#endif