Skip to content

Commit

Permalink
Merge pull request #7264 from joelucid/dshot_telemetry
Browse files Browse the repository at this point in the history 
Dshot telemetry
  • Loading branch information
@mikeller
mikeller committed Jan 7, 2019
2 parents dc7d669 + 7b241fd commit 00e0248
Show file tree
Hide file tree
Showing 14 changed files with 501 additions and 82 deletions.
1 change: 1 addition & 0 deletions src/main/build/debug.c
View file
Expand Up @@ -78,4 +78,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"ANTI_GRAVITY",
"DYN_LPF",
"RX_SPEKTRUM_SPI",
"DSHOT_TELEMETRY",
};
1 change: 1 addition & 0 deletions src/main/build/debug.h
View file
Expand Up @@ -96,6 +96,7 @@ typedef enum {
DEBUG_ANTI_GRAVITY,
DEBUG_DYN_LPF,
DEBUG_RX_SPEKTRUM_SPI,
DEBUG_RPM_TELEMETRY,
DEBUG_COUNT
} debugType_e;

Expand Down
40 changes: 40 additions & 0 deletions src/main/drivers/pwm_output.c
View file
Expand Up @@ -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;
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
}
Expand Down Expand Up @@ -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);
Expand All @@ -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));
Expand Down Expand Up @@ -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:
Expand All @@ -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) {
Expand All @@ -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++) {
Expand Down Expand Up @@ -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:
Expand All @@ -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);
Expand Down
39 changes: 37 additions & 2 deletions src/main/drivers/pwm_output.h
View file
Expand Up @@ -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
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -133,21 +136,43 @@ 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);

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

Expand Down Expand Up @@ -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

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

Expand Down

0 comments on commit 00e0248

Please sign in to comment.