diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c index 124554928e..22f2415a11 100644 --- a/src/main/drivers/motor.c +++ b/src/main/drivers/motor.c @@ -362,6 +362,14 @@ timeMs_t motorGetMotorEnableTimeMs(void) } #endif +IO_t motorGetIo(unsigned index) +{ + if (index >= motorDevice.count) { + return IO_NONE; + } + return motorDevice.vTable->getMotorIO ? motorDevice.vTable->getMotorIO(index) : IO_NONE; +} + /* functions below for empty methods and no active motors */ void motorPostInitNull(void) { @@ -433,6 +441,7 @@ static const motorVTable_t motorNullVTable = { .shutdown = motorShutdownNull, .requestTelemetry = NULL, .isMotorIdle = NULL, + .getMotorIO = NULL, }; void motorNullDevInit(motorDevice_t *device) diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h index d43f430446..c4a39877f6 100644 --- a/src/main/drivers/motor.h +++ b/src/main/drivers/motor.h @@ -54,5 +54,7 @@ float motorEstimateMaxRpm(void); bool motorIsEnabled(void); bool motorIsMotorEnabled(unsigned index); bool motorIsMotorIdle(unsigned index); +IO_t motorGetIo(unsigned index); + timeMs_t motorGetMotorEnableTimeMs(void); void motorShutdown(void); // Replaces stopPwmAllMotors diff --git a/src/main/drivers/motor_types.h b/src/main/drivers/motor_types.h index c70fcee0b0..f3a88eb7c1 100644 --- a/src/main/drivers/motor_types.h +++ b/src/main/drivers/motor_types.h @@ -22,6 +22,7 @@ #pragma once #include "common/time.h" +#include "drivers/io_types.h" #define ALL_MOTORS 255 #define MOTOR_OUTPUT_LIMIT_PERCENT_MIN 1 @@ -64,6 +65,7 @@ typedef struct motorVTable_s { void (*updateComplete)(void); void (*shutdown)(void); bool (*isMotorIdle)(unsigned index); + IO_t (*getMotorIO)(unsigned index); // Digital commands void (*requestTelemetry)(unsigned index); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 813b97e856..e8ae90887b 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -48,4 +48,23 @@ void analogInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, fl } } +IO_t pwmGetMotorIO(unsigned index) +{ + if (index >= pwmMotorCount) { + return IO_NONE; + } + return motors[index].io; +} + +bool pwmIsMotorEnabled(unsigned index) +{ + return motors[index].enabled; +} + +bool pwmEnableMotors(void) +{ + /* check motors can be enabled */ + return pwmMotorCount > 0; +} + #endif diff --git a/src/main/drivers/pwm_output_impl.h b/src/main/drivers/pwm_output_impl.h new file mode 100644 index 0000000000..403bf4782e --- /dev/null +++ b/src/main/drivers/pwm_output_impl.h @@ -0,0 +1,28 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/io_types.h" + +IO_t pwmGetMotorIO(unsigned index); +bool pwmIsMotorEnabled(unsigned index); +bool pwmEnableMotors(void); diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 552fc5d58c..644e37cbb0 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -36,7 +36,7 @@ #include "drivers/time.h" #include "drivers/timer.h" #include "drivers/light_led.h" -#include "drivers/pwm_output.h" +#include "drivers/motor.h" #include "flight/mixer.h" #include "io/beeper.h" @@ -143,23 +143,23 @@ inline void setEscOutput(uint8_t selEsc) uint8_t esc4wayInit(void) { - // StopPwmAllMotors(); - // XXX Review effect of motor refactor - //pwmDisableMotors(); - escCount = 0; + motorShutdown(); + uint8_t escIndex = 0; + memset(&escHardware, 0, sizeof(escHardware)); - pwmOutputPort_t *pwmMotors = pwmGetMotors(); for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - if (pwmMotors[i].enabled) { - if (pwmMotors[i].io != IO_NONE) { - escHardware[escCount].io = pwmMotors[i].io; - setEscInput(escCount); - setEscHi(escCount); - escCount++; + if (motorIsMotorEnabled(i)) { + const IO_t io = motorGetIo(i); + if (io != IO_NONE) { + escHardware[escIndex].io = io; + setEscInput(escIndex); + setEscHi(escIndex); + escIndex++; } } } motorDisable(); + escCount = escIndex; return escCount; } diff --git a/src/platform/APM32/dshot_bitbang.c b/src/platform/APM32/dshot_bitbang.c index 5776305d62..33b114fbc0 100644 --- a/src/platform/APM32/dshot_bitbang.c +++ b/src/platform/APM32/dshot_bitbang.c @@ -669,6 +669,14 @@ 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, @@ -685,6 +693,7 @@ static motorVTable_t bbVTable = { .shutdown = bbShutdown, .isMotorIdle = bbDshotIsMotorIdle, .requestTelemetry = bbDshotRequestTelemetry, + .getMotorIO = bbGetMotorIO, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) diff --git a/src/platform/APM32/pwm_output_apm32.c b/src/platform/APM32/pwm_output_apm32.c index d3de5ba53a..706591e0a0 100644 --- a/src/platform/APM32/pwm_output_apm32.c +++ b/src/platform/APM32/pwm_output_apm32.c @@ -31,6 +31,7 @@ #include "drivers/io.h" #include "drivers/motor.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_output_impl.h" #include "drivers/time.h" #include "drivers/timer.h" @@ -108,16 +109,6 @@ static void pwmDisableMotors(void) } static motorVTable_t motorPwmVTable; -static bool pwmEnableMotors(void) -{ - /* check motors can be enabled */ - return pwmMotorCount > 0; -} - -static bool pwmIsMotorEnabled(unsigned index) -{ - return motors[index].enabled; -} static void pwmCompleteMotorUpdate(void) { @@ -158,6 +149,7 @@ static motorVTable_t motorPwmVTable = { .updateComplete = pwmCompleteMotorUpdate, .requestTelemetry = NULL, .isMotorIdle = NULL, + .getMotorIO = pwmGetMotorIO, }; bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) diff --git a/src/platform/AT32/pwm_output_at32bsp.c b/src/platform/AT32/pwm_output_at32bsp.c index 8ebc593340..e04b39fc82 100644 --- a/src/platform/AT32/pwm_output_at32bsp.c +++ b/src/platform/AT32/pwm_output_at32bsp.c @@ -31,6 +31,7 @@ #include "drivers/io.h" #include "drivers/motor_impl.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_output_impl.h" #include "drivers/time.h" #include "drivers/timer.h" @@ -98,18 +99,6 @@ static void pwmDisableMotors(void) pwmShutdownPulsesForAllMotors(); } -static motorVTable_t motorPwmVTable; -static bool pwmEnableMotors(void) -{ - /* check motors can be enabled */ - return (pwmMotorDevice->vTable); -} - -static bool pwmIsMotorEnabled(unsigned index) -{ - return motors[index].enabled; -} - static bool useContinuousUpdate = true; static void pwmCompleteMotorUpdate(void) @@ -151,6 +140,7 @@ static motorVTable_t motorPwmVTable = { .updateComplete = pwmCompleteMotorUpdate, .requestTelemetry = NULL, .isMotorIdle = NULL, + .getMotorIO = pwmGetMotorIO, }; bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) diff --git a/src/platform/SIMULATOR/sitl.c b/src/platform/SIMULATOR/sitl.c index bd52fe2564..aa58bc45a8 100644 --- a/src/platform/SIMULATOR/sitl.c +++ b/src/platform/SIMULATOR/sitl.c @@ -41,6 +41,7 @@ #include "drivers/system.h" #include "drivers/time.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_output_impl.h" #include "drivers/light_led.h" #include "drivers/timer.h" @@ -543,8 +544,6 @@ void servoDevInit(const servoDevConfig_t *servoConfig) } } -static motorDevice_t pwmMotorDevice; // Forward - pwmOutputPort_t *pwmGetMotors(void) { return motors; @@ -562,14 +561,7 @@ static uint16_t pwmConvertToExternal(float motorValue) static void pwmDisableMotors(void) { - pwmMotorDevice.enabled = false; -} - -static bool pwmEnableMotors(void) -{ - pwmMotorDevice.enabled = true; - - return true; + // NOOP } static void pwmWriteMotor(uint8_t index, float value) @@ -594,12 +586,7 @@ static void pwmWriteMotorInt(uint8_t index, uint16_t value) static void pwmShutdownPulsesForAllMotors(void) { - pwmMotorDevice.enabled = false; -} - -static bool pwmIsMotorEnabled(unsigned index) -{ - return motors[index].enabled; + // NOOP } static void pwmCompleteMotorUpdate(void) @@ -647,7 +634,7 @@ static const motorVTable_t vTable = { .shutdown = pwmShutdownPulsesForAllMotors, .requestTelemetry = NULL, .isMotorIdle = NULL, - + .getMotorIO = NULL, }; bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t _idlePulse) diff --git a/src/platform/STM32/pwm_output_hw.c b/src/platform/STM32/pwm_output_hw.c index 99a2a3ce69..aed9bda188 100644 --- a/src/platform/STM32/pwm_output_hw.c +++ b/src/platform/STM32/pwm_output_hw.c @@ -29,6 +29,7 @@ #include "drivers/io.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_output_impl.h" #include "drivers/time.h" #include "drivers/timer.h" @@ -128,17 +129,6 @@ static void pwmDisableMotors(void) pwmShutdownPulsesForAllMotors(); } -static bool pwmEnableMotors(void) -{ - /* check motors can be enabled */ - return pwmMotorCount > 0; -} - -static bool pwmIsMotorEnabled(unsigned index) -{ - return motors[index].enabled; -} - static void pwmCompleteMotorUpdate(void) { if (useContinuousUpdate) { @@ -178,6 +168,7 @@ static const motorVTable_t motorPwmVTable = { .updateComplete = pwmCompleteMotorUpdate, .requestTelemetry = NULL, .isMotorIdle = NULL, + .getMotorIO = pwmGetMotorIO, }; bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) diff --git a/src/platform/common/stm32/dshot_dpwm.c b/src/platform/common/stm32/dshot_dpwm.c index cee7aa6003..29c099396e 100644 --- a/src/platform/common/stm32/dshot_dpwm.c +++ b/src/platform/common/stm32/dshot_dpwm.c @@ -126,6 +126,14 @@ static bool dshotPwmIsMotorEnabled(unsigned index) return motors[index].enabled; } +static IO_t pwmDshotGetMotorIO(unsigned index) +{ + if (index >= dshotMotorCount) { + return IO_NONE; + } + return motors[index].io; +} + static FAST_CODE void dshotWriteInt(uint8_t index, uint16_t value) { pwmWriteDshotInt(index, value); @@ -150,6 +158,7 @@ static const motorVTable_t dshotPwmVTable = { .shutdown = dshotPwmShutdown, .requestTelemetry = pwmDshotRequestTelemetry, .isMotorIdle = pwmDshotIsMotorIdle, + .getMotorIO = pwmDshotGetMotorIO, }; bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig)