From c2768d0409a6dd6487bc88a0952079e3048ba43a Mon Sep 17 00:00:00 2001 From: Jay Blackman Date: Fri, 24 Jan 2025 18:37:20 +1100 Subject: [PATCH] Refactoring motor to simplify implementation on other platforms (#14156) --- mk/source.mk | 2 +- src/main/blackbox/blackbox.c | 4 +- src/main/cli/cli.c | 2 - src/main/cli/settings.c | 6 +- src/main/cms/cms_menu_imu.c | 2 - src/main/cms/cms_menu_quick.c | 2 +- src/main/config/config.c | 31 +- .../drivers/accgyro/accgyro_spi_icm426xx.c | 1 - src/main/drivers/dshot.c | 15 +- src/main/drivers/dshot.h | 22 + src/main/drivers/dshot_bitbang.h | 17 +- src/main/drivers/dshot_command.c | 18 +- src/main/drivers/dshot_command.h | 4 +- src/main/drivers/light_ws2811strip.c | 4 - src/main/drivers/motor.c | 393 ++++++++++-------- src/main/drivers/motor.h | 68 +-- src/main/drivers/motor_impl.h | 31 ++ src/main/drivers/motor_types.h | 78 ++++ src/main/drivers/pwm_output.c | 51 +++ src/main/drivers/pwm_output.h | 16 +- src/main/drivers/serial_escserial.c | 4 - src/main/drivers/serial_escserial.h | 2 - src/main/fc/init.c | 12 +- src/main/flight/mixer.h | 2 +- src/main/flight/pid.c | 1 - src/main/io/serial_4way.c | 2 +- src/main/msp/msp.c | 12 +- src/main/pg/motor.c | 35 +- src/main/pg/motor.h | 25 +- src/main/sensors/esc_sensor.c | 4 +- src/platform/APM32/dshot_bitbang.c | 82 ++-- src/platform/APM32/dshot_bitbang_ddl.c | 2 +- src/platform/APM32/mk/APM32F4.mk | 6 +- src/platform/APM32/pwm_output_apm32.c | 92 ++-- src/platform/APM32/pwm_output_dshot_apm32.c | 18 +- src/platform/AT32/dshot_bitbang.c | 79 ++-- src/platform/AT32/dshot_bitbang_stdperiph.c | 3 +- src/platform/AT32/mk/AT32F4.mk | 6 +- src/platform/AT32/pwm_output_at32bsp.c | 83 ++-- src/platform/AT32/pwm_output_dshot.c | 20 +- src/platform/SIMULATOR/sitl.c | 55 +-- src/platform/STM32/dshot_bitbang.c | 89 ++-- src/platform/STM32/dshot_bitbang_ll.c | 3 +- src/platform/STM32/dshot_bitbang_stdperiph.c | 3 +- src/platform/STM32/mk/STM32F4.mk | 2 - src/platform/STM32/mk/STM32F7.mk | 3 - src/platform/STM32/mk/STM32G4.mk | 2 - src/platform/STM32/mk/STM32H5.mk | 2 - src/platform/STM32/mk/STM32H7.mk | 2 - src/platform/STM32/mk/STM32_COMMON.mk | 9 +- src/platform/STM32/pwm_output_dshot.c | 18 +- src/platform/STM32/pwm_output_dshot_hal.c | 18 +- .../STM32/{pwm_output.c => pwm_output_hw.c} | 81 ++-- src/platform/STM32/target/STM32F411/target.h | 2 +- src/platform/common/stm32/bus_spi_hw.c | 2 +- .../common/stm32}/dshot_bitbang_impl.h | 12 +- .../common/stm32/dshot_bitbang_shared.c | 68 +++ .../common/stm32}/dshot_dpwm.c | 61 ++- .../common/stm32}/dshot_dpwm.h | 15 +- .../common/stm32}/pwm_output_dshot_shared.c | 46 +- .../common/stm32}/pwm_output_dshot_shared.h | 29 +- src/test/unit/pg_unittest.cc | 6 +- 62 files changed, 975 insertions(+), 810 deletions(-) create mode 100644 src/main/drivers/motor_impl.h create mode 100644 src/main/drivers/motor_types.h create mode 100644 src/main/drivers/pwm_output.c rename src/platform/STM32/{pwm_output.c => pwm_output_hw.c} (82%) rename src/{main/drivers => platform/common/stm32}/dshot_bitbang_impl.h (96%) create mode 100644 src/platform/common/stm32/dshot_bitbang_shared.c rename src/{main/drivers => platform/common/stm32}/dshot_dpwm.c (79%) rename src/{main/drivers => platform/common/stm32}/dshot_dpwm.h (91%) rename src/{main/drivers => platform/common/stm32}/pwm_output_dshot_shared.c (89%) rename src/{main/drivers => platform/common/stm32}/pwm_output_dshot_shared.h (69%) diff --git a/mk/source.mk b/mk/source.mk index f455771251..b0606dbd6e 100644 --- a/mk/source.mk +++ b/mk/source.mk @@ -89,7 +89,6 @@ COMMON_SRC = \ cli/settings.c \ config/config.c \ drivers/dshot.c \ - drivers/dshot_dpwm.c \ drivers/dshot_command.c \ drivers/buf_writer.c \ drivers/bus.c \ @@ -110,6 +109,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/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index acebd8c2c2..ba8db32289 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1730,8 +1730,8 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_CAL_ON_FIRST_ARM, "%d", armingConfig()->gyro_cal_on_first_arm); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SERIAL_RX_PROVIDER, "%d", rxConfig()->serialrx_provider); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_USE_UNSYNCED_PWM, "%d", motorConfig()->dev.useUnsyncedPwm); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_PROTOCOL, "%d", motorConfig()->dev.motorPwmProtocol); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_USE_UNSYNCED_PWM, "%d", motorConfig()->dev.useContinuousUpdate); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_PROTOCOL, "%d", motorConfig()->dev.motorProtocol); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_RATE, "%d", motorConfig()->dev.motorPwmRate); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_IDLE, "%d", motorConfig()->motorIdle); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEBUG_MODE, "%d", debugMode); diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 2d5769b454..7fb1c481d3 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -68,8 +68,6 @@ bool cliMode = false; #include "drivers/dma_reqmap.h" #include "drivers/dshot.h" #include "drivers/dshot_command.h" -#include "drivers/dshot_dpwm.h" -#include "drivers/pwm_output_dshot_shared.h" #include "drivers/camera_control_impl.h" #include "drivers/compass/compass.h" #include "drivers/display.h" diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 3db4343db9..d9ba4c1455 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -936,10 +936,10 @@ const clivalue_t valueTable[] = { { "dshot_bitbang_timer", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DSHOT_BITBANGED_TIMER }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useDshotBitbangedTimer) }, #endif #endif // USE_DSHOT - { PARAM_NAME_USE_UNSYNCED_PWM, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useUnsyncedPwm) }, - { PARAM_NAME_MOTOR_PWM_PROTOCOL, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmProtocol) }, + { PARAM_NAME_USE_UNSYNCED_PWM, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useContinuousUpdate) }, + { PARAM_NAME_MOTOR_PWM_PROTOCOL, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorProtocol) }, { PARAM_NAME_MOTOR_PWM_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 32000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmRate) }, - { "motor_pwm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmInversion) }, + { "motor_pwm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorInversion) }, { PARAM_NAME_MOTOR_POLES, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 4, UINT8_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, motorPoleCount) }, { "motor_output_reordering", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = MAX_SUPPORTED_MOTORS, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorOutputReordering)}, diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index aa5929bf6e..d10e386652 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -42,8 +42,6 @@ #include "config/feature.h" #include "config/simplified_tuning.h" -#include "drivers/pwm_output.h" - #include "config/config.h" #include "fc/controlrate_profile.h" #include "fc/core.h" diff --git a/src/main/cms/cms_menu_quick.c b/src/main/cms/cms_menu_quick.c index b8cda64562..0c53c094a2 100644 --- a/src/main/cms/cms_menu_quick.c +++ b/src/main/cms/cms_menu_quick.c @@ -35,7 +35,7 @@ #include "common/printf.h" #include "config/config.h" -#include "drivers/pwm_output.h" +#include "drivers/motor_types.h" #include "fc/controlrate_profile.h" #include "fc/core.h" diff --git a/src/main/config/config.c b/src/main/config/config.c index aedfdb12d1..2c8295ebcc 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -36,6 +36,7 @@ #include "config/config_eeprom.h" #include "config/feature.h" +#include "drivers/dshot.h" #include "drivers/dshot_command.h" #include "drivers/motor.h" #include "drivers/system.h" @@ -92,8 +93,6 @@ #include "config.h" -#include "drivers/dshot.h" - static bool configIsDirty; /* someone indicated that the config is modified and it is not yet saved */ static bool rebootRequired = false; // set if a config change requires a reboot to take effect @@ -277,7 +276,7 @@ static void validateAndFixConfig(void) #endif } - if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { + if (motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_BRUSHED) { featureDisableImmediate(FEATURE_3D); if (motorConfig()->mincommand < 1000) { @@ -285,7 +284,7 @@ static void validateAndFixConfig(void) } } - if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) { + if ((motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_PWM ) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) { motorConfigMutable()->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; } @@ -381,7 +380,7 @@ static void validateAndFixConfig(void) } #if defined(USE_DSHOT_TELEMETRY) && defined(USE_DSHOT_BITBANG) - if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_PROSHOT1000 && motorConfig()->dev.useDshotTelemetry && + if (motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_PROSHOT1000 && motorConfig()->dev.useDshotTelemetry && motorConfig()->dev.useDshotBitbang == DSHOT_BITBANG_ON) { motorConfigMutable()->dev.useDshotBitbang = DSHOT_BITBANG_AUTO; } @@ -450,7 +449,7 @@ static void validateAndFixConfig(void) #if defined(USE_DSHOT) // If using DSHOT protocol disable unsynched PWM as it's meaningless if (configuredMotorProtocolDshot) { - motorConfigMutable()->dev.useUnsyncedPwm = false; + motorConfigMutable()->dev.useContinuousUpdate = false; } #if defined(USE_DSHOT_TELEMETRY) @@ -590,29 +589,29 @@ void validateAndFixGyroConfig(void) #endif && motorConfig()->dev.useDshotTelemetry ) { - if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_DSHOT600) { - motorConfigMutable()->dev.motorPwmProtocol = PWM_TYPE_DSHOT300; + if (motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_DSHOT600) { + motorConfigMutable()->dev.motorProtocol = MOTOR_PROTOCOL_DSHOT300; } if (gyro.sampleRateHz > 4000) { pidConfigMutable()->pid_process_denom = MAX(2, pidConfig()->pid_process_denom); } } #endif // USE_DSHOT && USE_PID_DENOM_CHECK - switch (motorConfig()->dev.motorPwmProtocol) { - case PWM_TYPE_STANDARD: + switch (motorConfig()->dev.motorProtocol) { + case MOTOR_PROTOCOL_PWM : motorUpdateRestriction = 1.0f / BRUSHLESS_MOTORS_PWM_RATE; break; - case PWM_TYPE_ONESHOT125: + case MOTOR_PROTOCOL_ONESHOT125: motorUpdateRestriction = 0.0005f; break; - case PWM_TYPE_ONESHOT42: + case MOTOR_PROTOCOL_ONESHOT42: motorUpdateRestriction = 0.0001f; break; #ifdef USE_DSHOT - case PWM_TYPE_DSHOT150: + case MOTOR_PROTOCOL_DSHOT150: motorUpdateRestriction = 0.000250f; break; - case PWM_TYPE_DSHOT300: + case MOTOR_PROTOCOL_DSHOT300: motorUpdateRestriction = 0.0001f; break; #endif @@ -621,11 +620,11 @@ void validateAndFixGyroConfig(void) break; } - if (motorConfig()->dev.useUnsyncedPwm) { + if (motorConfig()->dev.useContinuousUpdate) { bool configuredMotorProtocolDshot = false; checkMotorProtocolEnabled(&motorConfig()->dev, &configuredMotorProtocolDshot); // Prevent overriding the max rate of motors - if (!configuredMotorProtocolDshot && motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD) { + if (!configuredMotorProtocolDshot && motorConfig()->dev.motorProtocol != MOTOR_PROTOCOL_PWM ) { const uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction); motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate); } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c index 29dabbcb98..7d87a00383 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c @@ -42,7 +42,6 @@ #include "drivers/io.h" #include "drivers/sensor.h" #include "drivers/time.h" -#include "drivers/pwm_output.h" #include "sensors/gyro.h" #include "pg/gyrodev.h" diff --git a/src/main/drivers/dshot.c b/src/main/drivers/dshot.c index 623bf04290..ec73e04327 100644 --- a/src/main/drivers/dshot.c +++ b/src/main/drivers/dshot.c @@ -39,13 +39,12 @@ #include "config/feature.h" -#include "drivers/motor.h" +#include "drivers/motor_types.h" #include "drivers/timer.h" #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 40f592d832..19105b0659 100644 --- a/src/main/drivers/dshot.h +++ b/src/main/drivers/dshot.h @@ -26,6 +26,9 @@ #include "common/time.h" #include "pg/motor.h" +#include "drivers/motor_types.h" +// TODO: move bitbang as implementation detail of dshot (i.e. dshot should be the interface) +#include "drivers/dshot_bitbang.h" #define DSHOT_MIN_THROTTLE (48) #define DSHOT_MAX_THROTTLE (2047) @@ -90,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 @@ -109,6 +113,22 @@ typedef struct dshotTelemetryState_s { dshotRawValueState_t rawValueState; } dshotTelemetryState_t; +#ifdef USE_DSHOT_TELEMETRY +extern uint32_t readDoneCount; + +FAST_DATA_ZERO_INIT extern uint32_t inputStampUs; + +typedef struct dshotTelemetryCycleCounters_s { + uint32_t irqAt; + uint32_t changeDirectionCompletedAt; +} dshotTelemetryCycleCounters_t; + +FAST_DATA_ZERO_INIT extern dshotTelemetryCycleCounters_t dshotDMAHandlerCycleCounters; + +#endif + +bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig); + extern dshotTelemetryState_t dshotTelemetryState; #ifdef USE_DSHOT_TELEMETRY_STATS @@ -119,6 +139,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/dshot_bitbang.h b/src/main/drivers/dshot_bitbang.h index b24a8f446b..b791240815 100644 --- a/src/main/drivers/dshot_bitbang.h +++ b/src/main/drivers/dshot_bitbang.h @@ -20,13 +20,16 @@ #pragma once +#include "platform.h" + +#include "common/time.h" + +#include "drivers/dma.h" +#include "drivers/io_types.h" +#include "drivers/motor_types.h" #include "drivers/timer.h" -typedef enum { - DSHOT_BITBANG_OFF, - DSHOT_BITBANG_ON, - DSHOT_BITBANG_AUTO, -} dshotBitbangMode_e; +#include "pg/motor.h" typedef enum { DSHOT_BITBANG_STATUS_OK, @@ -35,9 +38,7 @@ typedef enum { DSHOT_BITBANG_STATUS_TOO_MANY_PORTS, } dshotBitbangStatus_e; -struct motorDevConfig_s; -struct motorDevice_s; -struct motorDevice_s *dshotBitbangDevInit(const struct motorDevConfig_s *motorConfig, uint8_t motorCount); +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/dshot_command.c b/src/main/drivers/dshot_command.c index a66e0454e6..90f80185d8 100644 --- a/src/main/drivers/dshot_command.c +++ b/src/main/drivers/dshot_command.c @@ -30,13 +30,9 @@ #include "drivers/io.h" #include "drivers/motor.h" #include "drivers/time.h" -#include "drivers/timer.h" #include "drivers/dshot.h" -#include "drivers/dshot_dpwm.h" -#include "drivers/pwm_output.h" - -#include "dshot_command.h" +#include "drivers/dshot_command.h" #define DSHOT_PROTOCOL_DETECTION_DELAY_MS 3000 #define DSHOT_INITIAL_DELAY_US 10000 @@ -143,8 +139,7 @@ static dshotCommandControl_t* addCommand(void) static bool allMotorsAreIdle(void) { for (unsigned i = 0; i < motorDeviceCount(); i++) { - const motorDmaOutput_t *motor = getMotorDmaOutput(i); - if (motor->protocolControl.value) { + if (!motorIsMotorIdle(i)) { return false; } } @@ -182,7 +177,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot uint8_t repeats = 1; timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US; - motorVTable_t *vTable = motorGetVTable(); + const motorVTable_t *vTable = motorGetVTable(); switch (command) { case DSHOT_CMD_SPIN_DIRECTION_1: @@ -232,8 +227,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot } for (uint8_t i = 0; i < motorDeviceCount(); i++) { - motorDmaOutput_t *const motor = getMotorDmaOutput(i); - motor->protocolControl.requestTelemetry = true; + vTable->requestTelemetry(i); vTable->writeInt(i, (i == index || index == ALL_MOTORS) ? command : DSHOT_CMD_MOTOR_STOP); } @@ -280,7 +274,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot } } -uint8_t dshotCommandGetCurrent(uint8_t index) +uint8_t dshotCommandGetCurrent(unsigned index) { return commandQueue[commandQueueTail].command[index]; } @@ -290,7 +284,7 @@ uint8_t dshotCommandGetCurrent(uint8_t index) // allows the motor output to be sent, "false" means delay until next loop. So take // the example of a dshot command that needs to repeat 10 times at 1ms intervals. // If we have a 8KHz PID loop we'll end up sending the dshot command every 8th motor output. -FAST_CODE_NOINLINE bool dshotCommandOutputIsEnabled(uint8_t motorCount) +FAST_CODE_NOINLINE bool dshotCommandOutputIsEnabled(unsigned motorCount) { UNUSED(motorCount); diff --git a/src/main/drivers/dshot_command.h b/src/main/drivers/dshot_command.h index 9ade0d459f..aa9b0d2b16 100644 --- a/src/main/drivers/dshot_command.h +++ b/src/main/drivers/dshot_command.h @@ -71,6 +71,6 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot void dshotSetPidLoopTime(uint32_t pidLoopTime); bool dshotCommandQueueEmpty(void); bool dshotCommandIsProcessing(void); -uint8_t dshotCommandGetCurrent(uint8_t index); -bool dshotCommandOutputIsEnabled(uint8_t motorCount); +uint8_t dshotCommandGetCurrent(unsigned index); +bool dshotCommandOutputIsEnabled(unsigned motorCount); bool dshotStreamingCommandsAreEnabled(void); diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index bbd59e2ba2..9547eda9c9 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -60,11 +60,7 @@ #define WS2811_DMA_BUF_CACHE_ALIGN_LENGTH (WS2811_DMA_BUF_CACHE_ALIGN_BYTES / sizeof(uint32_t)) DMA_RW_AXI __attribute__((aligned(32))) uint32_t ledStripDMABuffer[WS2811_DMA_BUF_CACHE_ALIGN_LENGTH]; #else -#if defined(STM32F7) FAST_DATA_ZERO_INIT uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#else -uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#endif #endif static ioTag_t ledStripIoTag; diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c index 62d472e7ed..124554928e 100644 --- a/src/main/drivers/motor.c +++ b/src/main/drivers/motor.c @@ -32,10 +32,10 @@ #include "config/feature.h" -#include "drivers/dshot.h" // for DSHOT_ constants in initEscEndpoints; may be gone in the future -#include "drivers/dshot_bitbang.h" -#include "drivers/dshot_dpwm.h" -#include "drivers/pwm_output.h" // for PWM_TYPE_* and others +#include "drivers/dshot.h" +#include "drivers/dshot_bitbang.h" // TODO: bitbang should be behind the veil of dshot (it is an implementation) +#include "drivers/pwm_output.h" + #include "drivers/time.h" #include "fc/rc_controls.h" // for flight3DConfig_t @@ -44,7 +44,7 @@ #include "motor.h" -static FAST_DATA_ZERO_INIT motorDevice_t *motorDevice; +static FAST_DATA_ZERO_INIT motorDevice_t motorDevice; static bool motorProtocolEnabled = false; static bool motorProtocolDshot = false; @@ -52,16 +52,16 @@ static bool motorProtocolDshot = false; void motorShutdown(void) { uint32_t shutdownDelayUs = 1500; - motorDevice->vTable.shutdown(); - motorDevice->enabled = false; - motorDevice->motorEnableTimeMs = 0; - motorDevice->initialized = false; + motorDevice.vTable->shutdown(); + motorDevice.enabled = false; + motorDevice.motorEnableTimeMs = 0; + motorDevice.initialized = false; - switch (motorConfig()->dev.motorPwmProtocol) { - case PWM_TYPE_STANDARD: - case PWM_TYPE_ONESHOT125: - case PWM_TYPE_ONESHOT42: - case PWM_TYPE_MULTISHOT: + switch (motorConfig()->dev.motorProtocol) { + case MOTOR_PROTOCOL_PWM : + case MOTOR_PROTOCOL_ONESHOT125: + case MOTOR_PROTOCOL_ONESHOT42: + case MOTOR_PROTOCOL_MULTISHOT: // Delay 500ms will disarm esc which can prevent motor spin while reboot shutdownDelayUs += 500 * 1000; break; @@ -75,31 +75,33 @@ void motorShutdown(void) void motorWriteAll(float *values) { #ifdef USE_PWM_OUTPUT - if (motorDevice->enabled) { + if (motorDevice.enabled) { #ifdef USE_DSHOT_BITBANG if (isDshotBitbangActive(&motorConfig()->dev)) { // Initialise the output buffers - if (motorDevice->vTable.updateInit) { - motorDevice->vTable.updateInit(); + if (motorDevice.vTable->updateInit) { + motorDevice.vTable->updateInit(); } // Update the motor data - for (int i = 0; i < motorDevice->count; i++) { - motorDevice->vTable.write(i, values[i]); + for (int i = 0; i < motorDevice.count; i++) { + motorDevice.vTable->write(i, values[i]); } // Don't attempt to write commands to the motors if telemetry is still being received - if (motorDevice->vTable.telemetryWait) { - (void)motorDevice->vTable.telemetryWait(); + if (motorDevice.vTable->telemetryWait) { + (void)motorDevice.vTable->telemetryWait(); } // Trigger the transmission of the motor data - motorDevice->vTable.updateComplete(); + motorDevice.vTable->updateComplete(); // Perform the decode of the last data received // New data will be received once the send of motor data, triggered above, completes #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) - motorDevice->vTable.decodeTelemetry(); + if (motorDevice.vTable->decodeTelemetry) { + motorDevice.vTable->decodeTelemetry(); + } #endif } else #endif @@ -107,17 +109,16 @@ void motorWriteAll(float *values) // Perform the decode of the last data received // New data will be received once the send of motor data, triggered above, completes #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) - motorDevice->vTable.decodeTelemetry(); + motorDevice.vTable->decodeTelemetry(); #endif // Update the motor data - for (int i = 0; i < motorDevice->count; i++) { - motorDevice->vTable.write(i, values[i]); + for (int i = 0; i < motorDevice.count; i++) { + motorDevice.vTable->write(i, values[i]); } // Trigger the transmission of the motor data - motorDevice->vTable.updateComplete(); - + motorDevice.vTable->updateComplete(); } } #else @@ -125,32 +126,25 @@ void motorWriteAll(float *values) #endif } +void motorRequestTelemetry(unsigned index) +{ + if (index >= motorDevice.count) { + return; + } + + if (motorDevice.vTable->requestTelemetry) { + motorDevice.vTable->requestTelemetry(index); + } +} + unsigned motorDeviceCount(void) { - return motorDevice->count; + return motorDevice.count; } -motorVTable_t *motorGetVTable(void) +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)); - } + return motorDevice.vTable; } bool checkMotorProtocolEnabled(const motorDevConfig_t *motorDevConfig, bool *isProtocolDshot) @@ -158,20 +152,20 @@ bool checkMotorProtocolEnabled(const motorDevConfig_t *motorDevConfig, bool *isP bool enabled = false; bool isDshot = false; - switch (motorDevConfig->motorPwmProtocol) { - case PWM_TYPE_STANDARD: - case PWM_TYPE_ONESHOT125: - case PWM_TYPE_ONESHOT42: - case PWM_TYPE_MULTISHOT: - case PWM_TYPE_BRUSHED: + switch (motorDevConfig->motorProtocol) { + case MOTOR_PROTOCOL_PWM : + case MOTOR_PROTOCOL_ONESHOT125: + case MOTOR_PROTOCOL_ONESHOT42: + case MOTOR_PROTOCOL_MULTISHOT: + case MOTOR_PROTOCOL_BRUSHED: enabled = true; break; #ifdef USE_DSHOT - case PWM_TYPE_DSHOT150: - case PWM_TYPE_DSHOT300: - case PWM_TYPE_DSHOT600: - case PWM_TYPE_PROSHOT1000: + case MOTOR_PROTOCOL_DSHOT150: + case MOTOR_PROTOCOL_DSHOT300: + case MOTOR_PROTOCOL_DSHOT600: + case MOTOR_PROTOCOL_PROSHOT1000: enabled = true; isDshot = true; break; @@ -187,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); @@ -198,32 +215,154 @@ 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; + } } } float motorConvertFromExternal(uint16_t externalValue) { - return motorDevice->vTable.convertExternalToMotor(externalValue); + return motorDevice.vTable->convertExternalToMotor(externalValue); } uint16_t motorConvertToExternal(float motorValue) { - return motorDevice->vTable.convertMotorToExternal(motorValue); + return motorDevice.vTable->convertMotorToExternal(motorValue); } void motorPostInit(void) { - motorDevice->vTable.postInit(); + if (motorDevice.vTable->postInit) { + motorDevice.vTable->postInit(); + } } +bool isMotorProtocolEnabled(void) +{ + return motorProtocolEnabled; +} + +bool isMotorProtocolDshot(void) +{ + return motorProtocolDshot; +} + +bool isMotorProtocolBidirDshot(void) +{ + return isMotorProtocolDshot() && useDshotTelemetry; +} + +void motorNullDevInit(motorDevice_t *device); + +void motorDevInit(unsigned motorCount) +{ +#if defined(USE_PWM_OUTPUT) || defined(USE_DSHOT) + const motorDevConfig_t *motorDevConfig = &motorConfig()->dev; +#endif + +#if defined(USE_PWM_OUTPUT) + uint16_t idlePulse = motorConfig()->mincommand; + if (featureIsEnabled(FEATURE_3D)) { + idlePulse = flight3DConfig()->neutral3d; + } + if (motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_BRUSHED) { + idlePulse = 0; // brushed motors + } +#endif + + bool success = false; + motorDevice.count = motorCount; + if (isMotorProtocolEnabled()) { + do { + if (!isMotorProtocolDshot()) { +#ifdef USE_PWM_OUTPUT + success = motorPwmDevInit(&motorDevice, motorDevConfig, idlePulse); +#endif + break; + } +#ifdef USE_DSHOT +#ifdef USE_DSHOT_BITBANG + if (isDshotBitbangActive(motorDevConfig)) { + success = dshotBitbangDevInit(&motorDevice, motorDevConfig); + break; + } +#endif + success = dshotPwmDevInit(&motorDevice, motorDevConfig); +#endif + } while(0); + } + + // if the VTable has been populated, the device is initialized. + if (success) { + motorDevice.initialized = true; + motorDevice.motorEnableTimeMs = 0; + motorDevice.enabled = false; + } else { + motorNullDevInit(&motorDevice); + } +} + +void motorDisable(void) +{ + motorDevice.vTable->disable(); + motorDevice.enabled = false; + motorDevice.motorEnableTimeMs = 0; +} + +void motorEnable(void) +{ + if (motorDevice.initialized && motorDevice.vTable->enable()) { + motorDevice.enabled = true; + motorDevice.motorEnableTimeMs = millis(); + } +} + +float motorEstimateMaxRpm(void) +{ + // Empirical testing found this relationship between estimated max RPM without props attached + // (unloaded) and measured max RPM with props attached (loaded), independent from prop size + float unloadedMaxRpm = 0.01f * getBatteryVoltage() * motorConfig()->kv; + float loadDerating = -5.44e-6f * unloadedMaxRpm + 0.944f; + + return unloadedMaxRpm * loadDerating; +} + +bool motorIsEnabled(void) +{ + return motorDevice.enabled; +} + +bool motorIsMotorEnabled(unsigned index) +{ + return motorDevice.vTable->isMotorEnabled(index); +} + +bool motorIsMotorIdle(unsigned index) +{ + return motorDevice.vTable->isMotorIdle ? motorDevice.vTable->isMotorIdle(index) : false; +} + +#ifdef USE_DSHOT +timeMs_t motorGetMotorEnableTimeMs(void) +{ + return motorDevice.motorEnableTimeMs; +} +#endif + +/* functions below for empty methods and no active motors */ void motorPostInitNull(void) { } @@ -237,10 +376,9 @@ static void motorDisableNull(void) { } -static bool motorIsEnabledNull(uint8_t index) +static bool motorIsEnabledNull(unsigned index) { UNUSED(index); - return false; } @@ -293,120 +431,15 @@ static const motorVTable_t motorNullVTable = { .convertExternalToMotor = motorConvertFromExternalNull, .convertMotorToExternal = motorConvertToExternalNull, .shutdown = motorShutdownNull, + .requestTelemetry = NULL, + .isMotorIdle = NULL, }; -static motorDevice_t motorNullDevice = { - .initialized = false, - .enabled = false, -}; - -bool isMotorProtocolEnabled(void) +void motorNullDevInit(motorDevice_t *device) { - return motorProtocolEnabled; + device->vTable = &motorNullVTable; + device->count = 0; } -bool isMotorProtocolDshot(void) -{ - return motorProtocolDshot; -} -bool isMotorProtocolBidirDshot(void) -{ - return isMotorProtocolDshot() && useDshotTelemetry; -} - -void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, uint8_t motorCount) -{ -#if defined(USE_PWM_OUTPUT) || defined(USE_DSHOT) - bool useUnsyncedPwm = motorDevConfig->useUnsyncedPwm; -#else - UNUSED(idlePulse); - UNUSED(motorDevConfig); -#endif - - if (isMotorProtocolEnabled()) { - do { - if (!isMotorProtocolDshot()) { -#ifdef USE_PWM_OUTPUT - motorDevice = motorPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm); -#endif - break; - } -#ifdef USE_DSHOT -#ifdef USE_DSHOT_BITBANG - if (isDshotBitbangActive(motorDevConfig)) { - motorDevice = dshotBitbangDevInit(motorDevConfig, motorCount); - break; - } -#endif - motorDevice = dshotPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm); -#endif - } while(0); - } - - if (motorDevice) { - motorDevice->count = motorCount; - motorDevice->initialized = true; - motorDevice->motorEnableTimeMs = 0; - motorDevice->enabled = false; - } else { - motorNullDevice.vTable = motorNullVTable; - motorDevice = &motorNullDevice; - } -} - -void motorDisable(void) -{ - motorDevice->vTable.disable(); - motorDevice->enabled = false; - motorDevice->motorEnableTimeMs = 0; -} - -void motorEnable(void) -{ - if (motorDevice->initialized && motorDevice->vTable.enable()) { - motorDevice->enabled = true; - motorDevice->motorEnableTimeMs = millis(); - } -} - -float motorEstimateMaxRpm(void) -{ - // Empirical testing found this relationship between estimated max RPM without props attached - // (unloaded) and measured max RPM with props attached (loaded), independent from prop size - float unloadedMaxRpm = 0.01f * getBatteryVoltage() * motorConfig()->kv; - float loadDerating = -5.44e-6f * unloadedMaxRpm + 0.944f; - - return unloadedMaxRpm * loadDerating; -} - -bool motorIsEnabled(void) -{ - return motorDevice->enabled; -} - -bool motorIsMotorEnabled(uint8_t index) -{ - return motorDevice->vTable.isMotorEnabled(index); -} - -#ifdef USE_DSHOT -timeMs_t motorGetMotorEnableTimeMs(void) -{ - return motorDevice->motorEnableTimeMs; -} -#endif - -#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->motorPwmProtocol != PWM_TYPE_PROSHOT1000); -#else - return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON || - (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->motorPwmProtocol != PWM_TYPE_PROSHOT1000); -#endif -} -#endif #endif // USE_MOTOR diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h index 2203ce7c2b..d43f430446 100644 --- a/src/main/drivers/motor.h +++ b/src/main/drivers/motor.h @@ -26,67 +26,24 @@ #include "pg/motor.h" -typedef enum { - PWM_TYPE_STANDARD = 0, - PWM_TYPE_ONESHOT125, - PWM_TYPE_ONESHOT42, - PWM_TYPE_MULTISHOT, - PWM_TYPE_BRUSHED, - PWM_TYPE_DSHOT150, - PWM_TYPE_DSHOT300, - PWM_TYPE_DSHOT600, -// PWM_TYPE_DSHOT1200, removed - PWM_TYPE_PROSHOT1000, - PWM_TYPE_DISABLED, - PWM_TYPE_MAX -} motorPwmProtocolTypes_e; +#include "drivers/motor_types.h" -typedef struct motorVTable_s { - // Common - void (*postInit)(void); - float (*convertExternalToMotor)(uint16_t externalValue); - uint16_t (*convertMotorToExternal)(float motorValue); - bool (*enable)(void); - void (*disable)(void); - bool (*isMotorEnabled)(uint8_t index); - bool (*telemetryWait)(void); - bool (*decodeTelemetry)(void); - void (*updateInit)(void); - void (*write)(uint8_t index, float value); - void (*writeInt)(uint8_t index, uint16_t value); - void (*updateComplete)(void); - void (*shutdown)(void); - - // Digital commands - -} motorVTable_t; - -typedef struct motorDevice_s { - motorVTable_t vTable; - uint8_t count; - bool initialized; - bool enabled; - timeMs_t motorEnableTimeMs; -} motorDevice_t; - -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); void motorInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3DHigh, float *deadbandMotor3DLow); float motorConvertFromExternal(uint16_t externalValue); uint16_t motorConvertToExternal(float motorValue); -struct motorDevConfig_s; // XXX Shouldn't be needed once pwm_output* is really cleaned up. -void motorDevInit(const struct motorDevConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount); +void motorDevInit(unsigned motorCount); unsigned motorDeviceCount(void); -motorVTable_t *motorGetVTable(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); @@ -95,12 +52,7 @@ void motorDisable(void); void motorEnable(void); float motorEstimateMaxRpm(void); bool motorIsEnabled(void); -bool motorIsMotorEnabled(uint8_t index); +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_impl.h b/src/main/drivers/motor_impl.h new file mode 100644 index 0000000000..c5defbc451 --- /dev/null +++ b/src/main/drivers/motor_impl.h @@ -0,0 +1,31 @@ +/* + * 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/motor_types.h" + +void motorPostInitNull(void); +void motorWriteNull(uint8_t index, float value); +bool motorDecodeTelemetryNull(void); +void motorUpdateCompleteNull(void); + +void motorNullDevInit(motorDevice_t *device); diff --git a/src/main/drivers/motor_types.h b/src/main/drivers/motor_types.h new file mode 100644 index 0000000000..c70fcee0b0 --- /dev/null +++ b/src/main/drivers/motor_types.h @@ -0,0 +1,78 @@ +/* + * 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 "common/time.h" + +#define ALL_MOTORS 255 +#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, + MOTOR_PROTOCOL_ONESHOT42, + MOTOR_PROTOCOL_MULTISHOT, + MOTOR_PROTOCOL_BRUSHED, + MOTOR_PROTOCOL_DSHOT150, + MOTOR_PROTOCOL_DSHOT300, + MOTOR_PROTOCOL_DSHOT600, +/* MOTOR_PROTOCOL_DSHOT1200, removed */ + MOTOR_PROTOCOL_PROSHOT1000, + MOTOR_PROTOCOL_DISABLED, + MOTOR_PROTOCOL_MAX +} motorProtocolTypes_e; + +typedef struct motorVTable_s { + // Common + void (*postInit)(void); + float (*convertExternalToMotor)(uint16_t externalValue); + uint16_t (*convertMotorToExternal)(float motorValue); + bool (*enable)(void); + void (*disable)(void); + bool (*isMotorEnabled)(unsigned index); + bool (*telemetryWait)(void); + bool (*decodeTelemetry)(void); + void (*updateInit)(void); + void (*write)(uint8_t index, float value); + void (*writeInt)(uint8_t index, uint16_t value); + void (*updateComplete)(void); + void (*shutdown)(void); + bool (*isMotorIdle)(unsigned index); + + // Digital commands + void (*requestTelemetry)(unsigned index); +} motorVTable_t; + +typedef struct motorDevice_s { + const motorVTable_t *vTable; + uint8_t count; + bool initialized; + bool enabled; + timeMs_t motorEnableTimeMs; +} motorDevice_t; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c new file mode 100644 index 0000000000..813b97e856 --- /dev/null +++ b/src/main/drivers/pwm_output.c @@ -0,0 +1,51 @@ +/* + * 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" +#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)) { + const 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 21b7de0078..644e2415cc 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -26,19 +26,14 @@ #include "drivers/dma.h" #include "drivers/io_types.h" -#include "drivers/motor.h" +#include "drivers/motor_types.h" #include "drivers/timer.h" -#define BRUSHED_MOTORS_PWM_RATE 16000 -#define BRUSHLESS_MOTORS_PWM_RATE 480 - -#define ALL_MOTORS 255 - -#define MOTOR_OUTPUT_LIMIT_PERCENT_MIN 1 -#define MOTOR_OUTPUT_LIMIT_PERCENT_MAX 100 +#include "pg/motor.h" #define PWM_TIMER_1MHZ MHZ_TO_HZ(1) +// TODO: move the implementation defintions to impl header (platform) struct timerHardware_s; typedef struct { @@ -56,9 +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; -struct motorDevConfig_s; -motorDevice_t *motorPwmDevInit(const struct motorDevConfig_s *motorDevConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm); +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) @@ -77,3 +72,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/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 369851d05d..b0d7424a1b 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -181,11 +181,7 @@ static void escSerialGPIOConfig(const timerHardware_t *timhw, ioConfig_t cfg) } IOInit(IOGetByTag(tag), OWNER_MOTOR, 0); -#if defined(STM32F7) || defined(STM32H7) IOConfigGPIOAF(IOGetByTag(tag), cfg, timhw->alternateFunction); -#else - IOConfigGPIO(IOGetByTag(tag), cfg); -#endif } static void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr) diff --git a/src/main/drivers/serial_escserial.h b/src/main/drivers/serial_escserial.h index ca1448409b..1efa9b808b 100644 --- a/src/main/drivers/serial_escserial.h +++ b/src/main/drivers/serial_escserial.h @@ -20,8 +20,6 @@ #pragma once -#include "drivers/pwm_output.h" - #define ESCSERIAL_BUFFER_SIZE 1024 typedef enum { diff --git a/src/main/fc/init.c b/src/main/fc/init.c index becf43fa18..8e1646411b 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -64,7 +64,6 @@ #include "drivers/nvic.h" #include "drivers/persistent.h" #include "drivers/pin_pull_up_down.h" -#include "drivers/pwm_output.h" #include "drivers/rx/rx_pwm.h" #include "drivers/sensor.h" #include "drivers/serial.h" @@ -541,21 +540,12 @@ void init(void) mixerInit(mixerConfig()->mixerMode); - uint16_t idlePulse = motorConfig()->mincommand; - if (featureIsEnabled(FEATURE_3D)) { - idlePulse = flight3DConfig()->neutral3d; - } - if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { - idlePulse = 0; // brushed motors - } #ifdef USE_MOTOR /* Motors needs to be initialized soon as posible because hardware initialization * may send spurious pulses to esc's causing their early initialization. Also ppm * receiver may share timer with motors so motors MUST be initialized here. */ - motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount()); + motorDevInit(getMotorCount()); systemState |= SYSTEM_STATE_MOTORS_READY; -#else - UNUSED(idlePulse); #endif if (0) {} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 7371f9bdee..c6e1afa326 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -30,7 +30,7 @@ #include "pg/pg.h" #include "drivers/io_types.h" -#include "drivers/pwm_output.h" +#include "drivers/motor.h" #define QUAD_MOTOR_COUNT 4 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 071c1feaf2..2635692f1a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -36,7 +36,6 @@ #include "config/config_reset.h" #include "config/simplified_tuning.h" -#include "drivers/pwm_output.h" #include "drivers/sound_beeper.h" #include "drivers/time.h" diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 6482febc27..5dda23f3b9 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 "flight/mixer.h" #include "io/beeper.h" diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 4b83a4708c..fdb95d7cd1 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1871,12 +1871,12 @@ case MSP_NAME: case MSP_ADVANCED_CONFIG: sbufWriteU8(dst, 1); // was gyroConfig()->gyro_sync_denom - removed in API 1.43 sbufWriteU8(dst, pidConfig()->pid_process_denom); - sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm); - sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol); + sbufWriteU8(dst, motorConfig()->dev.useContinuousUpdate); + sbufWriteU8(dst, motorConfig()->dev.motorProtocol); sbufWriteU16(dst, motorConfig()->dev.motorPwmRate); sbufWriteU16(dst, motorConfig()->motorIdle); sbufWriteU8(dst, 0); // DEPRECATED: gyro_use_32kHz - sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion); + sbufWriteU8(dst, motorConfig()->dev.motorInversion); sbufWriteU8(dst, gyroConfig()->gyro_to_use); sbufWriteU8(dst, gyroConfig()->gyro_high_fsr); sbufWriteU8(dst, gyroConfig()->gyroMovementCalibrationThreshold); @@ -3074,8 +3074,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, case MSP_SET_ADVANCED_CONFIG: sbufReadU8(src); // was gyroConfigMutable()->gyro_sync_denom - removed in API 1.43 pidConfigMutable()->pid_process_denom = sbufReadU8(src); - motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src); - motorConfigMutable()->dev.motorPwmProtocol = sbufReadU8(src); + motorConfigMutable()->dev.useContinuousUpdate = sbufReadU8(src); + motorConfigMutable()->dev.motorProtocol = sbufReadU8(src); motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src); if (sbufBytesRemaining(src) >= 2) { motorConfigMutable()->motorIdle = sbufReadU16(src); @@ -3084,7 +3084,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU8(src); // DEPRECATED: gyro_use_32khz } if (sbufBytesRemaining(src)) { - motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src); + motorConfigMutable()->dev.motorInversion = sbufReadU8(src); } if (sbufBytesRemaining(src) >= 8) { gyroConfigMutable()->gyro_to_use = sbufReadU8(src); diff --git a/src/main/pg/motor.c b/src/main/pg/motor.c index e89a9fbca4..d546ed1137 100644 --- a/src/main/pg/motor.c +++ b/src/main/pg/motor.c @@ -25,7 +25,7 @@ #ifdef USE_MOTOR -#include "drivers/pwm_output.h" +#include "drivers/motor_types.h" #include "pg/pg.h" #include "pg/pg_ids.h" @@ -51,31 +51,28 @@ PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 3); void pgResetFn_motorConfig(motorConfig_t *motorConfig) { -#ifdef BRUSHED_MOTORS +#if defined(USE_BRUSHED_MOTORS) motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED; - motorConfig->dev.useUnsyncedPwm = true; + motorConfig->dev.motorProtocol = MOTOR_PROTOCOL_BRUSHED; + motorConfig->dev.useContinuousUpdate = true; + motorConfig->motorIdle = 700; // historical default minThrottle for brushed was 1070 #else motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; -#ifndef USE_DSHOT - if (motorConfig->dev.motorPwmProtocol == PWM_TYPE_STANDARD) { - motorConfig->dev.useUnsyncedPwm = true; - } - motorConfig->dev.motorPwmProtocol = PWM_TYPE_DISABLED; -#elif defined(DEFAULT_MOTOR_DSHOT_SPEED) - motorConfig->dev.motorPwmProtocol = DEFAULT_MOTOR_DSHOT_SPEED; + motorConfig->motorIdle = 550; +#if !defined(USE_DSHOT) && defined(USE_PWM_OUTPUT) + motorConfig->dev.motorProtocol = MOTOR_PROTOCOL_PWM; + motorConfig->dev.useContinuousUpdate = true; +#elif defined(USE_DSHOT) && defined(DEFAULT_MOTOR_DSHOT_SPEED) + motorConfig->dev.motorProtocol = DEFAULT_MOTOR_DSHOT_SPEED; +#elif defined(USE_DSHOT) + motorConfig->dev.motorProtocol = MOTOR_PROTOCOL_DSHOT600; #else - motorConfig->dev.motorPwmProtocol = PWM_TYPE_DSHOT600; -#endif // USE_DSHOT -#endif // BRUSHED_MOTORS + motorConfig->dev.motorProtocol = MOTOR_PROTOCOL_DISABLED; +#endif // protocol selection +#endif // brushed motors motorConfig->maxthrottle = 2000; motorConfig->mincommand = 1000; -#ifdef BRUSHED_MOTORS - motorConfig->motorIdle = 700; // historical default minThrottle for brushed was 1070 -#else - motorConfig->motorIdle = 550; -#endif // BRUSHED_MOTORS motorConfig->kv = 1960; #ifdef USE_TIMER diff --git a/src/main/pg/motor.h b/src/main/pg/motor.h index c4fca53b2f..1b8564862a 100644 --- a/src/main/pg/motor.h +++ b/src/main/pg/motor.h @@ -23,30 +23,45 @@ #include "pg/pg.h" #include "drivers/io.h" -#include "drivers/dshot_bitbang.h" +#if !defined(BRUSHED_MOTORS_PWM_RATE) +#define BRUSHED_MOTORS_PWM_RATE 16000 +#endif + +#if !defined(BRUSHLESS_MOTORS_PWM_RATE) +#define BRUSHLESS_MOTORS_PWM_RATE 480 +#endif + +//TODO: Timers are platform specific. This should be moved to platform specific code. typedef enum { DSHOT_BITBANGED_TIMER_AUTO = 0, DSHOT_BITBANGED_TIMER_TIM1, DSHOT_BITBANGED_TIMER_TIM8, } dshotBitbangedTimer_e; +//TODO: DMAR is platform specific. This should be moved to platform specific code. typedef enum { DSHOT_DMAR_OFF, DSHOT_DMAR_ON, DSHOT_DMAR_AUTO } dshotDmar_e; +typedef enum { + DSHOT_BITBANG_OFF, + DSHOT_BITBANG_ON, + DSHOT_BITBANG_AUTO, +} dshotBitbangMode_e; + typedef enum { DSHOT_TELEMETRY_OFF, DSHOT_TELEMETRY_ON, } dshotTelemetry_e; typedef struct motorDevConfig_s { - uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) - uint8_t motorPwmProtocol; // Pwm Protocol - uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation - uint8_t useUnsyncedPwm; + uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) + uint8_t motorProtocol; // Pwm Protocol + uint8_t motorInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation + uint8_t useContinuousUpdate; uint8_t useBurstDshot; uint8_t useDshotTelemetry; uint8_t useDshotEdt; diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 06bed65e2b..067a889c1c 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -42,7 +42,6 @@ #include "drivers/timer.h" #include "drivers/motor.h" #include "drivers/dshot.h" -#include "drivers/dshot_dpwm.h" #include "drivers/serial.h" #include "drivers/serial_uart.h" @@ -321,8 +320,7 @@ void escSensorProcess(timeUs_t currentTimeUs) escTriggerTimestamp = currentTimeMs; startEscDataRead(telemetryBuffer, TELEMETRY_FRAME_SIZE); - motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor); - motor->protocolControl.requestTelemetry = true; + motorRequestTelemetry(escSensorMotor); escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING; DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1); diff --git a/src/platform/APM32/dshot_bitbang.c b/src/platform/APM32/dshot_bitbang.c index 92de301f43..5776305d62 100644 --- a/src/platform/APM32/dshot_bitbang.c +++ b/src/platform/APM32/dshot_bitbang.c @@ -36,15 +36,14 @@ #include "drivers/dma_reqmap.h" #include "drivers/dshot.h" #include "drivers/dshot_bitbang.h" -#include "drivers/dshot_bitbang_impl.h" +#include "dshot_bitbang_impl.h" #include "drivers/dshot_command.h" #include "drivers/motor.h" #include "drivers/nvic.h" -#include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring -#include "drivers/dshot_dpwm.h" // XXX for motorDmaOutput_t *getMotorDmaOutput(uint8_t index); should go away with refactoring #include "drivers/dshot_bitbang_decode.h" #include "drivers/time.h" #include "drivers/timer.h" +#include "pwm_output_dshot_shared.h" #include "pg/motor.h" #include "pg/pinio.h" @@ -58,17 +57,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]; - -static FAST_DATA_ZERO_INIT int motorCount; -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. @@ -106,10 +94,9 @@ const timerHardware_t bbTimerHardware[] = { #endif }; -static FAST_DATA_ZERO_INIT motorDevice_t bbDevice; static FAST_DATA_ZERO_INIT timeUs_t lastSendUs; -static motorPwmProtocolTypes_e motorPwmProtocol; +static motorProtocolTypes_e motorProtocol; // DMA GPIO output buffer formatting @@ -259,15 +246,15 @@ const resourceOwner_t *dshotBitbangTimerGetOwner(const timerHardware_t *timer) // Return frequency of smallest change [state/sec] -static uint32_t getDshotBaseFrequency(motorPwmProtocolTypes_e pwmProtocolType) +static uint32_t getDshotBaseFrequency(motorProtocolTypes_e pwmProtocolType) { switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT600): + case(MOTOR_PROTOCOL_DSHOT600): return MOTOR_DSHOT600_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; - case(PWM_TYPE_DSHOT300): + case(MOTOR_PROTOCOL_DSHOT300): return MOTOR_DSHOT300_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; default: - case(PWM_TYPE_DSHOT150): + case(MOTOR_PROTOCOL_DSHOT150): return MOTOR_DSHOT150_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; } } @@ -380,7 +367,7 @@ static void bbFindPacerTimer(void) } } -static void bbTimebaseSetup(bbPort_t *bbPort, motorPwmProtocolTypes_e dshotProtocolType) +static void bbTimebaseSetup(bbPort_t *bbPort, motorProtocolTypes_e dshotProtocolType) { uint32_t timerclock = timerClock(bbPort->timhw->tim); @@ -398,7 +385,7 @@ static void bbTimebaseSetup(bbPort_t *bbPort, motorPwmProtocolTypes_e dshotProto // it does not use the timer channel associated with the pin. // -static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output) { // Return if no GPIO is specified if (!io) { @@ -430,10 +417,6 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p } if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) { - bbDevice.vTable.write = motorWriteNull; - bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - bbDevice.vTable.updateComplete = motorUpdateCompleteNull; - return false; } @@ -531,7 +514,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, @@ -614,7 +597,7 @@ static void bbUpdateComplete(void) // If there is a dshot command loaded up, time it correctly with motor update if (!dshotCommandQueueEmpty()) { - if (!dshotCommandOutputIsEnabled(bbDevice.count)) { + if (!dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } } @@ -649,7 +632,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); } @@ -667,7 +650,7 @@ static void bbShutdown(void) return; } -static bool bbIsMotorEnabled(uint8_t index) +static bool bbIsMotorEnabled(unsigned index) { return bbMotors[index].enabled; } @@ -676,17 +659,13 @@ 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, motorPwmProtocol, bbMotors[motorIndex].output)) { + if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) { return; } bbMotors[motorIndex].enabled = true; - - // Fill in motors structure for 4way access (XXX Should be refactored) - - motors[motorIndex].enabled = true; } } @@ -704,6 +683,8 @@ static motorVTable_t bbVTable = { .convertExternalToMotor = dshotConvertFromExternal, .convertMotorToExternal = dshotConvertToExternal, .shutdown = bbShutdown, + .isMotorIdle = bbDshotIsMotorIdle, + .requestTelemetry = bbDshotRequestTelemetry, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) @@ -711,14 +692,18 @@ dshotBitbangStatus_e dshotBitbangGetStatus(void) return bbStatus; } -motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t count) +bool dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) { dbgPinLo(0); dbgPinLo(1); - motorPwmProtocol = motorConfig->motorPwmProtocol; - bbDevice.vTable = bbVTable; - motorCount = count; + if (!device || !motorConfig) { + return false; + } + + motorProtocol = motorConfig->motorProtocol; + device->vTable = &bbVTable; + dshotMotorCount = device->count; bbStatus = DSHOT_BITBANG_STATUS_OK; #ifdef USE_DSHOT_TELEMETRY @@ -727,12 +712,12 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t 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]); - uint8_t output = motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output; + uint8_t output = motorConfig->motorInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output; bbPuPdMode = (output & TIMER_OUTPUT_INVERTED) ? BB_GPIO_PULLDOWN : BB_GPIO_PULLUP; #ifdef USE_DSHOT_TELEMETRY @@ -743,11 +728,10 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t if (!IOIsFreeOrPreinit(io)) { /* not enough motors initialised for the mixer or a break in the motors */ - bbDevice.vTable.write = motorWriteNull; - bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - bbDevice.vTable.updateComplete = motorUpdateCompleteNull; + device->vTable = NULL; + dshotMotorCount = 0; bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT; - return NULL; + return false; } int pinIndex = IO_GPIOPinIdx(io); @@ -766,12 +750,8 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t } else { IOHi(io); } - - // Fill in motors structure for 4way access (XXX Should be refactored) - motors[motorIndex].io = bbMotors[motorIndex].io; } - - return &bbDevice; + return true; } #endif // USE_DSHOT_BITBANG diff --git a/src/platform/APM32/dshot_bitbang_ddl.c b/src/platform/APM32/dshot_bitbang_ddl.c index c03e02ff9b..50d53cf7e8 100644 --- a/src/platform/APM32/dshot_bitbang_ddl.c +++ b/src/platform/APM32/dshot_bitbang_ddl.c @@ -35,7 +35,7 @@ #include "drivers/dma.h" #include "drivers/dma_reqmap.h" #include "drivers/dshot.h" -#include "drivers/dshot_bitbang_impl.h" +#include "dshot_bitbang_impl.h" #include "drivers/dshot_command.h" #include "drivers/motor.h" #include "drivers/nvic.h" diff --git a/src/platform/APM32/mk/APM32F4.mk b/src/platform/APM32/mk/APM32F4.mk index 5203bf6781..b82d82cf56 100644 --- a/src/platform/APM32/mk/APM32F4.mk +++ b/src/platform/APM32/mk/APM32F4.mk @@ -152,7 +152,9 @@ MCU_COMMON_SRC = \ APM32/startup/system_apm32f4xx.c \ drivers/inverter.c \ drivers/dshot_bitbang_decode.c \ - drivers/pwm_output_dshot_shared.c \ + common/stm32/pwm_output_dshot_shared.c \ + common/stm32/dshot_dpwm.c \ + common/stm32/dshot_bitbang_shared.c \ APM32/bus_spi_apm32.c \ APM32/bus_i2c_apm32.c \ APM32/bus_i2c_apm32_init.c \ @@ -206,6 +208,8 @@ MSC_SRC = \ msc/usbd_storage_sdio.c SPEED_OPTIMISED_SRC += \ + common/stm32/dshot_bitbang_shared.c \ + common/stm32/pwm_output_dshot_shared.c \ common/stm32/bus_spi_hw.c \ common/stm32/system.c diff --git a/src/platform/APM32/pwm_output_apm32.c b/src/platform/APM32/pwm_output_apm32.c index a2d3254fe7..a49c58d748 100644 --- a/src/platform/APM32/pwm_output_apm32.c +++ b/src/platform/APM32/pwm_output_apm32.c @@ -36,12 +36,12 @@ #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); - if (Handle == NULL) return; + if (Handle == NULL) { + return; + } TMR_OC_InitTypeDef TMR_OCInitStructure; @@ -59,7 +59,9 @@ static void pwmOCConfig(TMR_TypeDef *tim, uint8_t channel, uint16_t value, uint8 void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion) { TMR_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim); - if (Handle == NULL) return; + if (Handle == NULL) { + return; + } configTimeBase(timerHardware->tim, period, hz); pwmOCConfig(timerHardware->tim, @@ -68,10 +70,11 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output ); - if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) + if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { DAL_TMREx_PWMN_Start(Handle, timerHardware->channel); - else + } else { DAL_TMR_PWM_Start(Handle, timerHardware->channel); + } DAL_TMR_Base_Start(Handle); channel->ccr = timerChCCR(timerHardware); @@ -81,13 +84,7 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, *channel->ccr = 0; } -static FAST_DATA_ZERO_INIT motorDevice_t motorPwmDevice; - -static void pwmWriteUnused(uint8_t index, float value) -{ - UNUSED(index); - UNUSED(value); -} +static bool useContinuousUpdate = true; static void pwmWriteStandard(uint8_t index, float value) { @@ -97,7 +94,7 @@ static void pwmWriteStandard(uint8_t index, float value) void pwmShutdownPulsesForAllMotors(void) { - for (int index = 0; index < motorPwmDevice.count; 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; @@ -114,17 +111,21 @@ static motorVTable_t motorPwmVTable; bool pwmEnableMotors(void) { /* check motors can be enabled */ - return (motorPwmVTable.write != &pwmWriteUnused); + return pwmMotorCount > 0; } -bool pwmIsMotorEnabled(uint8_t index) +bool pwmIsMotorEnabled(unsigned index) { return motors[index].enabled; } -static void pwmCompleteOneshotMotorUpdate(void) +static void pwmCompleteMotorUpdate(void) { - for (int index = 0; index < motorPwmDevice.count; index++) { + if (useContinuousUpdate) { + return; + } + + for (int index = 0; index < pwmMotorCount; index++) { if (motors[index].forceOverflow) { timerForceOverflow(motors[index].channel.tim); } @@ -145,65 +146,72 @@ static uint16_t pwmConvertToExternal(float motorValue) } static motorVTable_t motorPwmVTable = { - .postInit = motorPostInitNull, + .postInit = NULL, .enable = pwmEnableMotors, .disable = pwmDisableMotors, .isMotorEnabled = pwmIsMotorEnabled, .shutdown = pwmShutdownPulsesForAllMotors, .convertExternalToMotor = pwmConvertFromExternal, .convertMotorToExternal = pwmConvertToExternal, + .write = pwmWriteStandard, + .decodeTelemetry = NULL, + .updateComplete = pwmCompleteMotorUpdate, + .requestTelemetry = NULL, + .isMotorIdle = NULL, }; -motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm) +bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { memset(motors, 0, sizeof(motors)); - motorPwmDevice.vTable = motorPwmVTable; + if (!device || !motorConfig) { + return false; + } + pwmMotorCount = device->count; + device->vTable = &motorPwmVTable; + + useContinuousUpdate = motorConfig->useContinuousUpdate; float sMin = 0; float sLen = 0; - switch (motorConfig->motorPwmProtocol) { + switch (motorConfig->motorProtocol) { default: - case PWM_TYPE_ONESHOT125: + case MOTOR_PROTOCOL_ONESHOT125: sMin = 125e-6f; sLen = 125e-6f; break; - case PWM_TYPE_ONESHOT42: + case MOTOR_PROTOCOL_ONESHOT42: sMin = 42e-6f; sLen = 42e-6f; break; - case PWM_TYPE_MULTISHOT: + case MOTOR_PROTOCOL_MULTISHOT: sMin = 5e-6f; sLen = 20e-6f; break; - case PWM_TYPE_BRUSHED: + case MOTOR_PROTOCOL_BRUSHED: sMin = 0; - useUnsyncedPwm = true; + useContinuousUpdate = true; idlePulse = 0; break; - case PWM_TYPE_STANDARD: + case MOTOR_PROTOCOL_PWM : sMin = 1e-3f; sLen = 1e-3f; - useUnsyncedPwm = true; + useContinuousUpdate = true; idlePulse = 0; break; } - motorPwmDevice.vTable.write = pwmWriteStandard; - motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate; - - 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)); if (timerHardware == NULL) { /* not enough motors initialised for the mixer or a break in the motors */ - motorPwmDevice.vTable.write = &pwmWriteUnused; - motorPwmDevice.vTable.updateComplete = motorUpdateCompleteNull; + device->vTable = NULL; + pwmMotorCount = 0; /* TODO: block arming and add reason system cannot arm */ - return NULL; + return false; } motors[motorIndex].io = IOGetByTag(tag); @@ -213,23 +221,23 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl /* standard PWM outputs */ // margin of safety is 4 periods when unsynced - const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4)); + const unsigned pwmRateHz = useContinuousUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4)); const uint32_t clock = timerClock(timerHardware->tim); /* used to find the desired timer frequency for max resolution */ const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */ const uint32_t hz = clock / prescaler; - const unsigned period = useUnsyncedPwm ? hz / pwmRateHz : 0xffff; + const unsigned period = useContinuousUpdate ? hz / pwmRateHz : 0xffff; /* if brushed then it is the entire length of the period. TODO: this can be moved back to periodMin and periodLen once mixer outputs a 0..1 float value. */ - motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f; + motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); - pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion); + pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); bool timerAlreadyUsed = false; for (int i = 0; i < motorIndex; i++) { @@ -242,7 +250,7 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl motors[motorIndex].enabled = true; } - return &motorPwmDevice; + 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 4025aa531c..499897394f 100644 --- a/src/platform/APM32/pwm_output_dshot_apm32.c +++ b/src/platform/APM32/pwm_output_dshot_apm32.c @@ -34,13 +34,13 @@ #include "drivers/dma.h" #include "drivers/dma_reqmap.h" #include "drivers/dshot.h" -#include "drivers/dshot_dpwm.h" +#include "dshot_dpwm.h" #include "drivers/dshot_command.h" #include "drivers/io.h" #include "drivers/nvic.h" #include "drivers/motor.h" #include "drivers/pwm_output.h" -#include "drivers/pwm_output_dshot_shared.h" +#include "pwm_output_dshot_shared.h" #include "drivers/rcc.h" #include "drivers/time.h" #include "drivers/timer.h" @@ -48,9 +48,9 @@ #ifdef USE_DSHOT_TELEMETRY -void dshotEnableChannels(uint8_t motorCount) +void dshotEnableChannels(unsigned motorCount) { - for (int i = 0; i < motorCount; i++) { + for (unsigned i = 0; i < motorCount; i++) { if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) { DDL_TMR_CC_EnableChannel(dmaMotors[i].timerHardware->tim, dmaMotors[i].llChannel << 4); } else { @@ -128,7 +128,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(dshotPwmDevice.count)) { + if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } @@ -188,7 +188,7 @@ FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) } } -bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output) { #ifdef USE_DSHOT_TELEMETRY #define OCINIT motor->ocInitStruct @@ -276,7 +276,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m DDL_TMR_DisableCounter(timer); init.Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1); - init.Autoreload = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1; + init.Autoreload = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1; init.ClockDivision = DDL_TMR_CLOCKDIVISION_DIV1; init.RepetitionCounter = 0; init.CounterMode = DDL_TMR_COUNTERMODE_UP; @@ -356,7 +356,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m DMAINIT.FIFOMode = DDL_DMA_FIFOMODE_ENABLE; DMAINIT.MemBurst = DDL_DMA_MBURST_SINGLE; DMAINIT.PeriphBurst = DDL_DMA_PBURST_SINGLE; - DMAINIT.NbData = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; + DMAINIT.NbData = pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; DMAINIT.PeriphOrM2MSrcIncMode = DDL_DMA_PERIPH_NOINCREMENT; DMAINIT.MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_INCREMENT; DMAINIT.PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_WORD; @@ -374,7 +374,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m #ifdef USE_DSHOT_TELEMETRY motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 * ( 16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType); - motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; + motor->timer->outputPeriod = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; pwmDshotSetDirectionOutput(motor); #else pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT); diff --git a/src/platform/AT32/dshot_bitbang.c b/src/platform/AT32/dshot_bitbang.c index ccff17ae91..d3ac4f2e88 100644 --- a/src/platform/AT32/dshot_bitbang.c +++ b/src/platform/AT32/dshot_bitbang.c @@ -36,12 +36,11 @@ #include "drivers/dma_reqmap.h" #include "drivers/dshot.h" #include "drivers/dshot_bitbang.h" -#include "drivers/dshot_bitbang_impl.h" +#include "dshot_bitbang_impl.h" #include "drivers/dshot_command.h" #include "drivers/motor.h" #include "drivers/nvic.h" -#include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring -#include "drivers/dshot_dpwm.h" // XXX for motorDmaOutput_t *getMotorDmaOutput(uint8_t index); should go away with refactoring +#include "pwm_output_dshot_shared.h" #include "drivers/dshot_bitbang_decode.h" #include "drivers/time.h" #include "drivers/timer.h" @@ -51,17 +50,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]; - -static FAST_DATA_ZERO_INIT int motorCount; -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. @@ -91,10 +79,9 @@ const timerHardware_t bbTimerHardware[] = { DEF_TIM(TMR1, CH4, NONE, 0, 3, 0), }; -static FAST_DATA_ZERO_INIT motorDevice_t bbDevice; static FAST_DATA_ZERO_INIT timeUs_t lastSendUs; -static motorPwmProtocolTypes_e motorPwmProtocol; +static motorProtocolTypes_e motorProtocol; // DMA GPIO output buffer formatting @@ -244,15 +231,15 @@ const resourceOwner_t *dshotBitbangTimerGetOwner(const timerHardware_t *timer) // Return frequency of smallest change [state/sec] -static uint32_t getDshotBaseFrequency(motorPwmProtocolTypes_e pwmProtocolType) +static uint32_t getDshotBaseFrequency(motorProtocolTypes_e pwmProtocolType) { switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT600): + case(MOTOR_PROTOCOL_DSHOT600): return MOTOR_DSHOT600_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; - case(PWM_TYPE_DSHOT300): + case(MOTOR_PROTOCOL_DSHOT300): return MOTOR_DSHOT300_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; default: - case(PWM_TYPE_DSHOT150): + case(MOTOR_PROTOCOL_DSHOT150): return MOTOR_DSHOT150_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; } } @@ -365,7 +352,7 @@ static void bbFindPacerTimer(void) } } -static void bbTimebaseSetup(bbPort_t *bbPort, motorPwmProtocolTypes_e dshotProtocolType) +static void bbTimebaseSetup(bbPort_t *bbPort, motorProtocolTypes_e dshotProtocolType) { uint32_t timerclock = timerClock(bbPort->timhw->tim); @@ -383,7 +370,7 @@ static void bbTimebaseSetup(bbPort_t *bbPort, motorPwmProtocolTypes_e dshotProto // it does not use the timer channel associated with the pin. // -static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output) { int pinIndex = IO_GPIOPinIdx(io); int portIndex = IO_GPIOPortIdx(io); @@ -410,10 +397,6 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p } if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) { - bbDevice.vTable.write = motorWriteNull; - bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - bbDevice.vTable.updateComplete = motorUpdateCompleteNull; - return false; } @@ -511,7 +494,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, @@ -594,7 +577,7 @@ static void bbUpdateComplete(void) // If there is a dshot command loaded up, time it correctly with motor update if (!dshotCommandQueueEmpty()) { - if (!dshotCommandOutputIsEnabled(bbDevice.count)) { + if (!dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } } @@ -641,7 +624,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); } @@ -659,7 +642,7 @@ static void bbShutdown(void) return; } -static bool bbIsMotorEnabled(uint8_t index) +static bool bbIsMotorEnabled(unsigned index) { return bbMotors[index].enabled; } @@ -668,17 +651,13 @@ 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, motorPwmProtocol, bbMotors[motorIndex].output)) { + if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) { return; } bbMotors[motorIndex].enabled = true; - - // Fill in motors structure for 4way access (XXX Should be refactored) - - motors[motorIndex].enabled = true; } } @@ -696,6 +675,8 @@ static motorVTable_t bbVTable = { .convertExternalToMotor = dshotConvertFromExternal, .convertMotorToExternal = dshotConvertToExternal, .shutdown = bbShutdown, + .isMotorIdle = bbDshotIsMotorIdle, + .requestTelemetry = bbDshotRequestTelemetry, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) @@ -703,14 +684,15 @@ dshotBitbangStatus_e dshotBitbangGetStatus(void) return bbStatus; } -motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t count) +bool dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) { dbgPinLo(0); dbgPinLo(1); - motorPwmProtocol = motorConfig->motorPwmProtocol; - bbDevice.vTable = bbVTable; - motorCount = count; + motorProtocol = motorConfig->motorProtocol; + device->vTable = &bbVTable; + dshotMotorCount = device->count; + bbStatus = DSHOT_BITBANG_STATUS_OK; #ifdef USE_DSHOT_TELEMETRY @@ -719,12 +701,12 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t 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]); - uint8_t output = motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output; + uint8_t output = motorConfig->motorInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output; bbPuPdMode = (output & TIMER_OUTPUT_INVERTED) ? BB_GPIO_PULLDOWN : BB_GPIO_PULLUP; #ifdef USE_DSHOT_TELEMETRY @@ -735,11 +717,10 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t if (!IOIsFreeOrPreinit(io)) { /* not enough motors initialised for the mixer or a break in the motors */ - bbDevice.vTable.write = motorWriteNull; - bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - bbDevice.vTable.updateComplete = motorUpdateCompleteNull; bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT; - return NULL; + device->vTable = NULL; + dshotMotorCount = 0; + return false; } int pinIndex = IO_GPIOPinIdx(io); @@ -756,12 +737,8 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t } else { IOHi(io); } - - // Fill in motors structure for 4way access (TODO: Should be refactored) - motors[motorIndex].io = bbMotors[motorIndex].io; } - - return &bbDevice; + return true; } #endif // USE_DSHOT_BB diff --git a/src/platform/AT32/dshot_bitbang_stdperiph.c b/src/platform/AT32/dshot_bitbang_stdperiph.c index 356e4c29ae..927eae2684 100644 --- a/src/platform/AT32/dshot_bitbang_stdperiph.c +++ b/src/platform/AT32/dshot_bitbang_stdperiph.c @@ -36,11 +36,10 @@ #include "drivers/dma.h" #include "drivers/dma_reqmap.h" #include "drivers/dshot.h" -#include "drivers/dshot_bitbang_impl.h" +#include "dshot_bitbang_impl.h" #include "drivers/dshot_command.h" #include "drivers/motor.h" #include "drivers/nvic.h" -#include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring #include "drivers/time.h" #include "drivers/timer.h" diff --git a/src/platform/AT32/mk/AT32F4.mk b/src/platform/AT32/mk/AT32F4.mk index 759c36ad60..2a0149017e 100644 --- a/src/platform/AT32/mk/AT32F4.mk +++ b/src/platform/AT32/mk/AT32F4.mk @@ -113,7 +113,9 @@ MCU_COMMON_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/dshot_bitbang_decode.c \ drivers/inverter.c \ - drivers/pwm_output_dshot_shared.c \ + common/stm32/pwm_output_dshot_shared.c \ + common/stm32/dshot_dpwm.c \ + common/stm32/dshot_bitbang_shared.c \ $(MIDDLEWARES_DIR)/i2c_application_library/i2c_application.c \ drivers/bus_i2c_timing.c \ drivers/usb_msc_common.c \ @@ -133,6 +135,8 @@ MCU_COMMON_SRC = \ msc/usbd_storage_sd_spi.c SPEED_OPTIMISED_SRC += \ + common/stm32/dshot_bitbang_shared.c \ + common/stm32/pwm_output_dshot_shared.c \ common/stm32/bus_spi_hw.c \ common/stm32/system.c diff --git a/src/platform/AT32/pwm_output_at32bsp.c b/src/platform/AT32/pwm_output_at32bsp.c index 3f67b8676c..382dbd0925 100644 --- a/src/platform/AT32/pwm_output_at32bsp.c +++ b/src/platform/AT32/pwm_output_at32bsp.c @@ -29,15 +29,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 void pwmOCConfig(tmr_type *tim, uint8_t channel, uint16_t value, uint8_t output) { tmr_output_config_type tmr_OCInitStruct; @@ -77,13 +75,7 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, *channel->ccr = 0; } -static FAST_DATA_ZERO_INIT motorDevice_t motorPwmDevice; - -static void pwmWriteUnused(uint8_t index, float value) -{ - UNUSED(index); - UNUSED(value); -} +static FAST_DATA_ZERO_INIT motorDevice_t *pwmMotorDevice; static void pwmWriteStandard(uint8_t index, float value) { @@ -93,7 +85,7 @@ static void pwmWriteStandard(uint8_t index, float value) void pwmShutdownPulsesForAllMotors(void) { - for (int index = 0; index < motorPwmDevice.count; 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; @@ -110,17 +102,23 @@ static motorVTable_t motorPwmVTable; bool pwmEnableMotors(void) { /* check motors can be enabled */ - return (motorPwmVTable.write != &pwmWriteUnused); + return (pwmMotorDevice->vTable); } -bool pwmIsMotorEnabled(uint8_t index) +bool pwmIsMotorEnabled(unsigned index) { return motors[index].enabled; } -static void pwmCompleteOneshotMotorUpdate(void) +static bool useContinuousUpdate = true; + +static void pwmCompleteMotorUpdate(void) { - for (int index = 0; index < motorPwmDevice.count; index++) { + if (useContinuousUpdate) { + return; + } + + for (int index = 0; index < pwmMotorCount; index++) { if (motors[index].forceOverflow) { timerForceOverflow(motors[index].channel.tim); } @@ -148,58 +146,66 @@ static motorVTable_t motorPwmVTable = { .shutdown = pwmShutdownPulsesForAllMotors, .convertExternalToMotor = pwmConvertFromExternal, .convertMotorToExternal = pwmConvertToExternal, + .write = pwmWriteStandard, + .decodeTelemetry = motorDecodeTelemetryNull, + .updateComplete = pwmCompleteMotorUpdate, + .requestTelemetry = NULL, + .isMotorIdle = NULL, }; -motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm) +bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { memset(motors, 0, sizeof(motors)); - motorPwmDevice.vTable = motorPwmVTable; + if (!device) { + return false; + } + + device->vTable = &motorPwmVTable; + pwmMotorDevice = device; + pwmMotorCount = device->count; + useContinuousUpdate = motorConfig->useContinuousUpdate; float sMin = 0; float sLen = 0; - switch (motorConfig->motorPwmProtocol) { + switch (motorConfig->motorProtocol) { default: - case PWM_TYPE_ONESHOT125: + case MOTOR_PROTOCOL_ONESHOT125: sMin = 125e-6f; sLen = 125e-6f; break; - case PWM_TYPE_ONESHOT42: + case MOTOR_PROTOCOL_ONESHOT42: sMin = 42e-6f; sLen = 42e-6f; break; - case PWM_TYPE_MULTISHOT: + case MOTOR_PROTOCOL_MULTISHOT: sMin = 5e-6f; sLen = 20e-6f; break; - case PWM_TYPE_BRUSHED: + case MOTOR_PROTOCOL_BRUSHED: sMin = 0; - useUnsyncedPwm = true; + useContinuousUpdate = true; idlePulse = 0; break; - case PWM_TYPE_STANDARD: + case MOTOR_PROTOCOL_PWM : sMin = 1e-3f; sLen = 1e-3f; - useUnsyncedPwm = true; + useContinuousUpdate = true; idlePulse = 0; break; } - motorPwmDevice.vTable.write = pwmWriteStandard; - motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate; - - 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)); if (timerHardware == NULL) { /* not enough motors initialised for the mixer or a break in the motors */ - motorPwmDevice.vTable.write = &pwmWriteUnused; - motorPwmDevice.vTable.updateComplete = motorUpdateCompleteNull; + device->vTable = NULL; + pwmMotorCount = 0; /* TODO: block arming and add reason system cannot arm */ - return NULL; + return false; } motors[motorIndex].io = IOGetByTag(tag); @@ -209,23 +215,23 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl /* standard PWM outputs */ // margin of safety is 4 periods when unsynced - const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4)); + const unsigned pwmRateHz = useContinuousUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4)); const uint32_t clock = timerClock(timerHardware->tim); /* used to find the desired timer frequency for max resolution */ const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */ const uint32_t hz = clock / prescaler; - const unsigned period = useUnsyncedPwm ? hz / pwmRateHz : 0xffff; + const unsigned period = useContinuousUpdate ? hz / pwmRateHz : 0xffff; /* if brushed then it is the entire length of the period. TODO: this can be moved back to periodMin and periodLen once mixer outputs a 0..1 float value. */ - motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f; + motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); - pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion); + pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); bool timerAlreadyUsed = false; for (int i = 0; i < motorIndex; i++) { @@ -237,8 +243,7 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl motors[motorIndex].forceOverflow = !timerAlreadyUsed; motors[motorIndex].enabled = true; } - - return &motorPwmDevice; + 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 0998db708d..57b88f8efd 100644 --- a/src/platform/AT32/pwm_output_dshot.c +++ b/src/platform/AT32/pwm_output_dshot.c @@ -40,9 +40,9 @@ #include "drivers/pwm_output.h" #include "drivers/dshot.h" -#include "drivers/dshot_dpwm.h" +#include "dshot_dpwm.h" #include "drivers/dshot_command.h" -#include "drivers/pwm_output_dshot_shared.h" +#include "pwm_output_dshot_shared.h" /** * Convert from BF channel to AT32 constants for timer output channels @@ -115,9 +115,9 @@ tmr_channel_select_type toCHSelectType(const uint8_t bfChannel, const bool useNC * Called once for every dshot update if telemetry is being used (not just enabled by #def) * Called from pwm_output_dshot_shared.c pwmTelemetryDecode */ -void dshotEnableChannels(uint8_t motorCount) +void dshotEnableChannels(unsigned motorCount) { - for (int i = 0; i < motorCount; i++) { + for (unsigned i = 0; i < motorCount; i++) { tmr_primary_mode_select(dmaMotors[i].timerHardware->tim, TMR_PRIMARY_SEL_COMPARE); tmr_channel_select_type atCh = toCHSelectType(dmaMotors[i].timerHardware->channel, dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL); @@ -131,7 +131,7 @@ void dshotEnableChannels(uint8_t motorCount) * Set the timer and dma of the specified motor for use as an output * * Called from pwmDshotMotorHardwareConfig in this file and also from - * pwmTelemetryDecode in src/main/drivers/pwm_output_dshot_shared.c + * pwmTelemetryDecode in src/main/common/stm32/pwm_output_dshot_shared.c */ FAST_CODE void pwmDshotSetDirectionOutput( motorDmaOutput_t * const motor @@ -250,7 +250,7 @@ void pwmCompleteDshotMotorUpdate(void) { // If there is a dshot command loaded up, time it correctly with motor update if (!dshotCommandQueueEmpty()) { - if (!dshotCommandOutputIsEnabled(dshotPwmDevice.count)) { + if (!dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } } @@ -361,7 +361,7 @@ FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) } bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, - motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) + motorProtocolTypes_e pwmProtocolType, uint8_t output) { #ifdef USE_DSHOT_TELEMETRY #define OCINIT motor->ocInitStruct @@ -451,7 +451,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m tmr_counter_enable(timer, FALSE); uint32_t prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1); - uint32_t period = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; + uint32_t period = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; tmr_clock_source_div_set(timer, TMR_CLOCK_DIV1); tmr_repetition_counter_set(timer, 0); @@ -523,7 +523,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m DMAINIT.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; DMAINIT.DMA_PeripheralBaseAddr = (uint32_t)&timerHardware->tim->DMAR; - DMAINIT.DMA_BufferSize = (pwmProtocolType == PWM_TYPE_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX + DMAINIT.DMA_BufferSize = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX DMAINIT.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMAINIT.DMA_MemoryInc = DMA_MemoryInc_Enable; DMAINIT.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; @@ -551,7 +551,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m #ifdef USE_DSHOT_TELEMETRY motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 * (16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType); - motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; + motor->timer->outputPeriod = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; pwmDshotSetDirectionOutput(motor); #else pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT); diff --git a/src/platform/SIMULATOR/sitl.c b/src/platform/SIMULATOR/sitl.c index 2325107ae2..f4c9980d63 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" @@ -520,7 +520,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 @@ -537,7 +536,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig) } } -static motorDevice_t motorPwmDevice; // Forward +static motorDevice_t pwmMotorDevice; // Forward pwmOutputPort_t *pwmGetMotors(void) { @@ -556,12 +555,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; } @@ -588,10 +587,10 @@ static void pwmWriteMotorInt(uint8_t index, uint16_t value) static void pwmShutdownPulsesForAllMotors(void) { - motorPwmDevice.enabled = false; + pwmMotorDevice.enabled = false; } -bool pwmIsMotorEnabled(uint8_t index) +bool pwmIsMotorEnabled(unsigned index) { return motors[index].enabled; } @@ -627,27 +626,32 @@ void pwmWriteServo(uint8_t index, float value) } } -static motorDevice_t motorPwmDevice = { - .vTable = { - .postInit = motorPostInitNull, - .convertExternalToMotor = pwmConvertFromExternal, - .convertMotorToExternal = pwmConvertToExternal, - .enable = pwmEnableMotors, - .disable = pwmDisableMotors, - .isMotorEnabled = pwmIsMotorEnabled, - .decodeTelemetry = motorDecodeTelemetryNull, - .write = pwmWriteMotor, - .writeInt = pwmWriteMotorInt, - .updateComplete = pwmCompleteMotorUpdate, - .shutdown = pwmShutdownPulsesForAllMotors, - } +static const motorVTable_t vTable = { + .postInit = motorPostInitNull, + .convertExternalToMotor = pwmConvertFromExternal, + .convertMotorToExternal = pwmConvertToExternal, + .enable = pwmEnableMotors, + .disable = pwmDisableMotors, + .isMotorEnabled = pwmIsMotorEnabled, + .decodeTelemetry = motorDecodeTelemetryNull, + .write = pwmWriteMotor, + .writeInt = pwmWriteMotorInt, + .updateComplete = pwmCompleteMotorUpdate, + .shutdown = pwmShutdownPulsesForAllMotors, + .requestTelemetry = NULL, + .isMotorIdle = NULL, + }; -motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount, bool useUnsyncedPwm) +bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t _idlePulse) { UNUSED(motorConfig); - UNUSED(useUnsyncedPwm); + if (!device) { + return false; + } + device->vTable = &vTable; + const uint8_t motorCount = device->count; printf("Initialized motor count %d\n", motorCount); pwmRawPkt.motorCount = motorCount; @@ -656,11 +660,8 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t _id for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { motors[motorIndex].enabled = true; } - motorPwmDevice.count = motorCount; // Never used, but seemingly a right thing to set it anyways. - motorPwmDevice.initialized = true; - motorPwmDevice.enabled = false; - return &motorPwmDevice; + return true; } // ADC part diff --git a/src/platform/STM32/dshot_bitbang.c b/src/platform/STM32/dshot_bitbang.c index b6d98d3fc8..3cf7db42ce 100644 --- a/src/platform/STM32/dshot_bitbang.c +++ b/src/platform/STM32/dshot_bitbang.c @@ -35,12 +35,11 @@ #include "drivers/dma_reqmap.h" #include "drivers/dshot.h" #include "drivers/dshot_bitbang.h" -#include "drivers/dshot_bitbang_impl.h" +#include "dshot_bitbang_impl.h" #include "drivers/dshot_command.h" #include "drivers/motor.h" #include "drivers/nvic.h" -#include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring -#include "drivers/dshot_dpwm.h" // XXX for motorDmaOutput_t *getMotorDmaOutput(uint8_t index); should go away with refactoring +#include "pwm_output_dshot_shared.h" #include "drivers/dshot_bitbang_decode.h" #include "drivers/time.h" #include "drivers/timer.h" @@ -57,17 +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]; - -static FAST_DATA_ZERO_INIT int motorCount; -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. @@ -134,10 +122,9 @@ const timerHardware_t bbTimerHardware[] = { #endif }; -static FAST_DATA_ZERO_INIT motorDevice_t bbDevice; static FAST_DATA_ZERO_INIT timeUs_t lastSendUs; -static motorPwmProtocolTypes_e motorPwmProtocol; +static motorProtocolTypes_e motorProtocol; // DMA GPIO output buffer formatting @@ -287,15 +274,15 @@ const resourceOwner_t *dshotBitbangTimerGetOwner(const timerHardware_t *timer) // Return frequency of smallest change [state/sec] -static uint32_t getDshotBaseFrequency(motorPwmProtocolTypes_e pwmProtocolType) +static uint32_t getDshotBaseFrequency(motorProtocolTypes_e pwmProtocolType) { switch (pwmProtocolType) { - case(PWM_TYPE_DSHOT600): + case(MOTOR_PROTOCOL_DSHOT600): return MOTOR_DSHOT600_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; - case(PWM_TYPE_DSHOT300): + case(MOTOR_PROTOCOL_DSHOT300): return MOTOR_DSHOT300_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; default: - case(PWM_TYPE_DSHOT150): + case(MOTOR_PROTOCOL_DSHOT150): return MOTOR_DSHOT150_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL; } } @@ -408,7 +395,7 @@ static void bbFindPacerTimer(void) } } -static void bbTimebaseSetup(bbPort_t *bbPort, motorPwmProtocolTypes_e dshotProtocolType) +static void bbTimebaseSetup(bbPort_t *bbPort, motorProtocolTypes_e dshotProtocolType) { uint32_t timerclock = timerClock(bbPort->timhw->tim); @@ -426,7 +413,7 @@ static void bbTimebaseSetup(bbPort_t *bbPort, motorPwmProtocolTypes_e dshotProto // it does not use the timer channel associated with the pin. // -static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output) { // Return if no GPIO is specified if (!io) { @@ -458,10 +445,6 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p } if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) { - bbDevice.vTable.write = motorWriteNull; - bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - bbDevice.vTable.updateComplete = motorUpdateCompleteNull; - return false; } @@ -559,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, @@ -608,11 +591,6 @@ static void bbWriteInt(uint8_t motorIndex, uint16_t value) return; } - // fetch requestTelemetry from motors. Needs to be refactored. - motorDmaOutput_t * const motor = getMotorDmaOutput(motorIndex); - bbmotor->protocolControl.requestTelemetry = motor->protocolControl.requestTelemetry; - motor->protocolControl.requestTelemetry = false; - // If there is a command ready to go overwrite the value and send that instead if (dshotCommandIsProcessing()) { value = dshotCommandGetCurrent(motorIndex); @@ -647,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(bbDevice.count)) { + if (!dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } } @@ -685,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); } @@ -703,7 +681,7 @@ static void bbShutdown(void) return; } -static bool bbIsMotorEnabled(uint8_t index) +static bool bbIsMotorEnabled(unsigned index) { return bbMotors[index].enabled; } @@ -712,21 +690,17 @@ 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, motorPwmProtocol, bbMotors[motorIndex].output)) { + if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) { return; } bbMotors[motorIndex].enabled = true; - - // Fill in motors structure for 4way access (XXX Should be refactored) - - motors[motorIndex].enabled = true; } } -static motorVTable_t bbVTable = { +static const motorVTable_t bbVTable = { .postInit = bbPostInit, .enable = bbEnableMotors, .disable = bbDisableMotors, @@ -740,6 +714,8 @@ static motorVTable_t bbVTable = { .convertExternalToMotor = dshotConvertFromExternal, .convertMotorToExternal = dshotConvertToExternal, .shutdown = bbShutdown, + .isMotorIdle = bbDshotIsMotorIdle, + .requestTelemetry = bbDshotRequestTelemetry, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) @@ -747,14 +723,19 @@ dshotBitbangStatus_e dshotBitbangGetStatus(void) return bbStatus; } -motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t count) +bool dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) { dbgPinLo(0); dbgPinLo(1); - motorPwmProtocol = motorConfig->motorPwmProtocol; - bbDevice.vTable = bbVTable; - motorCount = count; + if (!device || !motorConfig) { + return false; + } + + motorProtocol = motorConfig->motorProtocol; + device->vTable = &bbVTable; + + dshotMotorCount = device->count; bbStatus = DSHOT_BITBANG_STATUS_OK; #ifdef USE_DSHOT_TELEMETRY @@ -763,12 +744,12 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t 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]); - uint8_t output = motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output; + uint8_t output = motorConfig->motorInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output; bbPuPdMode = (output & TIMER_OUTPUT_INVERTED) ? BB_GPIO_PULLDOWN : BB_GPIO_PULLUP; #ifdef USE_DSHOT_TELEMETRY @@ -779,11 +760,10 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t if (!IOIsFreeOrPreinit(io)) { /* not enough motors initialised for the mixer or a break in the motors */ - bbDevice.vTable.write = motorWriteNull; - bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - bbDevice.vTable.updateComplete = motorUpdateCompleteNull; + device->vTable = NULL; + dshotMotorCount = 0; bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT; - return NULL; + return false; } int pinIndex = IO_GPIOPinIdx(io); @@ -804,12 +784,9 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t } else { IOHi(io); } - - // Fill in motors structure for 4way access (XXX Should be refactored) - motors[motorIndex].io = bbMotors[motorIndex].io; } - return &bbDevice; + return true; } #endif // USE_DSHOT_BB diff --git a/src/platform/STM32/dshot_bitbang_ll.c b/src/platform/STM32/dshot_bitbang_ll.c index 2c6014fb97..dffd0ed77c 100644 --- a/src/platform/STM32/dshot_bitbang_ll.c +++ b/src/platform/STM32/dshot_bitbang_ll.c @@ -34,13 +34,14 @@ #include "drivers/dma.h" #include "drivers/dma_reqmap.h" #include "drivers/dshot.h" -#include "drivers/dshot_bitbang_impl.h" +#include "dshot_bitbang_impl.h" #include "drivers/dshot_command.h" #include "drivers/motor.h" #include "drivers/nvic.h" #include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring #include "drivers/time.h" #include "drivers/timer.h" +#include "pwm_output_dshot_shared.h" #include "pg/motor.h" diff --git a/src/platform/STM32/dshot_bitbang_stdperiph.c b/src/platform/STM32/dshot_bitbang_stdperiph.c index bafe2671ce..122f2f5718 100644 --- a/src/platform/STM32/dshot_bitbang_stdperiph.c +++ b/src/platform/STM32/dshot_bitbang_stdperiph.c @@ -35,11 +35,10 @@ #include "drivers/dma.h" #include "drivers/dma_reqmap.h" #include "drivers/dshot.h" -#include "drivers/dshot_bitbang_impl.h" +#include "dshot_bitbang_impl.h" #include "drivers/dshot_command.h" #include "drivers/motor.h" #include "drivers/nvic.h" -#include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring #include "drivers/time.h" #include "drivers/timer.h" diff --git a/src/platform/STM32/mk/STM32F4.mk b/src/platform/STM32/mk/STM32F4.mk index 09ac1e719d..e41e3db33b 100644 --- a/src/platform/STM32/mk/STM32F4.mk +++ b/src/platform/STM32/mk/STM32F4.mk @@ -179,7 +179,6 @@ MCU_COMMON_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/dshot_bitbang_decode.c \ drivers/inverter.c \ - drivers/pwm_output_dshot_shared.c \ STM32/pwm_output_dshot.c \ STM32/adc_stm32f4xx.c \ STM32/bus_i2c_stm32f4xx.c \ @@ -193,7 +192,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 5e035639c2..f68fd69525 100644 --- a/src/platform/STM32/mk/STM32F7.mk +++ b/src/platform/STM32/mk/STM32F7.mk @@ -143,7 +143,6 @@ MCU_COMMON_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/bus_i2c_timing.c \ drivers/dshot_bitbang_decode.c \ - drivers/pwm_output_dshot_shared.c \ STM32/adc_stm32f7xx.c \ STM32/audio_stm32f7xx.c \ STM32/bus_i2c_hal_init.c \ @@ -158,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 \ @@ -189,7 +187,6 @@ SPEED_OPTIMISED_SRC += \ STM32/bus_i2c_hal.c \ STM32/bus_spi_ll.c \ drivers/max7456.c \ - drivers/pwm_output_dshot_shared.c \ STM32/pwm_output_dshot_hal.c \ STM32/exti.c diff --git a/src/platform/STM32/mk/STM32G4.mk b/src/platform/STM32/mk/STM32G4.mk index 2761c42bea..38cf7cdbdd 100644 --- a/src/platform/STM32/mk/STM32G4.mk +++ b/src/platform/STM32/mk/STM32G4.mk @@ -120,7 +120,6 @@ MCU_COMMON_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/bus_i2c_timing.c \ drivers/dshot_bitbang_decode.c \ - drivers/pwm_output_dshot_shared.c \ STM32/adc_stm32g4xx.c \ STM32/bus_i2c_hal_init.c \ STM32/bus_i2c_hal.c \ @@ -136,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 4cfd67a706..90a30cc32b 100644 --- a/src/platform/STM32/mk/STM32H5.mk +++ b/src/platform/STM32/mk/STM32H5.mk @@ -151,7 +151,6 @@ MCU_COMMON_SRC = \ drivers/bus_i2c_timing.c \ drivers/bus_quadspi.c \ drivers/dshot_bitbang_decode.c \ - drivers/pwm_output_dshot_shared.c \ STM32/bus_i2c_hal_init.c \ STM32/bus_i2c_hal.c \ STM32/bus_spi_ll.c \ @@ -164,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 8302a6733b..edcd2c27b8 100644 --- a/src/platform/STM32/mk/STM32H7.mk +++ b/src/platform/STM32/mk/STM32H7.mk @@ -266,7 +266,6 @@ MCU_COMMON_SRC = \ drivers/bus_i2c_timing.c \ drivers/bus_quadspi.c \ drivers/dshot_bitbang_decode.c \ - drivers/pwm_output_dshot_shared.c \ STM32/adc_stm32h7xx.c \ STM32/audio_stm32h7xx.c \ STM32/bus_i2c_hal_init.c \ @@ -285,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 fe3da3c251..a70deb0de2 100644 --- a/src/platform/STM32/mk/STM32_COMMON.mk +++ b/src/platform/STM32/mk/STM32_COMMON.mk @@ -9,7 +9,11 @@ MCU_COMMON_SRC += \ drivers/bus_spi_config.c \ common/stm32/bus_spi_hw.c \ common/stm32/io_impl.c \ - common/stm32/serial_uart_hw.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 += \ drivers/bus_i2c_config.c \ @@ -20,5 +24,8 @@ SIZE_OPTIMISED_SRC += \ 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_dshot.c b/src/platform/STM32/pwm_output_dshot.c index 033a10907c..dcf2bba859 100644 --- a/src/platform/STM32/pwm_output_dshot.c +++ b/src/platform/STM32/pwm_output_dshot.c @@ -42,15 +42,15 @@ #include "drivers/pwm_output.h" #include "drivers/dshot.h" -#include "drivers/dshot_dpwm.h" +#include "dshot_dpwm.h" #include "drivers/dshot_command.h" -#include "drivers/pwm_output_dshot_shared.h" +#include "pwm_output_dshot_shared.h" #ifdef USE_DSHOT_TELEMETRY -void dshotEnableChannels(uint8_t motorCount) +void dshotEnableChannels(unsigned motorCount) { - for (int i = 0; i < motorCount; i++) { + for (unsigned i = 0; i < motorCount; i++) { if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) { TIM_CCxNCmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerHardware->channel, TIM_CCxN_Enable); } else { @@ -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(dshotPwmDevice.count)) { + if (!dshotCommandOutputIsEnabled(pwmMotorCount)) { return; } } @@ -200,7 +200,7 @@ FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) } } -bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output) { #ifdef USE_DSHOT_TELEMETRY #define OCINIT motor->ocInitStruct @@ -299,7 +299,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m TIM_Cmd(timer, DISABLE); TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1); - TIM_TimeBaseStructure.TIM_Period = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; + TIM_TimeBaseStructure.TIM_Period = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; @@ -359,7 +359,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m DMAINIT.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; DMAINIT.DMA_PeripheralBaseAddr = (uint32_t)&timerHardware->tim->DMAR; - DMAINIT.DMA_BufferSize = (pwmProtocolType == PWM_TYPE_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX + DMAINIT.DMA_BufferSize = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX DMAINIT.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMAINIT.DMA_MemoryInc = DMA_MemoryInc_Enable; DMAINIT.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; @@ -396,7 +396,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m #ifdef USE_DSHOT_TELEMETRY motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 * (16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType); - motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; + motor->timer->outputPeriod = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; pwmDshotSetDirectionOutput(motor); #else pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT); diff --git a/src/platform/STM32/pwm_output_dshot_hal.c b/src/platform/STM32/pwm_output_dshot_hal.c index 15d5a31fe2..3e6fe722a3 100644 --- a/src/platform/STM32/pwm_output_dshot_hal.c +++ b/src/platform/STM32/pwm_output_dshot_hal.c @@ -33,13 +33,13 @@ #include "drivers/dma.h" #include "drivers/dma_reqmap.h" #include "drivers/dshot.h" -#include "drivers/dshot_dpwm.h" +#include "dshot_dpwm.h" #include "drivers/dshot_command.h" #include "drivers/io.h" #include "drivers/nvic.h" #include "drivers/motor.h" #include "drivers/pwm_output.h" -#include "drivers/pwm_output_dshot_shared.h" +#include "pwm_output_dshot_shared.h" #include "drivers/rcc.h" #include "drivers/time.h" #include "drivers/timer.h" @@ -47,9 +47,9 @@ #ifdef USE_DSHOT_TELEMETRY -void dshotEnableChannels(uint8_t motorCount) +void dshotEnableChannels(unsigned motorCount) { - for (int i = 0; i < motorCount; i++) { + for (unsigned i = 0; i < motorCount; i++) { if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) { LL_EX_TIM_CC_EnableNChannel(dmaMotors[i].timerHardware->tim, dmaMotors[i].llChannel); } else { @@ -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(dshotPwmDevice.count)) { + if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotMotorCount)) { return; } @@ -200,7 +200,7 @@ FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) } } -bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) +bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output) { #ifdef USE_DSHOT_TELEMETRY #define OCINIT motor->ocInitStruct @@ -297,7 +297,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m LL_TIM_DisableCounter(timer); init.Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1); - init.Autoreload = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1; + init.Autoreload = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1; init.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; init.RepetitionCounter = 0; init.CounterMode = LL_TIM_COUNTERMODE_UP; @@ -391,7 +391,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m DMAINIT.MemBurst = LL_DMA_MBURST_SINGLE; DMAINIT.PeriphBurst = LL_DMA_PBURST_SINGLE; #endif - DMAINIT.NbData = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; + DMAINIT.NbData = pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; DMAINIT.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; DMAINIT.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; DMAINIT.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD; @@ -409,7 +409,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m #ifdef USE_DSHOT_TELEMETRY motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 * ( 16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType); - motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; + motor->timer->outputPeriod = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1; pwmDshotSetDirectionOutput(motor); #else pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT); diff --git a/src/platform/STM32/pwm_output.c b/src/platform/STM32/pwm_output_hw.c similarity index 82% rename from src/platform/STM32/pwm_output.c rename to src/platform/STM32/pwm_output_hw.c index 3def265a14..67284840cb 100644 --- a/src/platform/STM32/pwm_output.c +++ b/src/platform/STM32/pwm_output_hw.c @@ -28,14 +28,13 @@ #ifdef USE_PWM_OUTPUT #include "drivers/io.h" -#include "drivers/motor.h" #include "drivers/pwm_output.h" #include "drivers/time.h" #include "drivers/timer.h" -#include "pg/motor.h" +#include "drivers/motor_impl.h" -FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; +static bool useContinuousUpdate = true; static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) { @@ -108,14 +107,6 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, *channel->ccr = 0; } -static FAST_DATA_ZERO_INIT motorDevice_t motorPwmDevice; - -static void pwmWriteUnused(uint8_t index, float value) -{ - UNUSED(index); - UNUSED(value); -} - static void pwmWriteStandard(uint8_t index, float value) { /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */ @@ -124,7 +115,7 @@ static void pwmWriteStandard(uint8_t index, float value) void pwmShutdownPulsesForAllMotors(void) { - for (int index = 0; index < motorPwmDevice.count; 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; @@ -137,21 +128,24 @@ void pwmDisableMotors(void) pwmShutdownPulsesForAllMotors(); } -static motorVTable_t motorPwmVTable; bool pwmEnableMotors(void) { /* check motors can be enabled */ - return (motorPwmVTable.write != &pwmWriteUnused); + return pwmMotorCount > 0; } -bool pwmIsMotorEnabled(uint8_t index) +bool pwmIsMotorEnabled(unsigned index) { return motors[index].enabled; } -static void pwmCompleteOneshotMotorUpdate(void) +static void pwmCompleteMotorUpdate(void) { - for (int index = 0; index < motorPwmDevice.count; index++) { + if (useContinuousUpdate) { + return; + } + + for (int index = 0; pwmMotorCount; index++) { if (motors[index].forceOverflow) { timerForceOverflow(motors[index].channel.tim); } @@ -171,66 +165,69 @@ static uint16_t pwmConvertToExternal(float motorValue) return (uint16_t)motorValue; } -static motorVTable_t motorPwmVTable = { - .postInit = motorPostInitNull, +static const motorVTable_t motorPwmVTable = { + .postInit = NULL, .enable = pwmEnableMotors, .disable = pwmDisableMotors, .isMotorEnabled = pwmIsMotorEnabled, .shutdown = pwmShutdownPulsesForAllMotors, .convertExternalToMotor = pwmConvertFromExternal, .convertMotorToExternal = pwmConvertToExternal, + .write = pwmWriteStandard, + .decodeTelemetry = NULL, + .updateComplete = pwmCompleteMotorUpdate, + .requestTelemetry = NULL, + .isMotorIdle = NULL, }; -motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm) +bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse) { memset(motors, 0, sizeof(motors)); - motorPwmDevice.vTable = motorPwmVTable; + pwmMotorCount = device->count; + device->vTable = &motorPwmVTable; + useContinuousUpdate = motorConfig->useContinuousUpdate; float sMin = 0; float sLen = 0; - switch (motorConfig->motorPwmProtocol) { + switch (motorConfig->motorProtocol) { default: - case PWM_TYPE_ONESHOT125: + case MOTOR_PROTOCOL_ONESHOT125: sMin = 125e-6f; sLen = 125e-6f; break; - case PWM_TYPE_ONESHOT42: + case MOTOR_PROTOCOL_ONESHOT42: sMin = 42e-6f; sLen = 42e-6f; break; - case PWM_TYPE_MULTISHOT: + case MOTOR_PROTOCOL_MULTISHOT: sMin = 5e-6f; sLen = 20e-6f; break; - case PWM_TYPE_BRUSHED: + case MOTOR_PROTOCOL_BRUSHED: sMin = 0; - useUnsyncedPwm = true; + useContinuousUpdate = true; idlePulse = 0; break; - case PWM_TYPE_STANDARD: + case MOTOR_PROTOCOL_PWM : sMin = 1e-3f; sLen = 1e-3f; - useUnsyncedPwm = true; + useContinuousUpdate = true; idlePulse = 0; break; } - motorPwmDevice.vTable.write = pwmWriteStandard; - motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate; - - 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)); if (timerHardware == NULL) { /* not enough motors initialised for the mixer or a break in the motors */ - motorPwmDevice.vTable.write = &pwmWriteUnused; - motorPwmDevice.vTable.updateComplete = motorUpdateCompleteNull; + device->vTable = NULL; + pwmMotorCount = 0; /* TODO: block arming and add reason system cannot arm */ - return NULL; + return false; } motors[motorIndex].io = IOGetByTag(tag); @@ -240,23 +237,23 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl /* standard PWM outputs */ // margin of safety is 4 periods when unsynced - const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4)); + const unsigned pwmRateHz = useContinuousUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4)); const uint32_t clock = timerClock(timerHardware->tim); /* used to find the desired timer frequency for max resolution */ const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */ const uint32_t hz = clock / prescaler; - const unsigned period = useUnsyncedPwm ? hz / pwmRateHz : 0xffff; + const unsigned period = useContinuousUpdate ? hz / pwmRateHz : 0xffff; /* if brushed then it is the entire length of the period. TODO: this can be moved back to periodMin and periodLen once mixer outputs a 0..1 float value. */ - motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f; + motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f; motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); - pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion); + pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion); bool timerAlreadyUsed = false; for (int i = 0; i < motorIndex; i++) { @@ -269,7 +266,7 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl motors[motorIndex].enabled = true; } - return &motorPwmDevice; + return true; } pwmOutputPort_t *pwmGetMotors(void) diff --git a/src/platform/STM32/target/STM32F411/target.h b/src/platform/STM32/target/STM32F411/target.h index 0c93771a80..3eb65afac9 100644 --- a/src/platform/STM32/target/STM32F411/target.h +++ b/src/platform/STM32/target/STM32F411/target.h @@ -57,7 +57,7 @@ #define USE_BEEPER -#define DEFAULT_MOTOR_DSHOT_SPEED PWM_TYPE_DSHOT300 +#define DEFAULT_MOTOR_DSHOT_SPEED MOTOR_PROTOCOL_DSHOT300 #ifdef USE_SDCARD #define USE_SDCARD_SPI 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/main/drivers/dshot_bitbang_impl.h b/src/platform/common/stm32/dshot_bitbang_impl.h similarity index 96% rename from src/main/drivers/dshot_bitbang_impl.h rename to src/platform/common/stm32/dshot_bitbang_impl.h index 0139c24257..4e062e86c2 100644 --- a/src/main/drivers/dshot_bitbang_impl.h +++ b/src/platform/common/stm32/dshot_bitbang_impl.h @@ -20,8 +20,14 @@ #pragma once +#include "platform.h" + #include "common/time.h" -#include "drivers/motor.h" + +#include "drivers/timer.h" +#include "drivers/motor_types.h" +#include "drivers/dshot.h" +#include "pg/motor.h" #define USE_DMA_REGISTER_CACHE @@ -223,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, @@ -275,3 +282,6 @@ void bbDMA_Cmd(bbPort_t *bbPort, confirm_state NewState); void bbDMA_Cmd(bbPort_t *bbPort, FunctionalState NewState); #endif int bbDMA_Count(bbPort_t *bbPort); + +void bbDshotRequestTelemetry(unsigned motorIndex); +bool bbDshotIsMotorIdle(unsigned motorIndex); diff --git a/src/platform/common/stm32/dshot_bitbang_shared.c b/src/platform/common/stm32/dshot_bitbang_shared.c new file mode 100644 index 0000000000..c5152d59f3 --- /dev/null +++ b/src/platform/common/stm32/dshot_bitbang_shared.c @@ -0,0 +1,68 @@ +/* + * 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 "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)) { + return; + } + bbMotor_t *const bbmotor = &bbMotors[motorIndex]; + + if (!bbmotor->configured) { + return; + } + bbmotor->protocolControl.requestTelemetry = true; +} + +bool bbDshotIsMotorIdle(unsigned motorIndex) +{ + if (motorIndex >= ARRAYLEN(bbMotors)) { + return false; + } + + 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 diff --git a/src/main/drivers/dshot_dpwm.c b/src/platform/common/stm32/dshot_dpwm.c similarity index 79% rename from src/main/drivers/dshot_dpwm.c rename to src/platform/common/stm32/dshot_dpwm.c index a7ab462f51..cee7aa6003 100644 --- a/src/main/drivers/dshot_dpwm.c +++ b/src/platform/common/stm32/dshot_dpwm.c @@ -30,9 +30,10 @@ #ifdef USE_DSHOT #include "drivers/pwm_output.h" +#include "pwm_output_dshot_shared.h" #include "drivers/dshot.h" -#include "drivers/dshot_dpwm.h" -#include "drivers/motor.h" +#include "dshot_dpwm.h" +#include "drivers/motor_impl.h" #include "pg/motor.h" @@ -79,17 +80,17 @@ FAST_CODE_NOINLINE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, return PROSHOT_DMA_BUFFER_SIZE; } -uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) +uint32_t getDshotHz(motorProtocolTypes_e pwmProtocolType) { switch (pwmProtocolType) { - case(PWM_TYPE_PROSHOT1000): + case(MOTOR_PROTOCOL_PROSHOT1000): return MOTOR_PROSHOT1000_HZ; - case(PWM_TYPE_DSHOT600): + case(MOTOR_PROTOCOL_DSHOT600): return MOTOR_DSHOT600_HZ; - case(PWM_TYPE_DSHOT300): + case(MOTOR_PROTOCOL_DSHOT300): return MOTOR_DSHOT300_HZ; default: - case(PWM_TYPE_DSHOT150): + case(MOTOR_PROTOCOL_DSHOT150): return MOTOR_DSHOT150_HZ; } } @@ -110,7 +111,7 @@ static void dshotPwmDisableMotors(void) static bool dshotPwmEnableMotors(void) { - for (int i = 0; i < dshotPwmDevice.count; 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); @@ -120,7 +121,7 @@ static bool dshotPwmEnableMotors(void) return true; } -static bool dshotPwmIsMotorEnabled(uint8_t index) +static bool dshotPwmIsMotorEnabled(unsigned index) { return motors[index].enabled; } @@ -135,41 +136,37 @@ static FAST_CODE void dshotWrite(uint8_t index, float value) pwmWriteDshotInt(index, lrintf(value)); } -static motorVTable_t dshotPwmVTable = { +static const motorVTable_t dshotPwmVTable = { .postInit = motorPostInitNull, .enable = dshotPwmEnableMotors, .disable = dshotPwmDisableMotors, .isMotorEnabled = dshotPwmIsMotorEnabled, - .decodeTelemetry = motorDecodeTelemetryNull, // May be updated after copying + .decodeTelemetry = pwmTelemetryDecode, .write = dshotWrite, .writeInt = dshotWriteInt, .updateComplete = pwmCompleteDshotMotorUpdate, .convertExternalToMotor = dshotConvertFromExternal, .convertMotorToExternal = dshotConvertToExternal, .shutdown = dshotPwmShutdown, + .requestTelemetry = pwmDshotRequestTelemetry, + .isMotorIdle = pwmDshotIsMotorIdle, }; -FAST_DATA_ZERO_INIT motorDevice_t dshotPwmDevice; - -motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm) +bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig) { - UNUSED(idlePulse); - UNUSED(useUnsyncedPwm); - - dshotPwmDevice.vTable = dshotPwmVTable; - + device->vTable = &dshotPwmVTable; + dshotMotorCount = device->count; #ifdef USE_DSHOT_TELEMETRY useDshotTelemetry = motorConfig->useDshotTelemetry; - dshotPwmDevice.vTable.decodeTelemetry = pwmTelemetryDecode; #endif - switch (motorConfig->motorPwmProtocol) { - case PWM_TYPE_PROSHOT1000: + switch (motorConfig->motorProtocol) { + case MOTOR_PROTOCOL_PROSHOT1000: loadDmaBuffer = loadDmaBufferProshot; break; - case PWM_TYPE_DSHOT600: - case PWM_TYPE_DSHOT300: - case PWM_TYPE_DSHOT150: + case MOTOR_PROTOCOL_DSHOT600: + case MOTOR_PROTOCOL_DSHOT300: + case MOTOR_PROTOCOL_DSHOT150: loadDmaBuffer = loadDmaBufferDshot; #ifdef USE_DSHOT_DMAR useBurstDshot = motorConfig->useBurstDshot == DSHOT_DMAR_ON || @@ -178,7 +175,7 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl 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)); @@ -190,8 +187,8 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl if (pwmDshotMotorHardwareConfig(timerHardware, motorIndex, reorderedMotorIndex, - motorConfig->motorPwmProtocol, - motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output)) { + motorConfig->motorProtocol, + motorConfig->motorInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output)) { motors[motorIndex].enabled = true; continue; @@ -199,14 +196,12 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl } /* not enough motors initialised for the mixer or a break in the motors */ - dshotPwmDevice.vTable.write = motorWriteNull; - dshotPwmDevice.vTable.updateComplete = motorUpdateCompleteNull; - + dshotMotorCount = 0; /* TODO: block arming and add reason system cannot arm */ - return NULL; + return false; } - return &dshotPwmDevice; + return true; } #endif // USE_DSHOT diff --git a/src/main/drivers/dshot_dpwm.h b/src/platform/common/stm32/dshot_dpwm.h similarity index 91% rename from src/main/drivers/dshot_dpwm.h rename to src/platform/common/stm32/dshot_dpwm.h index 1c6a7400fb..0cdf3cf41b 100644 --- a/src/main/drivers/dshot_dpwm.h +++ b/src/platform/common/stm32/dshot_dpwm.h @@ -23,7 +23,7 @@ #pragma once #include "drivers/dshot.h" -#include "drivers/motor.h" +#include "drivers/motor_types.h" // Timer clock frequency for the dshot speeds #define MOTOR_DSHOT600_HZ MHZ_TO_HZ(12) @@ -47,10 +47,7 @@ extern FAST_DATA_ZERO_INIT loadDmaBufferFn *loadDmaBuffer; uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet); uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet); -uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); - -struct motorDevConfig_s; -motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm); +uint32_t getDshotHz(motorProtocolTypes_e pwmProtocolType); /* Motor DMA related, moved from pwm_output.h */ @@ -70,7 +67,7 @@ motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint1 #elif defined(STM32F7) #define DSHOT_DMA_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT #else -#define DSHOT_DMA_BUFFER_ATTRIBUTE // None +#define DSHOT_DMA_BUFFER_ATTRIBUTE /* Empty */ #endif #if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F435) || defined(APM32F4) @@ -156,15 +153,13 @@ typedef struct motorDmaOutput_s { DSHOT_DMA_BUFFER_UNIT *dmaBuffer; } motorDmaOutput_t; -motorDmaOutput_t *getMotorDmaOutput(uint8_t index); +motorDmaOutput_t *getMotorDmaOutput(unsigned index); void pwmWriteDshotInt(uint8_t index, uint16_t value); -bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); +bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output); #ifdef USE_DSHOT_TELEMETRY bool pwmTelemetryDecode(void); #endif void pwmCompleteDshotMotorUpdate(void); extern bool useBurstDshot; - -extern motorDevice_t dshotPwmDevice; diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/platform/common/stm32/pwm_output_dshot_shared.c similarity index 89% rename from src/main/drivers/pwm_output_dshot_shared.c rename to src/platform/common/stm32/pwm_output_dshot_shared.c index 998112becf..ccf2ab661d 100644 --- a/src/main/drivers/pwm_output_dshot_shared.c +++ b/src/platform/common/stm32/pwm_output_dshot_shared.c @@ -40,15 +40,16 @@ #include "stm32f4xx.h" #endif -#include "pwm_output.h" +#include "drivers/pwm_output.h" #include "drivers/dshot.h" -#include "drivers/dshot_dpwm.h" +#include "dshot_dpwm.h" #include "drivers/dshot_command.h" #include "drivers/motor.h" #include "pwm_output_dshot_shared.h" FAST_DATA_ZERO_INIT uint8_t dmaMotorTimerCount = 0; + #ifdef STM32F7 FAST_DATA_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; FAST_DATA_ZERO_INIT motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; @@ -60,14 +61,31 @@ motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; #ifdef USE_DSHOT_TELEMETRY FAST_DATA_ZERO_INIT uint32_t inputStampUs; -FAST_DATA_ZERO_INIT dshotDMAHandlerCycleCounters_t dshotDMAHandlerCycleCounters; +FAST_DATA_ZERO_INIT dshotTelemetryCycleCounters_t dshotDMAHandlerCycleCounters; #endif -motorDmaOutput_t *getMotorDmaOutput(uint8_t index) +motorDmaOutput_t *getMotorDmaOutput(unsigned index) { + if (index >= ARRAYLEN(dmaMotors)) { + return NULL; + } return &dmaMotors[index]; } +bool pwmDshotIsMotorIdle(unsigned index) +{ + const motorDmaOutput_t *motor = getMotorDmaOutput(index); + return motor && motor->protocolControl.value == 0; +} + +void pwmDshotRequestTelemetry(unsigned index) +{ + motorDmaOutput_t * const motor = getMotorDmaOutput(index); + if (motor) { + motor->protocolControl.requestTelemetry = true; + } +} + uint8_t getTimerIndex(TIM_TypeDef *timer) { for (int i = 0; i < dmaMotorTimerCount; i++) { @@ -127,12 +145,12 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value) #else xDMA_SetCurrDataCounter(motor->dmaRef, bufferSize); - // XXX we can remove this ifdef if we add a new macro for the TRUE/ENABLE constants - #ifdef AT32F435 +// XXX we can remove this ifdef if we add a new macro for the TRUE/ENABLE constants +#ifdef AT32F435 xDMA_Cmd(motor->dmaRef, TRUE); - #else +#else xDMA_Cmd(motor->dmaRef, ENABLE); - #endif +#endif #endif // USE_FULL_LL_DRIVER } @@ -141,7 +159,7 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value) #ifdef USE_DSHOT_TELEMETRY -void dshotEnableChannels(uint8_t motorCount); +void dshotEnableChannels(unsigned motorCount); static uint32_t decodeTelemetryPacket(const uint32_t buffer[], uint32_t count) { @@ -191,13 +209,15 @@ static uint32_t decodeTelemetryPacket(const uint32_t buffer[], uint32_t count) #endif -#ifdef USE_DSHOT_TELEMETRY /** * Process dshot telemetry packets before switching the channels back to outputs * */ FAST_CODE_NOINLINE bool pwmTelemetryDecode(void) { +#ifndef USE_DSHOT_TELEMETRY + return true; +#else if (!useDshotTelemetry) { return true; } @@ -207,7 +227,7 @@ FAST_CODE_NOINLINE bool pwmTelemetryDecode(void) #endif const timeUs_t currentUs = micros(); - for (int i = 0; i < dshotPwmDevice.count; i++) { + for (int i = 0; i < dshotMotorCount; i++) { timeDelta_t usSinceInput = cmpTimeUs(currentUs, inputStampUs); if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) { return false; @@ -257,9 +277,9 @@ FAST_CODE_NOINLINE bool pwmTelemetryDecode(void) dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED; inputStampUs = 0; - dshotEnableChannels(dshotPwmDevice.count); + dshotEnableChannels(dshotMotorCount); return true; +#endif // USE_DSHOT_TELEMETRY } -#endif // USE_DSHOT_TELEMETRY #endif // USE_DSHOT diff --git a/src/main/drivers/pwm_output_dshot_shared.h b/src/platform/common/stm32/pwm_output_dshot_shared.h similarity index 69% rename from src/main/drivers/pwm_output_dshot_shared.h rename to src/platform/common/stm32/pwm_output_dshot_shared.h index 7c282d5008..38f226b305 100644 --- a/src/main/drivers/pwm_output_dshot_shared.h +++ b/src/platform/common/stm32/pwm_output_dshot_shared.h @@ -20,32 +20,17 @@ #ifdef USE_DSHOT +#include "drivers/dshot.h" +#include "dshot_dpwm.h" + extern FAST_DATA_ZERO_INIT uint8_t dmaMotorTimerCount; -#if defined(STM32F7) || defined(STM32H7) + extern FAST_DATA_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; extern FAST_DATA_ZERO_INIT motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; -#else -extern motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; -extern motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; -#endif - -#ifdef USE_DSHOT_TELEMETRY -extern uint32_t readDoneCount; - -FAST_DATA_ZERO_INIT extern uint32_t inputStampUs; - -typedef struct dshotDMAHandlerCycleCounters_s { - uint32_t irqAt; - uint32_t changeDirectionCompletedAt; -} dshotDMAHandlerCycleCounters_t; - -FAST_DATA_ZERO_INIT extern dshotDMAHandlerCycleCounters_t dshotDMAHandlerCycleCounters; - -#endif uint8_t getTimerIndex(TIM_TypeDef *timer); -motorDmaOutput_t *getMotorDmaOutput(uint8_t index); -void dshotEnableChannels(uint8_t motorCount); +motorDmaOutput_t *getMotorDmaOutput(unsigned index); +void dshotEnableChannels(unsigned motorCount); #ifdef USE_DSHOT_TELEMETRY void pwmDshotSetDirectionOutput( @@ -59,6 +44,8 @@ void pwmDshotSetDirectionOutput( #endif ); +void pwmDshotRequestTelemetry(unsigned index); +bool pwmDshotIsMotorIdle(unsigned index); bool pwmTelemetryDecode(void); #endif diff --git a/src/test/unit/pg_unittest.cc b/src/test/unit/pg_unittest.cc index 23ce0c7fb8..8d0fd935db 100644 --- a/src/test/unit/pg_unittest.cc +++ b/src/test/unit/pg_unittest.cc @@ -37,9 +37,9 @@ PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .dev = { .motorPwmRate = 400, - .motorPwmProtocol = 0, - .motorPwmInversion = 0, - .useUnsyncedPwm = 0, + .motorProtocol = 0, + .motorInversion = 0, + .useContinuousUpdate = 0, .useBurstDshot = 0, .useDshotTelemetry = 0, .useDshotEdt = 0,