From 3dba5e65e4ab237e328b021b69df80d3109d2315 Mon Sep 17 00:00:00 2001 From: Jay Blackman Date: Fri, 31 Jan 2025 06:54:23 +1100 Subject: [PATCH] FIX: AT32 not reading ESC (#14220) --- src/main/drivers/pwm_output.c | 6 ++-- src/main/drivers/pwm_output.h | 2 +- src/platform/APM32/dshot_bitbang.c | 8 ----- src/platform/APM32/pwm_output_apm32.c | 34 +++++++++---------- src/platform/AT32/dshot_bitbang.c | 3 +- src/platform/AT32/io_at32.c | 12 +------ src/platform/AT32/pwm_output_at32bsp.c | 34 +++++++++---------- src/platform/SIMULATOR/sitl.c | 4 +-- src/platform/STM32/dshot_bitbang.c | 1 + src/platform/STM32/pwm_output_hw.c | 34 +++++++++---------- .../common/stm32/dshot_bitbang_impl.h | 1 + .../common/stm32/dshot_bitbang_shared.c | 8 +++++ src/platform/common/stm32/dshot_dpwm.c | 10 +++--- 13 files changed, 75 insertions(+), 82 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index e8ae90887b..901e9e1773 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -28,7 +28,7 @@ #if defined(USE_PWM_OUTPUT) && defined(USE_MOTOR) -FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; +FAST_DATA_ZERO_INIT pwmOutputPort_t pwmMotors[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) @@ -53,12 +53,12 @@ IO_t pwmGetMotorIO(unsigned index) if (index >= pwmMotorCount) { return IO_NONE; } - return motors[index].io; + return pwmMotors[index].io; } bool pwmIsMotorEnabled(unsigned index) { - return motors[index].enabled; + return pwmMotors[index].enabled; } bool pwmEnableMotors(void) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 644e2415cc..38198a1837 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -50,7 +50,7 @@ typedef struct { IO_t io; } pwmOutputPort_t; -extern FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; +extern FAST_DATA_ZERO_INIT pwmOutputPort_t pwmMotors[MAX_SUPPORTED_MOTORS]; extern FAST_DATA_ZERO_INIT uint8_t pwmMotorCount; bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorDevConfig, uint16_t idlePulse); diff --git a/src/platform/APM32/dshot_bitbang.c b/src/platform/APM32/dshot_bitbang.c index 33b114fbc0..cf971adf97 100644 --- a/src/platform/APM32/dshot_bitbang.c +++ b/src/platform/APM32/dshot_bitbang.c @@ -669,14 +669,6 @@ static void bbPostInit(void) } } -static IO_t bbGetMotorIO(unsigned index) -{ - if (index >= dshotMotorCount) { - return IO_NONE; - } - return bbMotors[index].io; -} - static motorVTable_t bbVTable = { .postInit = bbPostInit, .enable = bbEnableMotors, diff --git a/src/platform/APM32/pwm_output_apm32.c b/src/platform/APM32/pwm_output_apm32.c index 706591e0a0..cdec437029 100644 --- a/src/platform/APM32/pwm_output_apm32.c +++ b/src/platform/APM32/pwm_output_apm32.c @@ -90,15 +90,15 @@ static bool useContinuousUpdate = true; static void pwmWriteStandard(uint8_t index, float value) { /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */ - *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset); + *pwmMotors[index].channel.ccr = lrintf((value * pwmMotors[index].pulseScale) + pwmMotors[index].pulseOffset); } static void pwmShutdownPulsesForAllMotors(void) { 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; + if (pwmMotors[index].channel.ccr) { + *pwmMotors[index].channel.ccr = 0; } } } @@ -117,12 +117,12 @@ static void pwmCompleteMotorUpdate(void) } for (int index = 0; index < pwmMotorCount; index++) { - if (motors[index].forceOverflow) { - timerForceOverflow(motors[index].channel.tim); + if (pwmMotors[index].forceOverflow) { + timerForceOverflow(pwmMotors[index].channel.tim); } // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. - *motors[index].channel.ccr = 0; + *pwmMotors[index].channel.ccr = 0; } } @@ -154,7 +154,7 @@ static motorVTable_t motorPwmVTable = { bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { - memset(motors, 0, sizeof(motors)); + memset(pwmMotors, 0, sizeof(pwmMotors)); if (!device || !motorConfig) { return false; @@ -206,10 +206,10 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, return false; } - motors[motorIndex].io = IOGetByTag(tag); - IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); + pwmMotors[motorIndex].io = IOGetByTag(tag); + IOInit(pwmMotors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); - IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); + IOConfigGPIOAF(pwmMotors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); /* standard PWM outputs */ // margin of safety is 4 periods when unsynced @@ -226,20 +226,20 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, TODO: this can be moved back to periodMin and periodLen once mixer outputs a 0..1 float value. */ - motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; - motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); + pwmMotors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; + pwmMotors[motorIndex].pulseOffset = (sMin * hz) - (pwmMotors[motorIndex].pulseScale * 1000); - pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); + pwmOutConfig(&pwmMotors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); bool timerAlreadyUsed = false; for (int i = 0; i < motorIndex; i++) { - if (motors[i].channel.tim == motors[motorIndex].channel.tim) { + if (pwmMotors[i].channel.tim == pwmMotors[motorIndex].channel.tim) { timerAlreadyUsed = true; break; } } - motors[motorIndex].forceOverflow = !timerAlreadyUsed; - motors[motorIndex].enabled = true; + pwmMotors[motorIndex].forceOverflow = !timerAlreadyUsed; + pwmMotors[motorIndex].enabled = true; } return true; @@ -247,7 +247,7 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, pwmOutputPort_t *pwmGetMotors(void) { - return motors; + return pwmMotors; } #ifdef USE_SERVOS diff --git a/src/platform/AT32/dshot_bitbang.c b/src/platform/AT32/dshot_bitbang.c index d3ac4f2e88..fff7eea8b8 100644 --- a/src/platform/AT32/dshot_bitbang.c +++ b/src/platform/AT32/dshot_bitbang.c @@ -583,7 +583,7 @@ static void bbUpdateComplete(void) } #ifdef USE_DSHOT_CACHE_MGMT - for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; 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++) { @@ -677,6 +677,7 @@ static motorVTable_t bbVTable = { .shutdown = bbShutdown, .isMotorIdle = bbDshotIsMotorIdle, .requestTelemetry = bbDshotRequestTelemetry, + .getMotorIO = bbGetMotorIO, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) diff --git a/src/platform/AT32/io_at32.c b/src/platform/AT32/io_at32.c index 231f9e5e47..5905f4055f 100644 --- a/src/platform/AT32/io_at32.c +++ b/src/platform/AT32/io_at32.c @@ -122,16 +122,6 @@ void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) return; } - const rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; - RCC_ClockCmd(rcc, ENABLE); - - gpio_init_type init = { - .gpio_pins = IO_Pin(io), - .gpio_mode = (cfg >> 0) & 0x03, - .gpio_drive_strength = (cfg >> 2) & 0x03, - .gpio_out_type = (cfg >> 4) & 0x01, - .gpio_pull = (cfg >> 5) & 0x03, - }; - gpio_init(IO_GPIO(io), &init); + IOConfigGPIO(io, cfg); gpio_pin_mux_config(IO_GPIO(io), IO_GPIO_PinSource(io), af); } diff --git a/src/platform/AT32/pwm_output_at32bsp.c b/src/platform/AT32/pwm_output_at32bsp.c index e04b39fc82..82c1332cb6 100644 --- a/src/platform/AT32/pwm_output_at32bsp.c +++ b/src/platform/AT32/pwm_output_at32bsp.c @@ -81,15 +81,15 @@ static FAST_DATA_ZERO_INIT motorDevice_t *pwmMotorDevice; static void pwmWriteStandard(uint8_t index, float value) { /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */ - *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset); + *pwmMotors[index].channel.ccr = lrintf((value * pwmMotors[index].pulseScale) + pwmMotors[index].pulseOffset); } static void pwmShutdownPulsesForAllMotors(void) { 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; + if (pwmMotors[index].channel.ccr) { + *pwmMotors[index].channel.ccr = 0; } } } @@ -108,12 +108,12 @@ static void pwmCompleteMotorUpdate(void) } for (int index = 0; index < pwmMotorCount; index++) { - if (motors[index].forceOverflow) { - timerForceOverflow(motors[index].channel.tim); + if (pwmMotors[index].forceOverflow) { + timerForceOverflow(pwmMotors[index].channel.tim); } // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. - *motors[index].channel.ccr = 0; + *pwmMotors[index].channel.ccr = 0; } } @@ -145,7 +145,7 @@ static motorVTable_t motorPwmVTable = { bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { - memset(motors, 0, sizeof(motors)); + memset(pwmMotors, 0, sizeof(pwmMotors)); if (!device) { return false; @@ -198,10 +198,10 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, return false; } - motors[motorIndex].io = IOGetByTag(tag); - IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); + pwmMotors[motorIndex].io = IOGetByTag(tag); + IOInit(pwmMotors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); - IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); + IOConfigGPIOAF(pwmMotors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); /* standard PWM outputs */ // margin of safety is 4 periods when unsynced @@ -218,27 +218,27 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, TODO: this can be moved back to periodMin and periodLen once mixer outputs a 0..1 float value. */ - motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; - motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); + pwmMotors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; + pwmMotors[motorIndex].pulseOffset = (sMin * hz) - (pwmMotors[motorIndex].pulseScale * 1000); - pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); + pwmOutConfig(&pwmMotors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); bool timerAlreadyUsed = false; for (int i = 0; i < motorIndex; i++) { - if (motors[i].channel.tim == motors[motorIndex].channel.tim) { + if (pwmMotors[i].channel.tim == pwmMotors[motorIndex].channel.tim) { timerAlreadyUsed = true; break; } } - motors[motorIndex].forceOverflow = !timerAlreadyUsed; - motors[motorIndex].enabled = true; + pwmMotors[motorIndex].forceOverflow = !timerAlreadyUsed; + pwmMotors[motorIndex].enabled = true; } return true; } pwmOutputPort_t *pwmGetMotors(void) { - return motors; + return pwmMotors; } #ifdef USE_SERVOS diff --git a/src/platform/SIMULATOR/sitl.c b/src/platform/SIMULATOR/sitl.c index aa58bc45a8..862d2835fd 100644 --- a/src/platform/SIMULATOR/sitl.c +++ b/src/platform/SIMULATOR/sitl.c @@ -546,7 +546,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig) pwmOutputPort_t *pwmGetMotors(void) { - return motors; + return pwmMotors; } static float pwmConvertFromExternal(uint16_t externalValue) @@ -652,7 +652,7 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, idlePulse = _idlePulse; for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { - motors[motorIndex].enabled = true; + pwmMotors[motorIndex].enabled = true; } return true; diff --git a/src/platform/STM32/dshot_bitbang.c b/src/platform/STM32/dshot_bitbang.c index 3cf7db42ce..38f895b1ba 100644 --- a/src/platform/STM32/dshot_bitbang.c +++ b/src/platform/STM32/dshot_bitbang.c @@ -716,6 +716,7 @@ static const motorVTable_t bbVTable = { .shutdown = bbShutdown, .isMotorIdle = bbDshotIsMotorIdle, .requestTelemetry = bbDshotRequestTelemetry, + .getMotorIO = bbGetMotorIO, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) diff --git a/src/platform/STM32/pwm_output_hw.c b/src/platform/STM32/pwm_output_hw.c index aed9bda188..ae8c6a21af 100644 --- a/src/platform/STM32/pwm_output_hw.c +++ b/src/platform/STM32/pwm_output_hw.c @@ -111,15 +111,15 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, static void pwmWriteStandard(uint8_t index, float value) { /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */ - *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset); + *pwmMotors[index].channel.ccr = lrintf((value * pwmMotors[index].pulseScale) + pwmMotors[index].pulseOffset); } static void pwmShutdownPulsesForAllMotors(void) { 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; + if (pwmMotors[index].channel.ccr) { + *pwmMotors[index].channel.ccr = 0; } } } @@ -136,12 +136,12 @@ static void pwmCompleteMotorUpdate(void) } for (int index = 0; pwmMotorCount; index++) { - if (motors[index].forceOverflow) { - timerForceOverflow(motors[index].channel.tim); + if (pwmMotors[index].forceOverflow) { + timerForceOverflow(pwmMotors[index].channel.tim); } // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. - *motors[index].channel.ccr = 0; + *pwmMotors[index].channel.ccr = 0; } } @@ -173,7 +173,7 @@ static const motorVTable_t motorPwmVTable = { bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { - memset(motors, 0, sizeof(motors)); + memset(pwmMotors, 0, sizeof(pwmMotors)); pwmMotorCount = device->count; device->vTable = &motorPwmVTable; @@ -221,10 +221,10 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, return false; } - motors[motorIndex].io = IOGetByTag(tag); - IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); + pwmMotors[motorIndex].io = IOGetByTag(tag); + IOInit(pwmMotors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); - IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); + IOConfigGPIOAF(pwmMotors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction); /* standard PWM outputs */ // margin of safety is 4 periods when unsynced @@ -241,20 +241,20 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, TODO: this can be moved back to periodMin and periodLen once mixer outputs a 0..1 float value. */ - motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; - motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); + pwmMotors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; + pwmMotors[motorIndex].pulseOffset = (sMin * hz) - (pwmMotors[motorIndex].pulseScale * 1000); - pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); + pwmOutConfig(&pwmMotors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); bool timerAlreadyUsed = false; for (int i = 0; i < motorIndex; i++) { - if (motors[i].channel.tim == motors[motorIndex].channel.tim) { + if (pwmMotors[i].channel.tim == pwmMotors[motorIndex].channel.tim) { timerAlreadyUsed = true; break; } } - motors[motorIndex].forceOverflow = !timerAlreadyUsed; - motors[motorIndex].enabled = true; + pwmMotors[motorIndex].forceOverflow = !timerAlreadyUsed; + pwmMotors[motorIndex].enabled = true; } return true; @@ -262,7 +262,7 @@ bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, pwmOutputPort_t *pwmGetMotors(void) { - return motors; + return pwmMotors; } #ifdef USE_SERVOS diff --git a/src/platform/common/stm32/dshot_bitbang_impl.h b/src/platform/common/stm32/dshot_bitbang_impl.h index 4e062e86c2..76b6b06196 100644 --- a/src/platform/common/stm32/dshot_bitbang_impl.h +++ b/src/platform/common/stm32/dshot_bitbang_impl.h @@ -285,3 +285,4 @@ int bbDMA_Count(bbPort_t *bbPort); void bbDshotRequestTelemetry(unsigned motorIndex); bool bbDshotIsMotorIdle(unsigned motorIndex); +IO_t bbGetMotorIO(unsigned index); diff --git a/src/platform/common/stm32/dshot_bitbang_shared.c b/src/platform/common/stm32/dshot_bitbang_shared.c index c5152d59f3..3339df045b 100644 --- a/src/platform/common/stm32/dshot_bitbang_shared.c +++ b/src/platform/common/stm32/dshot_bitbang_shared.c @@ -54,6 +54,14 @@ bool bbDshotIsMotorIdle(unsigned motorIndex) return bbmotor->protocolControl.value == 0; } +IO_t bbGetMotorIO(unsigned index) +{ + if (index >= dshotMotorCount) { + return IO_NONE; + } + return bbMotors[index].io; +} + #ifdef USE_DSHOT_BITBANG bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig) { diff --git a/src/platform/common/stm32/dshot_dpwm.c b/src/platform/common/stm32/dshot_dpwm.c index 29c099396e..2e2186a1ac 100644 --- a/src/platform/common/stm32/dshot_dpwm.c +++ b/src/platform/common/stm32/dshot_dpwm.c @@ -123,7 +123,7 @@ static bool dshotPwmEnableMotors(void) static bool dshotPwmIsMotorEnabled(unsigned index) { - return motors[index].enabled; + return pwmMotors[index].enabled; } static IO_t pwmDshotGetMotorIO(unsigned index) @@ -131,7 +131,7 @@ static IO_t pwmDshotGetMotorIO(unsigned index) if (index >= dshotMotorCount) { return IO_NONE; } - return motors[index].io; + return pwmMotors[index].io; } static FAST_CODE void dshotWriteInt(uint8_t index, uint16_t value) @@ -190,15 +190,15 @@ bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); if (timerHardware != NULL) { - motors[motorIndex].io = IOGetByTag(tag); - IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); + pwmMotors[motorIndex].io = IOGetByTag(tag); + IOInit(pwmMotors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex)); if (pwmDshotMotorHardwareConfig(timerHardware, motorIndex, reorderedMotorIndex, motorConfig->motorProtocol, motorConfig->motorInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output)) { - motors[motorIndex].enabled = true; + pwmMotors[motorIndex].enabled = true; continue; }