From 2c340a63d3e623e5ca2d6360c25027ba932e4cf5 Mon Sep 17 00:00:00 2001 From: blckmn Date: Thu, 23 Jan 2025 16:22:32 +1100 Subject: [PATCH] Consolidating motorCounts, and removing dependencies. --- src/main/drivers/dshot.c | 13 ++++---- src/main/drivers/dshot.h | 3 +- src/main/drivers/dshot_bitbang.h | 2 +- src/main/drivers/motor.c | 9 +++--- src/main/drivers/motor.h | 7 +---- src/main/drivers/motor_impl.h | 1 + src/main/drivers/pwm_output.c | 4 +++ src/main/drivers/pwm_output.h | 3 +- src/platform/APM32/dshot_bitbang.c | 31 +++++++------------ src/platform/APM32/pwm_output_apm32.c | 23 +++++++------- src/platform/APM32/pwm_output_dshot_apm32.c | 2 +- src/platform/AT32/dshot_bitbang.c | 29 ++++++----------- src/platform/AT32/pwm_output_at32bsp.c | 28 ++++++++--------- src/platform/AT32/pwm_output_dshot.c | 2 +- src/platform/SIMULATOR/sitl.c | 19 ++++++------ src/platform/STM32/dshot_bitbang.c | 31 +++++++------------ src/platform/STM32/pwm_output_dshot.c | 2 +- src/platform/STM32/pwm_output_dshot_hal.c | 2 +- src/platform/STM32/pwm_output_hw.c | 20 ++++++------ .../common/stm32/dshot_bitbang_impl.h | 1 + .../common/stm32/dshot_bitbang_shared.c | 10 ++++++ src/platform/common/stm32/dshot_dpwm.c | 17 +++++----- .../common/stm32/pwm_output_dshot_shared.c | 13 ++++---- .../common/stm32/pwm_output_dshot_shared.h | 1 - 24 files changed, 128 insertions(+), 145 deletions(-) diff --git a/src/main/drivers/dshot.c b/src/main/drivers/dshot.c index a2e6953378..ec73e04327 100644 --- a/src/main/drivers/dshot.c +++ b/src/main/drivers/dshot.c @@ -45,7 +45,6 @@ #include "drivers/dshot_command.h" #include "drivers/nvic.h" -#include "flight/mixer.h" #include "pg/rpm_filter.h" @@ -55,6 +54,8 @@ #define ERPM_PER_LSB 100.0f +FAST_DATA_ZERO_INIT uint8_t dshotMotorCount = 0; + void dshotInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow) { float outputLimitOffset = DSHOT_RANGE * (1 - outputLimit); @@ -162,7 +163,7 @@ void initDshotTelemetry(const timeUs_t looptimeUs) if (motorConfig()->dev.useDshotTelemetry) { // init LPFs for RPM data - for (int i = 0; i < getMotorCount(); i++) { + for (unsigned i = 0; i < dshotMotorCount; i++) { pt1FilterInit(&motorFreqLpf[i], pt1FilterGain(rpmFilterConfig()->rpm_filter_lpf_hz, looptimeUs * 1e-6f)); } } @@ -188,7 +189,7 @@ static uint32_t dshot_decode_eRPM_telemetry_value(uint16_t value) static void dshot_decode_telemetry_value(uint8_t motorIndex, uint32_t *pDecoded, dshotTelemetryType_t *pType) { uint16_t value = dshotTelemetryState.motorState[motorIndex].rawValue; - const unsigned motorCount = motorDeviceCount(); + const unsigned motorCount = dshotMotorCount; if (dshotTelemetryState.motorState[motorIndex].telemetryTypes == DSHOT_NORMAL_TELEMETRY_MASK) { /* Check DSHOT_TELEMETRY_TYPE_eRPM mask */ // Decode eRPM telemetry @@ -301,7 +302,7 @@ FAST_CODE_NOINLINE void updateDshotTelemetry(void) return; } - const unsigned motorCount = motorDeviceCount(); + const unsigned motorCount = dshotMotorCount; uint32_t erpmTotal = 0; uint32_t rpmSamples = 0; @@ -330,7 +331,7 @@ FAST_CODE_NOINLINE void updateDshotTelemetry(void) // update filtered rotation speed of motors for features (e.g. "RPM filter") minMotorFrequencyHz = FLT_MAX; - for (int motor = 0; motor < getMotorCount(); motor++) { + for (unsigned motor = 0; motor < dshotMotorCount; motor++) { motorFrequencyHz[motor] = pt1FilterApply(&motorFreqLpf[motor], erpmToHz * getDshotErpm(motor)); minMotorFrequencyHz = MIN(minMotorFrequencyHz, motorFrequencyHz[motor]); } @@ -371,7 +372,7 @@ bool isDshotMotorTelemetryActive(uint8_t motorIndex) bool isDshotTelemetryActive(void) { - const unsigned motorCount = motorDeviceCount(); + const unsigned motorCount = dshotMotorCount; if (motorCount) { for (unsigned i = 0; i < motorCount; i++) { if (!isDshotMotorTelemetryActive(i)) { diff --git a/src/main/drivers/dshot.h b/src/main/drivers/dshot.h index 6208f1a989..19105b0659 100644 --- a/src/main/drivers/dshot.h +++ b/src/main/drivers/dshot.h @@ -93,6 +93,7 @@ uint16_t dshotConvertToExternal(float motorValue); uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb); extern bool useDshotTelemetry; +extern uint8_t dshotMotorCount; #ifdef USE_DSHOT_TELEMETRY @@ -126,7 +127,7 @@ FAST_DATA_ZERO_INIT extern dshotTelemetryCycleCounters_t dshotDMAHandlerCycleCou #endif -void dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig); +bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig); extern dshotTelemetryState_t dshotTelemetryState; diff --git a/src/main/drivers/dshot_bitbang.h b/src/main/drivers/dshot_bitbang.h index a9aacd2d79..b791240815 100644 --- a/src/main/drivers/dshot_bitbang.h +++ b/src/main/drivers/dshot_bitbang.h @@ -38,7 +38,7 @@ typedef enum { DSHOT_BITBANG_STATUS_TOO_MANY_PORTS, } dshotBitbangStatus_e; -void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig); +bool dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig); dshotBitbangStatus_e dshotBitbangGetStatus(); const timerHardware_t *dshotBitbangTimerGetAllocatedByNumberAndChannel(int8_t timerNumber, uint16_t timerChannel); const resourceOwner_t *dshotBitbangTimerGetOwner(const timerHardware_t *timer); diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c index c76d84cab0..124554928e 100644 --- a/src/main/drivers/motor.c +++ b/src/main/drivers/motor.c @@ -283,29 +283,30 @@ void motorDevInit(unsigned motorCount) } #endif + bool success = false; motorDevice.count = motorCount; if (isMotorProtocolEnabled()) { do { if (!isMotorProtocolDshot()) { #ifdef USE_PWM_OUTPUT - motorPwmDevInit(&motorDevice, motorDevConfig, idlePulse); + success = motorPwmDevInit(&motorDevice, motorDevConfig, idlePulse); #endif break; } #ifdef USE_DSHOT #ifdef USE_DSHOT_BITBANG if (isDshotBitbangActive(motorDevConfig)) { - dshotBitbangDevInit(&motorDevice, motorDevConfig); + success = dshotBitbangDevInit(&motorDevice, motorDevConfig); break; } #endif - dshotPwmDevInit(&motorDevice, motorDevConfig); + success = dshotPwmDevInit(&motorDevice, motorDevConfig); #endif } while(0); } // if the VTable has been populated, the device is initialized. - if (motorDevice.vTable) { + if (success) { motorDevice.initialized = true; motorDevice.motorEnableTimeMs = 0; motorDevice.enabled = false; diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h index 80ea50a97a..d43f430446 100644 --- a/src/main/drivers/motor.h +++ b/src/main/drivers/motor.h @@ -28,12 +28,7 @@ #include "drivers/motor_types.h" -void motorPostInitNull(); -void motorWriteNull(uint8_t index, float value); -bool motorDecodeTelemetryNull(void); -void motorUpdateCompleteNull(void); - -void motorPostInit(); +void motorPostInit(void); void motorWriteAll(float *values); void motorRequestTelemetry(unsigned index); diff --git a/src/main/drivers/motor_impl.h b/src/main/drivers/motor_impl.h index 02d9f226fd..c5defbc451 100644 --- a/src/main/drivers/motor_impl.h +++ b/src/main/drivers/motor_impl.h @@ -24,6 +24,7 @@ #include "drivers/motor_types.h" void motorPostInitNull(void); +void motorWriteNull(uint8_t index, float value); bool motorDecodeTelemetryNull(void); void motorUpdateCompleteNull(void); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 7d37318928..813b97e856 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -24,9 +24,13 @@ #include "pg/motor.h" #include "fc/rc_controls.h" // for flight3DConfig_t #include "config/feature.h" +#include "drivers/pwm_output.h" #if defined(USE_PWM_OUTPUT) && defined(USE_MOTOR) +FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; +FAST_DATA_ZERO_INIT uint8_t pwmMotorCount; + void analogInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow) { if (featureIsEnabled(FEATURE_3D)) { diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 4b09c8f571..644e2415cc 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -51,8 +51,9 @@ typedef struct { } pwmOutputPort_t; extern FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; +extern FAST_DATA_ZERO_INIT uint8_t pwmMotorCount; -void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorDevConfig, uint16_t idlePulse); +bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorDevConfig, uint16_t idlePulse); typedef struct servoDevConfig_s { // PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms) diff --git a/src/platform/APM32/dshot_bitbang.c b/src/platform/APM32/dshot_bitbang.c index 52c3d8b43f..1374ce40e6 100644 --- a/src/platform/APM32/dshot_bitbang.c +++ b/src/platform/APM32/dshot_bitbang.c @@ -56,16 +56,6 @@ // 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; - -FAST_DATA_ZERO_INIT bbPort_t bbPorts[MAX_SUPPORTED_MOTOR_PORTS]; -FAST_DATA_ZERO_INIT int usedMotorPorts; - -FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS]; - -dshotBitbangStatus_e bbStatus; - // For MCUs that use MPU to control DMA coherency, there might be a performance hit // on manipulating input buffer content especially if it is read multiple times, // as the buffer region is attributed as not cachable. @@ -523,7 +513,7 @@ static bool bbDecodeTelemetry(void) 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++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { #ifdef APM32F4 uint32_t rawValue = decode_bb_bitband( bbMotors[motorIndex].bbPort->portInputBuffer, @@ -606,7 +596,7 @@ static void bbUpdateComplete(void) // If there is a dshot command loaded up, time it correctly with motor update if (!dshotCommandQueueEmpty()) { - if (!dshotCommandOutputIsEnabled(motorCount)) { + if (!dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } } @@ -641,7 +631,7 @@ static void bbUpdateComplete(void) static bool bbEnableMotors(void) { - for (int i = 0; i < motorCount; i++) { + for (int i = 0; i < dshotMotorCount; i++) { if (bbMotors[i].configured) { IOConfigGPIO(bbMotors[i].io, bbMotors[i].iocfg); } @@ -668,7 +658,7 @@ static void bbPostInit(void) { bbFindPacerTimer(); - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) { return; @@ -701,18 +691,18 @@ dshotBitbangStatus_e dshotBitbangGetStatus(void) return bbStatus; } -void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) +bool dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) { dbgPinLo(0); dbgPinLo(1); if (!device || !motorConfig) { - return; + return false; } motorProtocol = motorConfig->motorProtocol; device->vTable = &bbVTable; - motorCount = device->count; + dshotMotorCount = device->count; bbStatus = DSHOT_BITBANG_STATUS_OK; #ifdef USE_DSHOT_TELEMETRY @@ -721,7 +711,7 @@ void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorCon memset(bbOutputBuffer, 0, sizeof(bbOutputBuffer)); - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; const timerHardware_t *timerHardware = timerGetConfiguredByTag(motorConfig->ioTags[reorderedMotorIndex]); const IO_t io = IOGetByTag(motorConfig->ioTags[reorderedMotorIndex]); @@ -738,9 +728,9 @@ void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorCon if (!IOIsFreeOrPreinit(io)) { /* not enough motors initialised for the mixer or a break in the motors */ device->vTable = NULL; - motorCount = 0; + dshotMotorCount = 0; bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT; - return; + return false; } int pinIndex = IO_GPIOPinIdx(io); @@ -760,6 +750,7 @@ void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorCon IOHi(io); } } + return true; } #endif // USE_DSHOT_BITBANG diff --git a/src/platform/APM32/pwm_output_apm32.c b/src/platform/APM32/pwm_output_apm32.c index ace22dc396..5b49aa0326 100644 --- a/src/platform/APM32/pwm_output_apm32.c +++ b/src/platform/APM32/pwm_output_apm32.c @@ -35,8 +35,6 @@ #include "pg/motor.h" -FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; - static void pwmOCConfig(TMR_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) { TMR_HandleTypeDef* Handle = timerFindTimerHandle(tim); @@ -85,7 +83,6 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, *channel->ccr = 0; } -static int motorCount = 0; static bool useContinuousUpdate = true; static void pwmWriteStandard(uint8_t index, float value) @@ -96,7 +93,7 @@ static void pwmWriteStandard(uint8_t index, float value) void pwmShutdownPulsesForAllMotors(void) { - for (int index = 0; index < motorCount; index++) { + for (int index = 0; index < pwmMotorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows if (motors[index].channel.ccr) { *motors[index].channel.ccr = 0; @@ -113,7 +110,7 @@ static motorVTable_t motorPwmVTable; bool pwmEnableMotors(void) { /* check motors can be enabled */ - return motorCount > 0; + return pwmMotorCount > 0; } bool pwmIsMotorEnabled(unsigned index) @@ -127,7 +124,7 @@ static void pwmCompleteMotorUpdate(void) return; } - for (int index = 0; index < motorCount; index++) { + for (int index = 0; index < pwmMotorCount; index++) { if (motors[index].forceOverflow) { timerForceOverflow(motors[index].channel.tim); } @@ -162,15 +159,15 @@ static motorVTable_t motorPwmVTable = { .isMotorIdle = NULL, }; -void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) +bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { memset(motors, 0, sizeof(motors)); if (!device || !motorConfig) { - return; + return false; } - motorCount = device->count; + pwmMotorCount = device->count; device->vTable = &motorPwmVTable; useContinuousUpdate = motorConfig->useContinuousUpdate; @@ -203,7 +200,7 @@ void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, break; } - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < pwmMotorCount; motorIndex++) { const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex]; const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); @@ -211,9 +208,9 @@ void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, if (timerHardware == NULL) { /* not enough motors initialised for the mixer or a break in the motors */ device->vTable = NULL; - motorCount = 0; + pwmMotorCount = 0; /* TODO: block arming and add reason system cannot arm */ - return; + return false; } motors[motorIndex].io = IOGetByTag(tag); @@ -251,6 +248,8 @@ void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, motors[motorIndex].forceOverflow = !timerAlreadyUsed; motors[motorIndex].enabled = true; } + + return true; } pwmOutputPort_t *pwmGetMotors(void) diff --git a/src/platform/APM32/pwm_output_dshot_apm32.c b/src/platform/APM32/pwm_output_dshot_apm32.c index 33118007ec..65ed998a9b 100644 --- a/src/platform/APM32/pwm_output_dshot_apm32.c +++ b/src/platform/APM32/pwm_output_dshot_apm32.c @@ -127,7 +127,7 @@ FAST_CODE static void pwmDshotSetDirectionInput( FAST_CODE void pwmCompleteDshotMotorUpdate(void) { /* If there is a dshot command loaded up, time it correctly with motor update*/ - if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(motorCount)) { + if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } diff --git a/src/platform/AT32/dshot_bitbang.c b/src/platform/AT32/dshot_bitbang.c index 50cfeea854..079a41ddee 100644 --- a/src/platform/AT32/dshot_bitbang.c +++ b/src/platform/AT32/dshot_bitbang.c @@ -49,16 +49,6 @@ // 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; - -FAST_DATA_ZERO_INIT bbPort_t bbPorts[MAX_SUPPORTED_MOTOR_PORTS]; -FAST_DATA_ZERO_INIT int usedMotorPorts; - -FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS]; - -dshotBitbangStatus_e bbStatus; - // For MCUs that use MPU to control DMA coherency, there might be a performance hit // on manipulating input buffer content especially if it is read multiple times, // as the buffer region is attributed as not cachable. @@ -503,7 +493,7 @@ static bool bbDecodeTelemetry(void) 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++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { uint32_t rawValue = decode_bb_bitband( bbMotors[motorIndex].bbPort->portInputBuffer, @@ -586,7 +576,7 @@ static void bbUpdateComplete(void) // If there is a dshot command loaded up, time it correctly with motor update if (!dshotCommandQueueEmpty()) { - if (!dshotCommandOutputIsEnabled(motorCount)) { + if (!dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } } @@ -633,7 +623,7 @@ static void bbUpdateComplete(void) static bool bbEnableMotors(void) { - for (int i = 0; i < motorCount; i++) { + for (int i = 0; i < dshotMotorCount; i++) { if (bbMotors[i].configured) { IOConfigGPIO(bbMotors[i].io, bbMotors[i].iocfg); } @@ -660,7 +650,7 @@ static void bbPostInit(void) { bbFindPacerTimer(); - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) { return; @@ -693,14 +683,14 @@ dshotBitbangStatus_e dshotBitbangGetStatus(void) return bbStatus; } -void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) +bool dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) { dbgPinLo(0); dbgPinLo(1); motorProtocol = motorConfig->motorProtocol; device->vTable = &bbVTable; - motorCount = device->count; + dshotMotorCount = device->count; bbStatus = DSHOT_BITBANG_STATUS_OK; @@ -710,7 +700,7 @@ void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorCon memset(bbOutputBuffer, 0, sizeof(bbOutputBuffer)); - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; const timerHardware_t *timerHardware = timerGetConfiguredByTag(motorConfig->ioTags[reorderedMotorIndex]); const IO_t io = IOGetByTag(motorConfig->ioTags[reorderedMotorIndex]); @@ -728,8 +718,8 @@ void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorCon /* not enough motors initialised for the mixer or a break in the motors */ bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT; device->vTable = NULL; - motorCount = 0; - return; + dshotMotorCount = 0; + return false; } int pinIndex = IO_GPIOPinIdx(io); @@ -747,6 +737,7 @@ void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorCon IOHi(io); } } + return true; } #endif // USE_DSHOT_BB diff --git a/src/platform/AT32/pwm_output_at32bsp.c b/src/platform/AT32/pwm_output_at32bsp.c index b69b7258e8..1a4be7ea07 100644 --- a/src/platform/AT32/pwm_output_at32bsp.c +++ b/src/platform/AT32/pwm_output_at32bsp.c @@ -28,16 +28,13 @@ #ifdef USE_PWM_OUTPUT #include "drivers/io.h" -#include "drivers/motor.h" +#include "drivers/motor_impl.h" #include "drivers/pwm_output.h" #include "drivers/time.h" #include "drivers/timer.h" #include "pg/motor.h" -FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; -static int motorCount = 0; - static void pwmOCConfig(tmr_type *tim, uint8_t channel, uint16_t value, uint8_t output) { tmr_output_config_type tmr_OCInitStruct; @@ -77,7 +74,7 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, *channel->ccr = 0; } -static FAST_DATA_ZERO_INIT motorDevice_t *motorPwmDevice; +static FAST_DATA_ZERO_INIT motorDevice_t *pwmMotorDevice; static void pwmWriteStandard(uint8_t index, float value) { @@ -87,7 +84,7 @@ static void pwmWriteStandard(uint8_t index, float value) void pwmShutdownPulsesForAllMotors(void) { - for (int index = 0; index < motorCount; index++) { + for (int index = 0; index < pwmMotorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows if (motors[index].channel.ccr) { *motors[index].channel.ccr = 0; @@ -104,7 +101,7 @@ static motorVTable_t motorPwmVTable; bool pwmEnableMotors(void) { /* check motors can be enabled */ - return (motorPwmDevice->vTable); + return (pwmMotorDevice->vTable); } bool pwmIsMotorEnabled(unsigned index) @@ -120,7 +117,7 @@ static void pwmCompleteMotorUpdate(void) return; } - for (int index = 0; index < motorCount; index++) { + for (int index = 0; index < pwmMotorCount; index++) { if (motors[index].forceOverflow) { timerForceOverflow(motors[index].channel.tim); } @@ -155,17 +152,17 @@ static motorVTable_t motorPwmVTable = { .isMotorIdle = NULL, }; -void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) +bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { memset(motors, 0, sizeof(motors)); if (!device) { - return; + return false; } device->vTable = &motorPwmVTable; - motorPwmDevice = device; - motorCount = device->count; + pwmMotorDevice = device; + pwmMotorCount = device->count; useContinuousUpdate = motorConfig->useContinuousUpdate; float sMin = 0; @@ -197,7 +194,7 @@ void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, break; } - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < pwmMotorCount; motorIndex++) { const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex]; const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); @@ -205,9 +202,9 @@ void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, if (timerHardware == NULL) { /* not enough motors initialised for the mixer or a break in the motors */ device->vTable = NULL; - motorCount = 0; + pwmMotorCount = 0; /* TODO: block arming and add reason system cannot arm */ - return; + return false; } motors[motorIndex].io = IOGetByTag(tag); @@ -245,6 +242,7 @@ void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, motors[motorIndex].forceOverflow = !timerAlreadyUsed; motors[motorIndex].enabled = true; } + return true; } pwmOutputPort_t *pwmGetMotors(void) diff --git a/src/platform/AT32/pwm_output_dshot.c b/src/platform/AT32/pwm_output_dshot.c index 10686c8c03..b91a116e07 100644 --- a/src/platform/AT32/pwm_output_dshot.c +++ b/src/platform/AT32/pwm_output_dshot.c @@ -249,7 +249,7 @@ void pwmCompleteDshotMotorUpdate(void) { // If there is a dshot command loaded up, time it correctly with motor update if (!dshotCommandQueueEmpty()) { - if (!dshotCommandOutputIsEnabled(motorCount)) { + if (!dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } } diff --git a/src/platform/SIMULATOR/sitl.c b/src/platform/SIMULATOR/sitl.c index 14ceca0310..aa262031d1 100644 --- a/src/platform/SIMULATOR/sitl.c +++ b/src/platform/SIMULATOR/sitl.c @@ -32,7 +32,7 @@ #include "drivers/io.h" #include "drivers/dma.h" -#include "drivers/motor.h" +#include "drivers/motor_impl.h" #include "drivers/serial.h" #include "drivers/serial_tcp.h" #include "drivers/system.h" @@ -504,7 +504,6 @@ int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y) } // PWM part -pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS]; // real value to send @@ -521,7 +520,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig) } } -static motorDevice_t motorPwmDevice; // Forward +static motorDevice_t pwmMotorDevice; // Forward pwmOutputPort_t *pwmGetMotors(void) { @@ -540,12 +539,12 @@ static uint16_t pwmConvertToExternal(float motorValue) static void pwmDisableMotors(void) { - motorPwmDevice.enabled = false; + pwmMotorDevice.enabled = false; } static bool pwmEnableMotors(void) { - motorPwmDevice.enabled = true; + pwmMotorDevice.enabled = true; return true; } @@ -572,7 +571,7 @@ static void pwmWriteMotorInt(uint8_t index, uint16_t value) static void pwmShutdownPulsesForAllMotors(void) { - motorPwmDevice.enabled = false; + pwmMotorDevice.enabled = false; } bool pwmIsMotorEnabled(unsigned index) @@ -628,15 +627,15 @@ static const motorVTable_t vTable = { }; -void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t _idlePulse) +bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t _idlePulse) { UNUSED(motorConfig); if (!device) { - return; + return false; } device->vTable = &vTable; - uint8_t motorCount = device->count; + const uint8_t motorCount = device->count; printf("Initialized motor count %d\n", motorCount); pwmRawPkt.motorCount = motorCount; @@ -645,6 +644,8 @@ void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { motors[motorIndex].enabled = true; } + + return true; } // ADC part diff --git a/src/platform/STM32/dshot_bitbang.c b/src/platform/STM32/dshot_bitbang.c index cbaeff1135..3cf7db42ce 100644 --- a/src/platform/STM32/dshot_bitbang.c +++ b/src/platform/STM32/dshot_bitbang.c @@ -56,16 +56,6 @@ // 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; - -FAST_DATA_ZERO_INIT bbPort_t bbPorts[MAX_SUPPORTED_MOTOR_PORTS]; -FAST_DATA_ZERO_INIT int usedMotorPorts; - -FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS]; - -dshotBitbangStatus_e bbStatus; - // For MCUs that use MPU to control DMA coherency, there might be a performance hit // on manipulating input buffer content especially if it is read multiple times, // as the buffer region is attributed as not cachable. @@ -552,7 +542,7 @@ static bool bbDecodeTelemetry(void) 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++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { #ifdef STM32F4 uint32_t rawValue = decode_bb_bitband( bbMotors[motorIndex].bbPort->portInputBuffer, @@ -635,7 +625,7 @@ static void bbUpdateComplete(void) // If there is a dshot command loaded up, time it correctly with motor update if (!dshotCommandQueueEmpty()) { - if (!dshotCommandOutputIsEnabled(motorCount)) { + if (!dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } } @@ -673,7 +663,7 @@ static void bbUpdateComplete(void) static bool bbEnableMotors(void) { - for (int i = 0; i < motorCount; i++) { + for (int i = 0; i < dshotMotorCount; i++) { if (bbMotors[i].configured) { IOConfigGPIO(bbMotors[i].io, bbMotors[i].iocfg); } @@ -700,7 +690,7 @@ static void bbPostInit(void) { bbFindPacerTimer(); - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) { return; @@ -733,19 +723,19 @@ dshotBitbangStatus_e dshotBitbangGetStatus(void) return bbStatus; } -void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) +bool dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) { dbgPinLo(0); dbgPinLo(1); if (!device || !motorConfig) { - return; + return false; } motorProtocol = motorConfig->motorProtocol; device->vTable = &bbVTable; - motorCount = device->count; + dshotMotorCount = device->count; bbStatus = DSHOT_BITBANG_STATUS_OK; #ifdef USE_DSHOT_TELEMETRY @@ -754,7 +744,7 @@ void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorCon memset(bbOutputBuffer, 0, sizeof(bbOutputBuffer)); - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; const timerHardware_t *timerHardware = timerGetConfiguredByTag(motorConfig->ioTags[reorderedMotorIndex]); const IO_t io = IOGetByTag(motorConfig->ioTags[reorderedMotorIndex]); @@ -771,9 +761,9 @@ void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorCon if (!IOIsFreeOrPreinit(io)) { /* not enough motors initialised for the mixer or a break in the motors */ device->vTable = NULL; - motorCount = 0; + dshotMotorCount = 0; bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT; - return; + return false; } int pinIndex = IO_GPIOPinIdx(io); @@ -796,6 +786,7 @@ void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorCon } } + return true; } #endif // USE_DSHOT_BB diff --git a/src/platform/STM32/pwm_output_dshot.c b/src/platform/STM32/pwm_output_dshot.c index e989b1e738..dcf2bba859 100644 --- a/src/platform/STM32/pwm_output_dshot.c +++ b/src/platform/STM32/pwm_output_dshot.c @@ -144,7 +144,7 @@ void pwmCompleteDshotMotorUpdate(void) { /* If there is a dshot command loaded up, time it correctly with motor update*/ if (!dshotCommandQueueEmpty()) { - if (!dshotCommandOutputIsEnabled(motorCount)) { + if (!dshotCommandOutputIsEnabled(pwmMotorCount)) { return; } } diff --git a/src/platform/STM32/pwm_output_dshot_hal.c b/src/platform/STM32/pwm_output_dshot_hal.c index 4e2589c698..3e6fe722a3 100644 --- a/src/platform/STM32/pwm_output_dshot_hal.c +++ b/src/platform/STM32/pwm_output_dshot_hal.c @@ -138,7 +138,7 @@ FAST_CODE static void pwmDshotSetDirectionInput( FAST_CODE void pwmCompleteDshotMotorUpdate(void) { /* If there is a dshot command loaded up, time it correctly with motor update*/ - if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(motorCount)) { + if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } diff --git a/src/platform/STM32/pwm_output_hw.c b/src/platform/STM32/pwm_output_hw.c index d2889e3a26..67284840cb 100644 --- a/src/platform/STM32/pwm_output_hw.c +++ b/src/platform/STM32/pwm_output_hw.c @@ -34,9 +34,6 @@ #include "drivers/motor_impl.h" -FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; - -static int motorCount = 0; static bool useContinuousUpdate = true; static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) @@ -118,7 +115,7 @@ static void pwmWriteStandard(uint8_t index, float value) void pwmShutdownPulsesForAllMotors(void) { - for (int index = 0; motorCount; index++) { + for (int index = 0; pwmMotorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows if (motors[index].channel.ccr) { *motors[index].channel.ccr = 0; @@ -134,7 +131,7 @@ void pwmDisableMotors(void) bool pwmEnableMotors(void) { /* check motors can be enabled */ - return motorCount > 0; + return pwmMotorCount > 0; } bool pwmIsMotorEnabled(unsigned index) @@ -148,7 +145,7 @@ static void pwmCompleteMotorUpdate(void) return; } - for (int index = 0; motorCount; index++) { + for (int index = 0; pwmMotorCount; index++) { if (motors[index].forceOverflow) { timerForceOverflow(motors[index].channel.tim); } @@ -183,11 +180,11 @@ static const motorVTable_t motorPwmVTable = { .isMotorIdle = NULL, }; -void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) +bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { memset(motors, 0, sizeof(motors)); - motorCount = device->count; + pwmMotorCount = device->count; device->vTable = &motorPwmVTable; useContinuousUpdate = motorConfig->useContinuousUpdate; @@ -220,7 +217,7 @@ void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, break; } - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < pwmMotorCount; motorIndex++) { const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex]; const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); @@ -228,9 +225,9 @@ void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, if (timerHardware == NULL) { /* not enough motors initialised for the mixer or a break in the motors */ device->vTable = NULL; - motorCount = 0; + pwmMotorCount = 0; /* TODO: block arming and add reason system cannot arm */ - return; + return false; } motors[motorIndex].io = IOGetByTag(tag); @@ -269,6 +266,7 @@ void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, motors[motorIndex].enabled = true; } + return true; } pwmOutputPort_t *pwmGetMotors(void) diff --git a/src/platform/common/stm32/dshot_bitbang_impl.h b/src/platform/common/stm32/dshot_bitbang_impl.h index 32c1aa40c7..4e062e86c2 100644 --- a/src/platform/common/stm32/dshot_bitbang_impl.h +++ b/src/platform/common/stm32/dshot_bitbang_impl.h @@ -229,6 +229,7 @@ extern FAST_DATA_ZERO_INIT int usedMotorPorts; extern FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS]; extern uint8_t bbPuPdMode; +extern dshotBitbangStatus_e bbStatus; // DMA buffers // Note that we are not sharing input and output buffers, diff --git a/src/platform/common/stm32/dshot_bitbang_shared.c b/src/platform/common/stm32/dshot_bitbang_shared.c index f2d88b24a5..c5152d59f3 100644 --- a/src/platform/common/stm32/dshot_bitbang_shared.c +++ b/src/platform/common/stm32/dshot_bitbang_shared.c @@ -21,6 +21,16 @@ #include "dshot_bitbang_impl.h" +FAST_DATA_ZERO_INIT bbPacer_t bbPacers[MAX_MOTOR_PACERS]; // TIM1 or TIM8 +FAST_DATA_ZERO_INIT int usedMotorPacers = 0; + +FAST_DATA_ZERO_INIT bbPort_t bbPorts[MAX_SUPPORTED_MOTOR_PORTS]; +FAST_DATA_ZERO_INIT int usedMotorPorts; + +FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS]; + +dshotBitbangStatus_e bbStatus; + void bbDshotRequestTelemetry(unsigned motorIndex) { if (motorIndex >= ARRAYLEN(bbMotors)) { diff --git a/src/platform/common/stm32/dshot_dpwm.c b/src/platform/common/stm32/dshot_dpwm.c index aa10d1869e..cee7aa6003 100644 --- a/src/platform/common/stm32/dshot_dpwm.c +++ b/src/platform/common/stm32/dshot_dpwm.c @@ -33,7 +33,7 @@ #include "pwm_output_dshot_shared.h" #include "drivers/dshot.h" #include "dshot_dpwm.h" -#include "drivers/motor.h" +#include "drivers/motor_impl.h" #include "pg/motor.h" @@ -111,7 +111,7 @@ static void dshotPwmDisableMotors(void) static bool dshotPwmEnableMotors(void) { - for (int i = 0; i < motorCount; i++) { + for (int i = 0; i < dshotMotorCount; i++) { motorDmaOutput_t *motor = getMotorDmaOutput(i); const IO_t motorIO = IOGetByTag(motor->timerHardware->tag); IOConfigGPIOAF(motorIO, motor->iocfg, motor->timerHardware->alternateFunction); @@ -152,10 +152,10 @@ static const motorVTable_t dshotPwmVTable = { .isMotorIdle = pwmDshotIsMotorIdle, }; -void dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) +bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) { device->vTable = &dshotPwmVTable; - motorCount = device->count; + dshotMotorCount = device->count; #ifdef USE_DSHOT_TELEMETRY useDshotTelemetry = motorConfig->useDshotTelemetry; #endif @@ -175,7 +175,7 @@ void dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) break; } - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) { const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex]; const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); @@ -196,11 +196,12 @@ void dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) } /* not enough motors initialised for the mixer or a break in the motors */ - device->vTable = NULL; - motorCount = 0; + dshotMotorCount = 0; /* TODO: block arming and add reason system cannot arm */ - return; + return false; } + + return true; } #endif // USE_DSHOT diff --git a/src/platform/common/stm32/pwm_output_dshot_shared.c b/src/platform/common/stm32/pwm_output_dshot_shared.c index 21de80a11b..ccf2ab661d 100644 --- a/src/platform/common/stm32/pwm_output_dshot_shared.c +++ b/src/platform/common/stm32/pwm_output_dshot_shared.c @@ -49,7 +49,6 @@ #include "pwm_output_dshot_shared.h" FAST_DATA_ZERO_INIT uint8_t dmaMotorTimerCount = 0; -FAST_DATA_ZERO_INIT uint8_t motorCount = 0; #ifdef STM32F7 FAST_DATA_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; @@ -216,7 +215,9 @@ static uint32_t decodeTelemetryPacket(const uint32_t buffer[], uint32_t count) */ FAST_CODE_NOINLINE bool pwmTelemetryDecode(void) { -#ifdef USE_DSHOT_TELEMETRY +#ifndef USE_DSHOT_TELEMETRY + return true; +#else if (!useDshotTelemetry) { return true; } @@ -226,7 +227,7 @@ FAST_CODE_NOINLINE bool pwmTelemetryDecode(void) #endif const timeUs_t currentUs = micros(); - for (int i = 0; i < motorCount; i++) { + for (int i = 0; i < dshotMotorCount; i++) { timeDelta_t usSinceInput = cmpTimeUs(currentUs, inputStampUs); if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) { return false; @@ -276,11 +277,9 @@ FAST_CODE_NOINLINE bool pwmTelemetryDecode(void) dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED; inputStampUs = 0; - dshotEnableChannels(motorCount); - - -#endif // USE_DSHOT_TELEMETRY + dshotEnableChannels(dshotMotorCount); return true; +#endif // USE_DSHOT_TELEMETRY } #endif // USE_DSHOT diff --git a/src/platform/common/stm32/pwm_output_dshot_shared.h b/src/platform/common/stm32/pwm_output_dshot_shared.h index dbcb2b8fb1..38f226b305 100644 --- a/src/platform/common/stm32/pwm_output_dshot_shared.h +++ b/src/platform/common/stm32/pwm_output_dshot_shared.h @@ -24,7 +24,6 @@ #include "dshot_dpwm.h" extern FAST_DATA_ZERO_INIT uint8_t dmaMotorTimerCount; -extern FAST_DATA_ZERO_INIT uint8_t motorCount; extern FAST_DATA_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; extern FAST_DATA_ZERO_INIT motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];