1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00

DSHOT timing improvements (#12709)

DSHOT timing improvements:

If DSHOT telemetry is still being received, wait (12612)
Optimise DSHOT cache management loops (12672)
Send motor data and then immediately decode prior telemetry data for bitbanged DSHOT (12685)
This commit is contained in:
Steve Evans 2023-04-22 00:11:36 +01:00 committed by GitHub
parent aa228ea583
commit 93f0a2380e
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
15 changed files with 183 additions and 67 deletions

View file

@ -108,4 +108,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"VTX_MSP",
"GPS_DOP",
"FAILSAFE",
"DSHOT_TELEMETRY_COUNTS"
};

View file

@ -106,6 +106,7 @@ typedef enum {
DEBUG_VTX_MSP,
DEBUG_GPS_DOP,
DEBUG_FAILSAFE,
DEBUG_DSHOT_TELEMETRY_COUNTS,
DEBUG_COUNT
} debugType_e;

View file

@ -630,8 +630,8 @@ void validateAndFixGyroConfig(void)
// check for looptime restrictions based on motor protocol. Motor times have safety margin
float motorUpdateRestriction;
#if defined(STM32F411xE)
/* If bidirectional DSHOT is being used on an F411 then force DSHOT300. The motor update restrictions then applied
#if defined(STM32F4)|| defined(STM32G4)
/* If bidirectional DSHOT is being used on an F4 or G4 then force DSHOT300. The motor update restrictions then applied
* will automatically consider the loop time and adjust pid_process_denom appropriately
*/
if (motorConfig()->dev.useDshotTelemetry && (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_DSHOT600)) {

View file

@ -46,6 +46,16 @@
#include "drivers/timer.h"
#include "pg/motor.h"
#include "pg/pinio.h"
// DEBUG_DSHOT_TELEMETRY_COUNTS
// 0 - Count of telemetry packets read
// 1 - Count of missing edge
// 2 - Count of reception not complete in time
// 3 - Number of high bits before telemetry start
// Maximum time to wait for telemetry reception to complete
#define DSHOT_TELEMETRY_TIMEOUT 2000
FAST_DATA_ZERO_INIT bbPacer_t bbPacers[MAX_MOTOR_PACERS]; // TIM1 or TIM8
FAST_DATA_ZERO_INIT int usedMotorPacers = 0;
@ -324,9 +334,12 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) {
bbPort->telemetryPending = false;
#ifdef DEBUG_COUNT_INTERRUPT
bbPort->inputIrq++;
#endif
// Disable DMA as telemetry reception is complete
bbDMA_Cmd(bbPort, DISABLE);
} else {
#ifdef DEBUG_COUNT_INTERRUPT
bbPort->outputIrq++;
@ -335,6 +348,7 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
// Switch to input
bbSwitchToInput(bbPort);
bbPort->telemetryPending = true;
bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, ENABLE);
}
@ -441,7 +455,7 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
bbDevice.vTable.write = motorWriteNull;
bbDevice.vTable.updateStart = motorUpdateStartNull;
bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
return false;
@ -493,49 +507,71 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
return true;
}
static bool bbUpdateStart(void)
static bool bbTelemetryWait(void)
{
// Wait for telemetry reception to complete
bool telemetryPending;
bool telemetryWait = false;
const timeUs_t startTimeUs = micros();
do {
telemetryPending = false;
for (int i = 0; i < usedMotorPorts; i++) {
telemetryPending |= bbPorts[i].telemetryPending;
}
telemetryWait |= telemetryPending;
if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) {
break;
}
} while (telemetryPending);
if (telemetryWait) {
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
}
return telemetryWait;
}
static void bbUpdateInit(void)
{
for (int i = 0; i < usedMotorPorts; i++) {
bbOutputDataClear(bbPorts[i].portOutputBuffer);
}
}
static bool bbDecodeTelemetry(void)
{
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
#ifdef USE_DSHOT_TELEMETRY_STATS
const timeMs_t currentTimeMs = millis();
#endif
timeUs_t currentUs = micros();
// don't send while telemetry frames might still be incoming
if (cmpTimeUs(currentUs, lastSendUs) < (timeDelta_t)(40 + 2 * dshotFrameUs)) {
return false;
}
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
#ifdef USE_DSHOT_CACHE_MGMT
// Only invalidate the buffer once. If all motors are on a common port they'll share a buffer.
bool invalidated = false;
for (int i = 0; i < motorIndex; i++) {
if (bbMotors[motorIndex].bbPort->portInputBuffer == bbMotors[i].bbPort->portInputBuffer) {
invalidated = true;
}
}
if (!invalidated) {
SCB_InvalidateDCache_by_Addr((uint32_t *)bbMotors[motorIndex].bbPort->portInputBuffer,
DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
}
for (int i = 0; i < usedMotorPorts; i++) {
bbPort_t *bbPort = &bbPorts[i];
SCB_InvalidateDCache_by_Addr((uint32_t *)bbPort->portInputBuffer, DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
}
#endif
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
#ifdef STM32F4
uint32_t rawValue = decode_bb_bitband(
bbMotors[motorIndex].bbPort->portInputBuffer,
bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
bbMotors[motorIndex].bbPort->portInputCount,
bbMotors[motorIndex].pinIndex);
#else
uint32_t rawValue = decode_bb(
bbMotors[motorIndex].bbPort->portInputBuffer,
bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
bbMotors[motorIndex].bbPort->portInputCount,
bbMotors[motorIndex].pinIndex);
#endif
if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 1, debug[1] + 1);
continue;
}
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1);
dshotTelemetryState.readCount++;
if (rawValue != DSHOT_TELEMETRY_INVALID) {
@ -556,10 +592,6 @@ static bool bbUpdateStart(void)
dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
}
#endif
for (int i = 0; i < usedMotorPorts; i++) {
bbDMA_Cmd(&bbPorts[i], DISABLE);
bbOutputDataClear(bbPorts[i].portOutputBuffer);
}
return true;
}
@ -616,23 +648,11 @@ static void bbUpdateComplete(void)
}
}
#ifdef USE_DSHOT_CACHE_MGMT
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
// Only clean each buffer once. If all motors are on a common port they'll share a buffer.
bool clean = false;
for (int i = 0; i < motorIndex; i++) {
if (bbMotors[motorIndex].bbPort->portOutputBuffer == bbMotors[i].bbPort->portOutputBuffer) {
clean = true;
}
}
if (!clean) {
SCB_CleanDCache_by_Addr(bbMotors[motorIndex].bbPort->portOutputBuffer, MOTOR_DSHOT_BUF_CACHE_ALIGN_BYTES);
}
}
#endif
for (int i = 0; i < usedMotorPorts; i++) {
bbPort_t *bbPort = &bbPorts[i];
#ifdef USE_DSHOT_CACHE_MGMT
SCB_CleanDCache_by_Addr(bbPort->portOutputBuffer, MOTOR_DSHOT_BUF_CACHE_ALIGN_BYTES);
#endif
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
@ -708,7 +728,9 @@ static motorVTable_t bbVTable = {
.enable = bbEnableMotors,
.disable = bbDisableMotors,
.isMotorEnabled = bbIsMotorEnabled,
.updateStart = bbUpdateStart,
.telemetryWait = bbTelemetryWait,
.decodeTelemetry = bbDecodeTelemetry,
.updateInit = bbUpdateInit,
.write = bbWrite,
.writeInt = bbWriteInt,
.updateComplete = bbUpdateComplete,
@ -755,7 +777,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
if (!IOIsFreeOrPreinit(io)) {
/* not enough motors initialised for the mixer or a break in the motors */
bbDevice.vTable.write = motorWriteNull;
bbDevice.vTable.updateStart = motorUpdateStartNull;
bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
return NULL;

View file

@ -1,3 +1,23 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <string.h>
#include <stdio.h>
@ -8,6 +28,7 @@
#include "common/maths.h"
#include "common/utils.h"
#include "build/debug.h"
#include "drivers/dshot.h"
#include "drivers/dshot_bitbang_decode.h"
@ -28,7 +49,8 @@ uint16_t bbBuffer[134];
#define BITBAND_SRAM_BASE 0x22000000
#define BITBAND_SRAM(a,b) ((BITBAND_SRAM_BASE + (((a)-BITBAND_SRAM_REF)<<5) + ((b)<<2))) // Convert SRAM address
#define DSHOT_TELEMETRY_START_MARGIN 10
static uint8_t preambleSkip = 0;
typedef struct bitBandWord_s {
uint32_t value;
@ -84,6 +106,8 @@ static uint32_t decode_bb_value(uint32_t value, uint16_t buffer[], uint32_t coun
uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
{
uint8_t startMargin;
#ifdef DEBUG_BBDECODE
memset(sequence, 0, sizeof(sequence));
sequenceIndex = 0;
@ -94,6 +118,9 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
bitBandWord_t* b = p;
bitBandWord_t* endP = p + (count - MIN_VALID_BBSAMPLES);
// Jump forward in the buffer to just before where we anticipate the first zero
p += preambleSkip;
// Eliminate leading high signal level by looking for first zero bit in data stream.
// Manual loop unrolling and branch hinting to produce faster code.
while (p < endP) {
@ -105,6 +132,9 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
}
}
startMargin = p - b;
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, startMargin);
if (p >= endP) {
// not returning telemetry is ok if the esc cpu is
// overburdened. in that case no edge will be found and
@ -188,6 +218,10 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
sequence[sequenceIndex] = sequence[sequenceIndex] + (nlen) * 3;
sequenceIndex++;
#endif
// The anticipated edges were observed
preambleSkip = startMargin - DSHOT_TELEMETRY_START_MARGIN;
if (nlen > 0) {
value <<= nlen;
value |= 1 << (nlen - 1);
@ -198,6 +232,8 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
{
uint8_t startMargin;
#ifdef DEBUG_BBDECODE
memset(sequence, 0, sizeof(sequence));
sequenceIndex = 0;
@ -209,12 +245,15 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
memset(sequence, 0, sizeof(sequence));
int sequenceIndex = 0;
#endif
uint16_t lastValue = 0;
uint32_t value = 0;
uint16_t* p = buffer;
uint16_t* endP = p + count - MIN_VALID_BBSAMPLES;
// Jump forward in the buffer to just before where we anticipate the first zero
p += preambleSkip;
// Eliminate leading high signal level by looking for first zero bit in data stream.
// Manual loop unrolling and branch hinting to produce faster code.
while (p < endP) {
@ -226,10 +265,15 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
}
}
if(*p & mask) {
startMargin = p - buffer;
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, startMargin);
if (p >= endP) {
// not returning telemetry is ok if the esc cpu is
// overburdened. in that case no edge will be found and
// BB_NOEDGE indicates the condition to caller
// Increase the start margin
preambleSkip--;
return DSHOT_TELEMETRY_NOEDGE;
}
@ -268,6 +312,8 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
// length of last sequence has to be inferred since the last bit with inverted dshot is high
if (bits < 18) {
// Increase the start margin
preambleSkip--;
return DSHOT_TELEMETRY_NOEDGE;
}
@ -278,9 +324,14 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
#endif
if (nlen < 0) {
// Increase the start margin
preambleSkip--;
return DSHOT_TELEMETRY_NOEDGE;
}
// The anticipated edges were observed
preambleSkip = startMargin - DSHOT_TELEMETRY_START_MARGIN;
if (nlen > 0) {
value <<= nlen;
value |= 1 << (nlen - 1);

View file

@ -167,6 +167,7 @@ typedef struct bbPort_s {
uint16_t *portInputBuffer;
uint32_t portInputCount;
bool inputActive;
volatile bool telemetryPending;
// Misc
#ifdef DEBUG_COUNT_INTERRUPT

View file

@ -218,7 +218,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
#ifdef USE_DSHOT_TELEMETRY
timeUs_t timeoutUs = micros() + 1000;
while (!motorGetVTable().updateStart() &&
while (!motorGetVTable().decodeTelemetry() &&
cmpTimeUs(timeoutUs, micros()) > 0);
#endif
for (uint8_t i = 0; i < motorDeviceCount(); i++) {

View file

@ -140,7 +140,7 @@ static motorVTable_t dshotPwmVTable = {
.enable = dshotPwmEnableMotors,
.disable = dshotPwmDisableMotors,
.isMotorEnabled = dshotPwmIsMotorEnabled,
.updateStart = motorUpdateStartNull, // May be updated after copying
.decodeTelemetry = motorDecodeTelemetryNull, // May be updated after copying
.write = dshotWrite,
.writeInt = dshotWriteInt,
.updateComplete = pwmCompleteDshotMotorUpdate,
@ -160,7 +160,7 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
#ifdef USE_DSHOT_TELEMETRY
useDshotTelemetry = motorConfig->useDshotTelemetry;
dshotPwmDevice.vTable.updateStart = pwmStartDshotMotorUpdate;
dshotPwmDevice.vTable.decodeTelemetry = pwmTelemetryDecode;
#endif
switch (motorConfig->motorPwmProtocol) {

View file

@ -159,7 +159,7 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
void pwmWriteDshotInt(uint8_t index, uint16_t value);
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
#ifdef USE_DSHOT_TELEMETRY
bool pwmStartDshotMotorUpdate(void);
bool pwmTelemetryDecode(void);
#endif
void pwmCompleteDshotMotorUpdate(void);

View file

@ -60,15 +60,49 @@ void motorWriteAll(float *values)
{
#ifdef USE_PWM_OUTPUT
if (motorDevice->enabled) {
#ifdef USE_DSHOT_BITBANG
if (isDshotBitbangActive(&motorConfig()->dev)) {
// Initialise the output buffers
if (motorDevice->vTable.updateInit) {
motorDevice->vTable.updateInit();
}
// Update the motor data
for (int i = 0; i < motorDevice->count; i++) {
motorDevice->vTable.write(i, values[i]);
}
// Don't attempt to write commands to the motors if telemetry is still being received
if (motorDevice->vTable.telemetryWait) {
(void)motorDevice->vTable.telemetryWait();
}
// Trigger the transmission of the motor data
motorDevice->vTable.updateComplete();
// Perform the decode of the last data received
// New data will be received once the send of motor data, triggered above, completes
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
if (!motorDevice->vTable.updateStart()) {
return;
}
motorDevice->vTable.decodeTelemetry();
#endif
for (int i = 0; i < motorDevice->count; i++) {
motorDevice->vTable.write(i, values[i]);
} else
#endif
{
// Perform the decode of the last data received
// New data will be received once the send of motor data, triggered above, completes
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
motorDevice->vTable.decodeTelemetry();
#endif
// Update the motor data
for (int i = 0; i < motorDevice->count; i++) {
motorDevice->vTable.write(i, values[i]);
}
// Trigger the transmission of the motor data
motorDevice->vTable.updateComplete();
}
motorDevice->vTable.updateComplete();
}
#else
UNUSED(values);
@ -193,7 +227,7 @@ static bool motorIsEnabledNull(uint8_t index)
return false;
}
bool motorUpdateStartNull(void)
bool motorDecodeTelemetryNull(void)
{
return true;
}
@ -235,7 +269,7 @@ static const motorVTable_t motorNullVTable = {
.enable = motorEnableNull,
.disable = motorDisableNull,
.isMotorEnabled = motorIsEnabledNull,
.updateStart = motorUpdateStartNull,
.decodeTelemetry = motorDecodeTelemetryNull,
.write = motorWriteNull,
.writeInt = motorWriteIntNull,
.updateComplete = motorUpdateCompleteNull,

View file

@ -50,7 +50,9 @@ typedef struct motorVTable_s {
bool (*enable)(void);
void (*disable)(void);
bool (*isMotorEnabled)(uint8_t index);
bool (*updateStart)(void);
bool (*telemetryWait)(void);
bool (*decodeTelemetry)(void);
void (*updateInit)(void);
void (*write)(uint8_t index, float value);
void (*writeInt)(uint8_t index, uint16_t value);
void (*updateComplete)(void);
@ -70,7 +72,7 @@ typedef struct motorDevice_s {
void motorPostInitNull();
void motorWriteNull(uint8_t index, float value);
bool motorUpdateStartNull(void);
bool motorDecodeTelemetryNull(void);
void motorUpdateCompleteNull(void);
void motorPostInit();

View file

@ -215,7 +215,7 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
}
motorPwmDevice.vTable.write = pwmWriteStandard;
motorPwmDevice.vTable.updateStart = motorUpdateStartNull;
motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {

View file

@ -176,7 +176,11 @@ static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
#endif
#ifdef USE_DSHOT_TELEMETRY
FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
/**
* Process dshot telemetry packets before switching the channels back to outputs
*
*/
FAST_CODE_NOINLINE bool pwmTelemetryDecode(void)
{
if (!useDshotTelemetry) {
return true;

View file

@ -59,7 +59,7 @@ void pwmDshotSetDirectionOutput(
#endif
);
bool pwmStartDshotMotorUpdate(void);
bool pwmTelemetryDecode(void);
#endif
#endif

View file

@ -520,7 +520,7 @@ static motorDevice_t motorPwmDevice = {
.enable = pwmEnableMotors,
.disable = pwmDisableMotors,
.isMotorEnabled = pwmIsMotorEnabled,
.updateStart = motorUpdateStartNull,
.decodeTelemetry = motorDecodeTelemetryNull,
.write = pwmWriteMotor,
.writeInt = pwmWriteMotorInt,
.updateComplete = pwmCompleteMotorUpdate,