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

Send motor data and then immediately decode prior telemetry data for bitbanged DSHOT (#12685)

Browse source 
* Send motor data and then immediately decode prior telemetry data for bitbanged DSHOT
F4 can't handle 8K PID loop

* Fix missing cfg_pms.version

* Limit G4 to 4K PID rate as per F4
This commit is contained in:
Steve Evans 2023-04-19 12:05:46 +01:00 • committed by GitHub
parent fea097a893
commit e8126dd6dd
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
16 changed files with 143 additions and 95 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/config/config.c
View file

@ -670,8 +670,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)) {

2
src/main/drivers/at32/pwm_output_at32bsp.c
View file

@ -184,7 +184,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++) {

4
src/main/drivers/at32/pwm_output_dshot.c
View file

@ -112,7 +112,7 @@ tmr_channel_select_type toCHSelectType(const uint8_t bfChannel, const bool useNC
* Enable the timer channels for all motors
*
* Called once for every dshot update if telemetry is being used (not just enabled by #def)
* Called from pwm_output_dshot_shared.c pwmStartDshotMotorUpdate
* Called from pwm_output_dshot_shared.c pwmTelemetryDecode
*/
void dshotEnableChannels(uint8_t motorCount)
{
@ -130,7 +130,7 @@ void dshotEnableChannels(uint8_t motorCount)
* Set the timer and dma of the specified motor for use as an output
*
* Called from pwmDshotMotorHardwareConfig in this file and also from
* pwmStartDshotMotorUpdate in src/main/drivers/pwm_output_dshot_shared.c
* pwmTelemetryDecode in src/main/drivers/pwm_output_dshot_shared.c
*/
FAST_CODE void pwmDshotSetDirectionOutput(
motorDmaOutput_t * const motor

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

@ -268,7 +268,7 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
startMargin = p - buffer;
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, startMargin);
if(*p & mask) {
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

2
src/main/drivers/dshot_command.c
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++) {

4
src/main/drivers/dshot_dpwm.c
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) {

2
src/main/drivers/dshot_dpwm.h
View file

@ -162,7 +162,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);

50
src/main/drivers/motor.c
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,

6
src/main/drivers/motor.h
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();

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

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

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

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

150
src/main/drivers/stm32/dshot_bitbang.c
View file

@ -46,6 +46,7 @@
#include "drivers/timer.h"
#include "pg/motor.h"
#include "pg/pinio.h"
// DEBUG_DSHOT_TELEMETRY_COUNTS
// 0 - Count of telemetry packets read
@ -337,6 +338,8 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
#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++;
@ -452,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;
@ -504,7 +507,41 @@ 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) {
@ -512,76 +549,49 @@ static bool bbUpdateStart(void)
const timeMs_t currentTimeMs = millis();
#endif
// Wait for telemetry reception to complete before decode
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) {
return false;
}
} while (telemetryPending);
if (telemetryWait) {
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
} else {
#ifdef USE_DSHOT_CACHE_MGMT
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].pinIndex);
#else
uint32_t rawValue = decode_bb(
bbMotors[motorIndex].bbPort->portInputBuffer,
bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
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) {
// Check EDT enable or store raw value
if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
} else {
dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
}
} else {
dshotTelemetryState.invalidPacketCount++;
}
#ifdef USE_DSHOT_TELEMETRY_STATS
updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
#endif
}
dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
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,
bbMotors[motorIndex].pinIndex);
#else
uint32_t rawValue = decode_bb(
bbMotors[motorIndex].bbPort->portInputBuffer,
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++;
for (int i = 0; i < usedMotorPorts; i++) {
bbDMA_Cmd(&bbPorts[i], DISABLE);
bbOutputDataClear(bbPorts[i].portOutputBuffer);
if (rawValue != DSHOT_TELEMETRY_INVALID) {
// Check EDT enable or store raw value
if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
} else {
dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
}
} else {
dshotTelemetryState.invalidPacketCount++;
}
#ifdef USE_DSHOT_TELEMETRY_STATS
updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
#endif
}
dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
}
#endif
return true;
}
@ -718,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,
@ -765,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;

2
src/main/drivers/stm32/pwm_output.c
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++) {

2
src/main/target/SITL/sitl.c
View file

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