diff --git a/src/main/config/config.c b/src/main/config/config.c index e5e867232d..458dfae362 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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)) { diff --git a/src/main/drivers/at32/exti_at32.c b/src/main/drivers/at32/exti_at32.c index 73f0873ec5..78f40edea5 100644 --- a/src/main/drivers/at32/exti_at32.c +++ b/src/main/drivers/at32/exti_at32.c @@ -183,4 +183,4 @@ _EXTI_IRQ_HANDLER(EXINT4_IRQHandler, 0x0010); _EXTI_IRQ_HANDLER(EXINT9_5_IRQHandler, 0x03e0); _EXTI_IRQ_HANDLER(EXINT15_10_IRQHandler, 0xfc00); -#endif \ No newline at end of file +#endif diff --git a/src/main/drivers/at32/pwm_output_at32bsp.c b/src/main/drivers/at32/pwm_output_at32bsp.c index 530e46b11f..d93a51c19d 100644 --- a/src/main/drivers/at32/pwm_output_at32bsp.c +++ b/src/main/drivers/at32/pwm_output_at32bsp.c @@ -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++) { diff --git a/src/main/drivers/at32/pwm_output_dshot.c b/src/main/drivers/at32/pwm_output_dshot.c index 511defa701..96528e74c1 100644 --- a/src/main/drivers/at32/pwm_output_dshot.c +++ b/src/main/drivers/at32/pwm_output_dshot.c @@ -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 diff --git a/src/main/drivers/at32/serial_usb_vcp_at32f4.c b/src/main/drivers/at32/serial_usb_vcp_at32f4.c index 68a4285fe1..67a789149b 100644 --- a/src/main/drivers/at32/serial_usb_vcp_at32f4.c +++ b/src/main/drivers/at32/serial_usb_vcp_at32f4.c @@ -519,4 +519,4 @@ uint32_t usbVcpGetBaudRate(serialPort_t *instance) return pcdc->linecoding.bitrate; } -#endif \ No newline at end of file +#endif diff --git a/src/main/drivers/dshot_bitbang_decode.c b/src/main/drivers/dshot_bitbang_decode.c index 65d8689520..591436cdee 100644 --- a/src/main/drivers/dshot_bitbang_decode.c +++ b/src/main/drivers/dshot_bitbang_decode.c @@ -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 diff --git a/src/main/drivers/dshot_command.c b/src/main/drivers/dshot_command.c index ded30ad706..cd04ccc65f 100644 --- a/src/main/drivers/dshot_command.c +++ b/src/main/drivers/dshot_command.c @@ -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++) { diff --git a/src/main/drivers/dshot_dpwm.c b/src/main/drivers/dshot_dpwm.c index de26d589da..a7ab462f51 100644 --- a/src/main/drivers/dshot_dpwm.c +++ b/src/main/drivers/dshot_dpwm.c @@ -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) { diff --git a/src/main/drivers/dshot_dpwm.h b/src/main/drivers/dshot_dpwm.h index bbb59613de..c3b32f2f9d 100644 --- a/src/main/drivers/dshot_dpwm.h +++ b/src/main/drivers/dshot_dpwm.h @@ -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); diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c index f6917d0b47..e604303f0d 100644 --- a/src/main/drivers/motor.c +++ b/src/main/drivers/motor.c @@ -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, diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h index b8dcabaf39..fb9846aa6f 100644 --- a/src/main/drivers/motor.h +++ b/src/main/drivers/motor.h @@ -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(); diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/main/drivers/pwm_output_dshot_shared.c index 15a4845aae..a8ba64ae85 100644 --- a/src/main/drivers/pwm_output_dshot_shared.c +++ b/src/main/drivers/pwm_output_dshot_shared.c @@ -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; diff --git a/src/main/drivers/pwm_output_dshot_shared.h b/src/main/drivers/pwm_output_dshot_shared.h index 9e4ef0faf8..7c282d5008 100644 --- a/src/main/drivers/pwm_output_dshot_shared.h +++ b/src/main/drivers/pwm_output_dshot_shared.h @@ -59,7 +59,7 @@ void pwmDshotSetDirectionOutput( #endif ); -bool pwmStartDshotMotorUpdate(void); +bool pwmTelemetryDecode(void); #endif #endif diff --git a/src/main/drivers/stm32/dshot_bitbang.c b/src/main/drivers/stm32/dshot_bitbang.c index 09919cba92..52eff23825 100644 --- a/src/main/drivers/stm32/dshot_bitbang.c +++ b/src/main/drivers/stm32/dshot_bitbang.c @@ -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; diff --git a/src/main/drivers/stm32/pwm_output.c b/src/main/drivers/stm32/pwm_output.c index 1a031322f0..5c996b7bbd 100644 --- a/src/main/drivers/stm32/pwm_output.c +++ b/src/main/drivers/stm32/pwm_output.c @@ -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++) { diff --git a/src/main/target/SITL/sitl.c b/src/main/target/SITL/sitl.c index 216b015f6e..4302bf20aa 100644 --- a/src/main/target/SITL/sitl.c +++ b/src/main/target/SITL/sitl.c @@ -619,7 +619,7 @@ static motorDevice_t motorPwmDevice = { .enable = pwmEnableMotors, .disable = pwmDisableMotors, .isMotorEnabled = pwmIsMotorEnabled, - .updateStart = motorUpdateStartNull, + .decodeTelemetry = motorDecodeTelemetryNull, .write = pwmWriteMotor, .writeInt = pwmWriteMotorInt, .updateComplete = pwmCompleteMotorUpdate,