diff --git a/mk/source.mk b/mk/source.mk index 33ccb4c2c0..7d5136ac15 100644 --- a/mk/source.mk +++ b/mk/source.mk @@ -108,6 +108,7 @@ COMMON_SRC = \ drivers/motor.c \ drivers/pinio.c \ drivers/pin_pull_up_down.c \ + drivers/pwm_output.c \ drivers/resource.c \ drivers/serial.c \ drivers/serial_impl.c \ diff --git a/src/main/drivers/dshot.c b/src/main/drivers/dshot.c index 623bf04290..a2e6953378 100644 --- a/src/main/drivers/dshot.c +++ b/src/main/drivers/dshot.c @@ -39,7 +39,7 @@ #include "config/feature.h" -#include "drivers/motor.h" +#include "drivers/motor_types.h" #include "drivers/timer.h" #include "drivers/dshot_command.h" diff --git a/src/main/drivers/dshot.h b/src/main/drivers/dshot.h index 3896cfbd6f..6208f1a989 100644 --- a/src/main/drivers/dshot.h +++ b/src/main/drivers/dshot.h @@ -138,6 +138,8 @@ void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool pac void initDshotTelemetry(const timeUs_t looptimeUs); void updateDshotTelemetry(void); +bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig); + uint16_t getDshotErpm(uint8_t motorIndex); float getDshotRpm(uint8_t motorIndex); float getDshotRpmAverage(void); diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c index a9c1a6a427..c76d84cab0 100644 --- a/src/main/drivers/motor.c +++ b/src/main/drivers/motor.c @@ -132,7 +132,7 @@ void motorRequestTelemetry(unsigned index) return; } - if (motorDevice.vTable && motorDevice.vTable->requestTelemetry) { + if (motorDevice.vTable->requestTelemetry) { motorDevice.vTable->requestTelemetry(index); } } @@ -147,24 +147,6 @@ const motorVTable_t *motorGetVTable(void) return motorDevice.vTable; } -// This is not motor generic anymore; should be moved to analog pwm module -static void analogInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow) -{ - if (featureIsEnabled(FEATURE_3D)) { - float outputLimitOffset = (flight3DConfig()->limit3d_high - flight3DConfig()->limit3d_low) * (1 - outputLimit) / 2; - *disarm = flight3DConfig()->neutral3d; - *outputLow = flight3DConfig()->limit3d_low + outputLimitOffset; - *outputHigh = flight3DConfig()->limit3d_high - outputLimitOffset; - *deadbandMotor3dHigh = flight3DConfig()->deadband3d_high; - *deadbandMotor3dLow = flight3DConfig()->deadband3d_low; - } else { - *disarm = motorConfig->mincommand; - const float minThrottle = motorConfig->mincommand + motorConfig->motorIdle * 0.1f; - *outputLow = minThrottle; - *outputHigh = motorConfig->maxthrottle - ((motorConfig->maxthrottle - minThrottle) * (1 - outputLimit)); - } -} - bool checkMotorProtocolEnabled(const motorDevConfig_t *motorDevConfig, bool *isProtocolDshot) { bool enabled = false; @@ -199,6 +181,29 @@ bool checkMotorProtocolEnabled(const motorDevConfig_t *motorDevConfig, bool *isP return enabled; } +motorProtocolFamily_e motorGetProtocolFamily(void) +{ + switch (motorConfig()->dev.motorProtocol) { +#ifdef USE_PWMOUTPUT + case MOTOR_PROTOCOL_PWM : + case MOTOR_PROTOCOL_ONESHOT125: + case MOTOR_PROTOCOL_ONESHOT42: + case MOTOR_PROTOCOL_MULTISHOT: + case MOTOR_PROTOCOL_BRUSHED: + return MOTOR_PROTOCOL_FAMILY_PWM; +#endif +#ifdef USE_DSHOT + case MOTOR_PROTOCOL_DSHOT150: + case MOTOR_PROTOCOL_DSHOT300: + case MOTOR_PROTOCOL_DSHOT600: + case MOTOR_PROTOCOL_PROSHOT1000: + return MOTOR_PROTOCOL_FAMILY_DSHOT; +#endif + default: + return MOTOR_PROTOCOL_FAMILY_UNKNOWN; + } +} + static void checkMotorProtocol(const motorDevConfig_t *motorDevConfig) { motorProtocolEnabled = checkMotorProtocolEnabled(motorDevConfig, &motorProtocolDshot); @@ -210,14 +215,21 @@ void motorInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, flo checkMotorProtocol(&motorConfig->dev); if (isMotorProtocolEnabled()) { - if (!isMotorProtocolDshot()) { + switch (motorGetProtocolFamily()) { +#ifdef USE_PWM_OUTPUT + case MOTOR_PROTOCOL_FAMILY_PWM: analogInitEndpoints(motorConfig, outputLimit, outputLow, outputHigh, disarm, deadbandMotor3dHigh, deadbandMotor3dLow); - } -#ifdef USE_DSHOT - else { - dshotInitEndpoints(motorConfig, outputLimit, outputLow, outputHigh, disarm, deadbandMotor3dHigh, deadbandMotor3dLow); - } + break; #endif +#ifdef USE_DSHOT + case MOTOR_PROTOCOL_FAMILY_DSHOT: + dshotInitEndpoints(motorConfig, outputLimit, outputLow, outputHigh, disarm, deadbandMotor3dHigh, deadbandMotor3dLow); + break; +#endif + default: + // TODO: perhaps a failure mode here? + break; + } } } @@ -233,7 +245,7 @@ uint16_t motorConvertToExternal(float motorValue) void motorPostInit(void) { - if (motorDevice.vTable && motorDevice.vTable->postInit) { + if (motorDevice.vTable->postInit) { motorDevice.vTable->postInit(); } } @@ -311,7 +323,7 @@ void motorDisable(void) void motorEnable(void) { - if (motorDevice.initialized && motorDevice.vTable && motorDevice.vTable->enable()) { + if (motorDevice.initialized && motorDevice.vTable->enable()) { motorDevice.enabled = true; motorDevice.motorEnableTimeMs = millis(); } @@ -349,20 +361,6 @@ timeMs_t motorGetMotorEnableTimeMs(void) } #endif -//TODO: Remove this function -#ifdef USE_DSHOT_BITBANG -bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig) -{ -#if defined(STM32F4) || defined(APM32F4) - return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON || - (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->useDshotTelemetry && motorDevConfig->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000); -#else - return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON || - (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000); -#endif -} -#endif - /* functions below for empty methods and no active motors */ void motorPostInitNull(void) { diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h index 32cadfea90..80ea50a97a 100644 --- a/src/main/drivers/motor.h +++ b/src/main/drivers/motor.h @@ -46,6 +46,9 @@ void motorDevInit(unsigned motorCount); unsigned motorDeviceCount(void); const motorVTable_t *motorGetVTable(void); bool checkMotorProtocolEnabled(const motorDevConfig_t *motorConfig, bool *protocolIsDshot); + +motorProtocolFamily_e motorGetProtocolFamily(void); + bool isMotorProtocolDshot(void); bool isMotorProtocolBidirDshot(void); bool isMotorProtocolEnabled(void); @@ -58,9 +61,3 @@ bool motorIsMotorEnabled(unsigned index); bool motorIsMotorIdle(unsigned index); timeMs_t motorGetMotorEnableTimeMs(void); void motorShutdown(void); // Replaces stopPwmAllMotors - -#ifdef USE_DSHOT_BITBANG -struct motorDevConfig_s; -typedef struct motorDevConfig_s motorDevConfig_t; -bool isDshotBitbangActive(const motorDevConfig_t *motorConfig); -#endif diff --git a/src/main/drivers/motor_types.h b/src/main/drivers/motor_types.h index 5496ab2af8..c70fcee0b0 100644 --- a/src/main/drivers/motor_types.h +++ b/src/main/drivers/motor_types.h @@ -27,6 +27,12 @@ #define MOTOR_OUTPUT_LIMIT_PERCENT_MIN 1 #define MOTOR_OUTPUT_LIMIT_PERCENT_MAX 100 +typedef enum { + MOTOR_PROTOCOL_FAMILY_UNKNOWN = 0, + MOTOR_PROTOCOL_FAMILY_PWM, + MOTOR_PROTOCOL_FAMILY_DSHOT, +} motorProtocolFamily_e; + typedef enum { MOTOR_PROTOCOL_PWM = 0, MOTOR_PROTOCOL_ONESHOT125, diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c new file mode 100644 index 0000000000..16c89dc98a --- /dev/null +++ b/src/main/drivers/pwm_output.c @@ -0,0 +1,47 @@ +/* + * 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 . + */ + +#include "platform.h" + +#include "pg/motor.h" +#include "fc/rc_controls.h" // for flight3DConfig_t +#include "config/feature.h" + +#if defined(USE_PWM_OUTPUT) && defined(USE_MOTOR) + +void analogInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow) +{ + if (featureIsEnabled(FEATURE_3D)) { + float outputLimitOffset = (flight3DConfig()->limit3d_high - flight3DConfig()->limit3d_low) * (1 - outputLimit) / 2; + *disarm = flight3DConfig()->neutral3d; + *outputLow = flight3DConfig()->limit3d_low + outputLimitOffset; + *outputHigh = flight3DConfig()->limit3d_high - outputLimitOffset; + *deadbandMotor3dHigh = flight3DConfig()->deadband3d_high; + *deadbandMotor3dLow = flight3DConfig()->deadband3d_low; + } else { + *disarm = motorConfig->mincommand; + const float minThrottle = motorConfig->mincommand + motorConfig->motorIdle * 0.1f; + *outputLow = minThrottle; + *outputHigh = motorConfig->maxthrottle - ((motorConfig->maxthrottle - minThrottle) * (1 - outputLimit)); + } +} + +#endif diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 7a98738e10..4b09c8f571 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -71,3 +71,4 @@ void pwmWriteServo(uint8_t index, float value); pwmOutputPort_t *pwmGetMotors(void); bool pwmIsSynced(void); +void analogInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow); diff --git a/src/platform/STM32/mk/STM32F4.mk b/src/platform/STM32/mk/STM32F4.mk index de52550c13..73bc5c537a 100644 --- a/src/platform/STM32/mk/STM32F4.mk +++ b/src/platform/STM32/mk/STM32F4.mk @@ -188,7 +188,6 @@ MCU_COMMON_SRC = \ STM32/io_stm32.c \ STM32/light_ws2811strip_stdperiph.c \ STM32/persistent.c \ - STM32/pwm_output.c \ STM32/rcc_stm32.c \ STM32/sdio_f4xx.c \ STM32/serial_uart_stdperiph.c \ diff --git a/src/platform/STM32/mk/STM32F7.mk b/src/platform/STM32/mk/STM32F7.mk index ab362b4683..c1de3f0d7b 100644 --- a/src/platform/STM32/mk/STM32F7.mk +++ b/src/platform/STM32/mk/STM32F7.mk @@ -157,7 +157,6 @@ MCU_COMMON_SRC = \ STM32/io_stm32.c \ STM32/light_ws2811strip_hal.c \ STM32/persistent.c \ - STM32/pwm_output.c \ STM32/pwm_output_dshot_hal.c \ STM32/rcc_stm32.c \ STM32/sdio_f7xx.c \ diff --git a/src/platform/STM32/mk/STM32G4.mk b/src/platform/STM32/mk/STM32G4.mk index f7d2409b3b..b1907fb509 100644 --- a/src/platform/STM32/mk/STM32G4.mk +++ b/src/platform/STM32/mk/STM32G4.mk @@ -135,7 +135,6 @@ MCU_COMMON_SRC = \ STM32/memprot_hal.c \ STM32/memprot_stm32g4xx.c \ STM32/persistent.c \ - STM32/pwm_output.c \ STM32/pwm_output_dshot_hal.c \ STM32/rcc_stm32.c \ STM32/serial_uart_hal.c \ diff --git a/src/platform/STM32/mk/STM32H5.mk b/src/platform/STM32/mk/STM32H5.mk index b85922afff..705f4249b9 100644 --- a/src/platform/STM32/mk/STM32H5.mk +++ b/src/platform/STM32/mk/STM32H5.mk @@ -163,7 +163,6 @@ MCU_COMMON_SRC = \ STM32/io_stm32.c \ STM32/light_ws2811strip_hal.c \ STM32/persistent.c \ - STM32/pwm_output.c \ STM32/pwm_output_dshot_hal.c \ STM32/rcc_stm32.c \ STM32/serial_uart_hal.c \ diff --git a/src/platform/STM32/mk/STM32H7.mk b/src/platform/STM32/mk/STM32H7.mk index 9b2e076fd1..fe07c65954 100644 --- a/src/platform/STM32/mk/STM32H7.mk +++ b/src/platform/STM32/mk/STM32H7.mk @@ -284,7 +284,6 @@ MCU_COMMON_SRC = \ STM32/memprot_hal.c \ STM32/memprot_stm32h7xx.c \ STM32/persistent.c \ - STM32/pwm_output.c \ STM32/pwm_output_dshot_hal.c \ STM32/rcc_stm32.c \ STM32/sdio_h7xx.c \ diff --git a/src/platform/STM32/mk/STM32_COMMON.mk b/src/platform/STM32/mk/STM32_COMMON.mk index 4f5201794a..bc3b2c7e6b 100644 --- a/src/platform/STM32/mk/STM32_COMMON.mk +++ b/src/platform/STM32/mk/STM32_COMMON.mk @@ -9,10 +9,10 @@ MCU_COMMON_SRC += \ common/stm32/io_impl.c \ common/stm32/serial_uart_hw.c \ common/stm32/dshot_dpwm.c \ + STM32/pwm_output_hw.c \ common/stm32/pwm_output_dshot_shared.c \ common/stm32/dshot_bitbang_shared.c - SIZE_OPTIMISED_SRC += \ common/stm32/config_flash.c \ common/stm32/bus_spi_pinconfig.c @@ -21,6 +21,7 @@ SPEED_OPTIMISED_SRC += \ common/stm32/system.c \ common/stm32/bus_spi_hw.c \ common/stm32/pwm_output_dshot_shared.c \ + STM32/pwm_output_hw.c \ common/stm32/dshot_bitbang_shared.c \ common/stm32/io_impl.c diff --git a/src/platform/STM32/pwm_output.c b/src/platform/STM32/pwm_output_hw.c similarity index 100% rename from src/platform/STM32/pwm_output.c rename to src/platform/STM32/pwm_output_hw.c diff --git a/src/platform/common/stm32/bus_spi_hw.c b/src/platform/common/stm32/bus_spi_hw.c index b0befd8c34..653690ecfe 100644 --- a/src/platform/common/stm32/bus_spi_hw.c +++ b/src/platform/common/stm32/bus_spi_hw.c @@ -32,7 +32,7 @@ #include "drivers/bus_spi.h" #include "drivers/bus_spi_impl.h" #include "drivers/dma_reqmap.h" -#include "drivers/motor.h" +#include "drivers/dshot.h" #include "drivers/nvic.h" #include "pg/bus_spi.h" diff --git a/src/platform/common/stm32/dshot_bitbang_impl.h b/src/platform/common/stm32/dshot_bitbang_impl.h index ab93c8a5bd..32c1aa40c7 100644 --- a/src/platform/common/stm32/dshot_bitbang_impl.h +++ b/src/platform/common/stm32/dshot_bitbang_impl.h @@ -27,6 +27,7 @@ #include "drivers/timer.h" #include "drivers/motor_types.h" #include "drivers/dshot.h" +#include "pg/motor.h" #define USE_DMA_REGISTER_CACHE diff --git a/src/platform/common/stm32/dshot_bitbang_shared.c b/src/platform/common/stm32/dshot_bitbang_shared.c index 15f9d0572b..f2d88b24a5 100644 --- a/src/platform/common/stm32/dshot_bitbang_shared.c +++ b/src/platform/common/stm32/dshot_bitbang_shared.c @@ -43,3 +43,16 @@ bool bbDshotIsMotorIdle(unsigned motorIndex) bbMotor_t *const bbmotor = &bbMotors[motorIndex]; return bbmotor->protocolControl.value == 0; } + +#ifdef USE_DSHOT_BITBANG +bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig) +{ +#if defined(STM32F4) || defined(APM32F4) + return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON || + (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->useDshotTelemetry && motorDevConfig->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000); +#else + return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON || + (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000); +#endif +} +#endif