diff --git a/src/main/drivers/dshot_dpwm.c b/src/main/drivers/dshot_dpwm.c index 70f233f0e4..9c43128688 100644 --- a/src/main/drivers/dshot_dpwm.c +++ b/src/main/drivers/dshot_dpwm.c @@ -179,22 +179,26 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl const ioTag_t tag = motorConfig->ioTags[motorIndex]; const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); - if (timerHardware == NULL) { - /* not enough motors initialised for the mixer or a break in the motors */ - dshotPwmDevice.vTable.write = motorWriteNull; - dshotPwmDevice.vTable.updateComplete = motorUpdateCompleteNull; - /* TODO: block arming and add reason system cannot arm */ - return NULL; + if (timerHardware != NULL) { + motors[motorIndex].io = IOGetByTag(tag); + IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); + + if (pwmDshotMotorHardwareConfig(timerHardware, + motorIndex, + motorConfig->motorPwmProtocol, + motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output)) { + motors[motorIndex].enabled = true; + + continue; + } } - motors[motorIndex].io = IOGetByTag(tag); - IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); + /* not enough motors initialised for the mixer or a break in the motors */ + dshotPwmDevice.vTable.write = motorWriteNull; + dshotPwmDevice.vTable.updateComplete = motorUpdateCompleteNull; - pwmDshotMotorHardwareConfig(timerHardware, - motorIndex, - motorConfig->motorPwmProtocol, - motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); - motors[motorIndex].enabled = true; + /* TODO: block arming and add reason system cannot arm */ + return NULL; } return &dshotPwmDevice; diff --git a/src/main/drivers/dshot_dpwm.h b/src/main/drivers/dshot_dpwm.h index 361ab5c786..482157b527 100644 --- a/src/main/drivers/dshot_dpwm.h +++ b/src/main/drivers/dshot_dpwm.h @@ -161,7 +161,7 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index); bool isMotorProtocolDshot(void); void pwmWriteDshotInt(uint8_t index, uint16_t value); -void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); +bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); #ifdef USE_DSHOT_TELEMETRY bool pwmStartDshotMotorUpdate(void); #endif diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index c5eb18d95b..b1153dd5eb 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -205,7 +205,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) } } -void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { #ifdef USE_DSHOT_TELEMETRY #define OCINIT motor->ocInitStruct @@ -247,7 +247,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m #endif if (dmaRef == NULL) { - return; + return false; } motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; @@ -423,6 +423,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m } #endif motor->configured = true; + + return true; } #endif diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index 16129f1d85..4ec4965e56 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -181,7 +181,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) } } -void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { #ifdef USE_DSHOT_TELEMETRY #define OCINIT motor->ocInitStruct @@ -215,7 +215,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m #endif if (dmaRef == NULL) { - return; + return false; } motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; @@ -381,5 +381,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m } #endif motor->configured = true; + + return true; } #endif diff --git a/src/main/drivers/pwm_output_dshot_hal_hal.c b/src/main/drivers/pwm_output_dshot_hal_hal.c index 97bb00efae..541a9135e4 100644 --- a/src/main/drivers/pwm_output_dshot_hal_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal_hal.c @@ -233,7 +233,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) } } -void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) { dmaResource_t *dmaRef = NULL; uint32_t dmaChannel; @@ -266,7 +266,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m #endif if (dmaRef == NULL) { - return; + return false; } motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; @@ -298,7 +298,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m if (result != HAL_OK) { /* Initialization Error */ - return; + return false; } } @@ -334,7 +334,7 @@ P - High - High - if (result != HAL_OK) { /* Configuration Error */ - return; + return false; } // DMA setup @@ -347,7 +347,7 @@ P - High - High - if (!configureTimer) { motor->configured = true; - return; + return false; } } else #endif @@ -418,7 +418,7 @@ P - High - High - if (result != HAL_OK) { /* Initialization Error */ - return; + return false; } dmaIdentifier_e identifier = dmaGetIdentifier(dmaRef); @@ -444,9 +444,11 @@ P - High - High - if (result != HAL_OK) { /* Starting PWM generation Error */ - return; + return false; } motor->configured = true; + + return true; } #endif