diff --git a/mk/source.mk b/mk/source.mk index 9b981c05cb..33ccb4c2c0 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 \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index c8f359b924..625c842550 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1719,8 +1719,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.useUnsyncedUpdate); + 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 12dd19cd87..c583429a42 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 ebc5b894e4..cd9394f4ae 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -934,10 +934,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.useUnsyncedUpdate) }, + { 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..c145c78e13 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_STANDARD) && (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.useUnsyncedUpdate = 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_STANDARD: 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.useUnsyncedUpdate) { 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_STANDARD) { 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 1063ae4eef..28451335d1 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.h b/src/main/drivers/dshot.h index 40f592d832..f87bc11a15 100644 --- a/src/main/drivers/dshot.h +++ b/src/main/drivers/dshot.h @@ -26,6 +26,7 @@ #include "common/time.h" #include "pg/motor.h" +#include "drivers/motor_types.h" #define DSHOT_MIN_THROTTLE (48) #define DSHOT_MAX_THROTTLE (2047) @@ -109,6 +110,23 @@ 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 + +struct motorDevConfig_s; +motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate); + extern dshotTelemetryState_t dshotTelemetryState; #ifdef USE_DSHOT_TELEMETRY_STATS diff --git a/src/main/drivers/dshot_command.c b/src/main/drivers/dshot_command.c index a66e0454e6..aca08ce337 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; } } @@ -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); } 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..3d64d07ebb 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 @@ -57,11 +57,11 @@ void motorShutdown(void) 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_STANDARD: + 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; @@ -125,6 +125,17 @@ void motorWriteAll(float *values) #endif } +void motorRequestTelemetry(uint8_t index) +{ + if (index > motorDevice->count - 1) { + return; + } + + if (motorDevice->vTable.requestTelemetry) { + motorDevice->vTable.requestTelemetry(index); + } +} + unsigned motorDeviceCount(void) { return motorDevice->count; @@ -158,20 +169,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_STANDARD: + 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; @@ -240,7 +251,12 @@ static void motorDisableNull(void) static bool motorIsEnabledNull(uint8_t index) { UNUSED(index); + return false; +} +static bool motorIsIdleNull(uint8_t index) +{ + UNUSED(index); return false; } @@ -293,6 +309,7 @@ static const motorVTable_t motorNullVTable = { .convertExternalToMotor = motorConvertFromExternalNull, .convertMotorToExternal = motorConvertToExternalNull, .shutdown = motorShutdownNull, + .isMotorIdle = motorIsIdleNull, }; static motorDevice_t motorNullDevice = { @@ -318,7 +335,7 @@ bool isMotorProtocolBidirDshot(void) void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, uint8_t motorCount) { #if defined(USE_PWM_OUTPUT) || defined(USE_DSHOT) - bool useUnsyncedPwm = motorDevConfig->useUnsyncedPwm; + bool useUnsyncedUpdate = motorDevConfig->useUnsyncedUpdate; #else UNUSED(idlePulse); UNUSED(motorDevConfig); @@ -328,7 +345,7 @@ void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, ui do { if (!isMotorProtocolDshot()) { #ifdef USE_PWM_OUTPUT - motorDevice = motorPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm); + motorDevice = motorPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedUpdate); #endif break; } @@ -339,7 +356,7 @@ void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, ui break; } #endif - motorDevice = dshotPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm); + motorDevice = dshotPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedUpdate); #endif } while(0); } @@ -390,6 +407,11 @@ bool motorIsMotorEnabled(uint8_t index) return motorDevice->vTable.isMotorEnabled(index); } +bool motorIsMotorIdle(uint8_t index) +{ + return motorDevice->vTable.isMotorIdle ? motorDevice->vTable.isMotorIdle(index) : false; +} + #ifdef USE_DSHOT timeMs_t motorGetMotorEnableTimeMs(void) { @@ -402,10 +424,10 @@ 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); + (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->motorPwmProtocol != PWM_TYPE_PROSHOT1000); + (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000); #endif } #endif diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h index 2203ce7c2b..4cf936e2b5 100644 --- a/src/main/drivers/motor.h +++ b/src/main/drivers/motor.h @@ -26,48 +26,7 @@ #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; - -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; +#include "drivers/motor_types.h" void motorPostInitNull(); void motorWriteNull(uint8_t index, float value); @@ -76,6 +35,7 @@ void motorUpdateCompleteNull(void); void motorPostInit(); void motorWriteAll(float *values); +void motorRequestTelemetry(uint8_t index); void motorInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3DHigh, float *deadbandMotor3DLow); @@ -96,6 +56,7 @@ void motorEnable(void); float motorEstimateMaxRpm(void); bool motorIsEnabled(void); bool motorIsMotorEnabled(uint8_t index); +bool motorIsMotorIdle(uint8_t index); timeMs_t motorGetMotorEnableTimeMs(void); void motorShutdown(void); // Replaces stopPwmAllMotors diff --git a/src/main/drivers/motor_types.h b/src/main/drivers/motor_types.h new file mode 100644 index 0000000000..764e3150ee --- /dev/null +++ b/src/main/drivers/motor_types.h @@ -0,0 +1,72 @@ +/* + * 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_STANDARD = 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)(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); + bool (*isMotorIdle)(uint8_t index); + + // Digital commands + void (*requestTelemetry)(uint8_t index); +} motorVTable_t; + +typedef struct motorDevice_s { + motorVTable_t vTable; + uint8_t count; + bool initialized; + bool enabled; + timeMs_t motorEnableTimeMs; +} motorDevice_t; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 21b7de0078..210140105e 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -29,16 +29,9 @@ #include "drivers/motor.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 - #define PWM_TIMER_1MHZ MHZ_TO_HZ(1) +// TODO: move the implementation defintions to impl header (platform) struct timerHardware_s; typedef struct { 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 e6e90800fe..84abb8f4e1 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" @@ -545,7 +544,7 @@ void init(void) if (featureIsEnabled(FEATURE_3D)) { idlePulse = flight3DConfig()->neutral3d; } - if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { + if (motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_BRUSHED) { idlePulse = 0; // brushed motors } #ifdef USE_MOTOR 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 b0fa8fc43b..c52b5dea2d 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 891a5cf7da..35644d777e 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1879,12 +1879,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.useUnsyncedUpdate); + 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); @@ -3087,8 +3087,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.useUnsyncedUpdate = sbufReadU8(src); + motorConfigMutable()->dev.motorProtocol = sbufReadU8(src); motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src); if (sbufBytesRemaining(src) >= 2) { motorConfigMutable()->motorIdle = sbufReadU16(src); @@ -3097,7 +3097,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..a5ef36789e 100644 --- a/src/main/pg/motor.c +++ b/src/main/pg/motor.c @@ -25,12 +25,20 @@ #ifdef USE_MOTOR -#include "drivers/pwm_output.h" +#include "drivers/motor_types.h" #include "pg/pg.h" #include "pg/pg_ids.h" #include "pg/motor.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 + #if !defined(DEFAULT_DSHOT_BITBANG) #define DEFAULT_DSHOT_BITBANG DSHOT_BITBANG_AUTO #endif @@ -53,19 +61,19 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig) { #ifdef 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.useUnsyncedUpdate = true; #else motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; #ifndef USE_DSHOT - if (motorConfig->dev.motorPwmProtocol == PWM_TYPE_STANDARD) { - motorConfig->dev.useUnsyncedPwm = true; + if (motorConfig->dev.motorProtocol == MOTOR_PROTOCOL_STANDARD) { + motorConfig->dev.useUnsyncedUpdate = true; } - motorConfig->dev.motorPwmProtocol = PWM_TYPE_DISABLED; + motorConfig->dev.motorProtocol = MOTOR_PROTOCOL_DISABLED; #elif defined(DEFAULT_MOTOR_DSHOT_SPEED) - motorConfig->dev.motorPwmProtocol = DEFAULT_MOTOR_DSHOT_SPEED; + motorConfig->dev.motorProtocol = DEFAULT_MOTOR_DSHOT_SPEED; #else - motorConfig->dev.motorPwmProtocol = PWM_TYPE_DSHOT600; + motorConfig->dev.motorProtocol = MOTOR_PROTOCOL_DSHOT600; #endif // USE_DSHOT #endif // BRUSHED_MOTORS diff --git a/src/main/pg/motor.h b/src/main/pg/motor.h index c4fca53b2f..d13b345671 100644 --- a/src/main/pg/motor.h +++ b/src/main/pg/motor.h @@ -25,6 +25,14 @@ #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 + typedef enum { DSHOT_BITBANGED_TIMER_AUTO = 0, DSHOT_BITBANGED_TIMER_TIM1, @@ -44,9 +52,9 @@ typedef enum { 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; + uint8_t motorProtocol; // Pwm Protocol + uint8_t motorInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation + uint8_t useUnsyncedUpdate; 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 d128aec8f2..c7af249855 100644 --- a/src/platform/APM32/dshot_bitbang.c +++ b/src/platform/APM32/dshot_bitbang.c @@ -35,15 +35,16 @@ #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 "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" @@ -108,7 +109,7 @@ const timerHardware_t bbTimerHardware[] = { 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 @@ -258,15 +259,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; } } @@ -379,7 +380,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); @@ -397,7 +398,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) { @@ -677,7 +678,7 @@ static void bbPostInit(void) for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { - if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorPwmProtocol, bbMotors[motorIndex].output)) { + if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) { return; } @@ -703,6 +704,8 @@ static motorVTable_t bbVTable = { .convertExternalToMotor = dshotConvertFromExternal, .convertMotorToExternal = dshotConvertToExternal, .shutdown = bbShutdown, + .isMotorIdle = bbDshotIsMotorIdle, + .requestTelemetry = bbDshotRequestTelemetry, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) @@ -715,7 +718,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t dbgPinLo(0); dbgPinLo(1); - motorPwmProtocol = motorConfig->motorPwmProtocol; + motorProtocol = motorConfig->motorProtocol; bbDevice.vTable = bbVTable; motorCount = count; bbStatus = DSHOT_BITBANG_STATUS_OK; @@ -731,7 +734,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t 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 diff --git a/src/platform/APM32/dshot_bitbang_ddl.c b/src/platform/APM32/dshot_bitbang_ddl.c index f26a21966f..47edec5ee5 100644 --- a/src/platform/APM32/dshot_bitbang_ddl.c +++ b/src/platform/APM32/dshot_bitbang_ddl.c @@ -34,7 +34,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 c0f40dd948..b95d4e42de 100644 --- a/src/platform/APM32/pwm_output_apm32.c +++ b/src/platform/APM32/pwm_output_apm32.c @@ -153,7 +153,7 @@ static motorVTable_t motorPwmVTable = { .convertMotorToExternal = pwmConvertToExternal, }; -motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm) +motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate) { memset(motors, 0, sizeof(motors)); @@ -161,36 +161,36 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl 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; + useUnsyncedUpdate = true; idlePulse = 0; break; - case PWM_TYPE_STANDARD: + case MOTOR_PROTOCOL_STANDARD: sMin = 1e-3f; sLen = 1e-3f; - useUnsyncedPwm = true; + useUnsyncedUpdate = true; idlePulse = 0; break; } motorPwmDevice.vTable.write = pwmWriteStandard; motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate; + motorPwmDevice.vTable.updateComplete = useUnsyncedUpdate ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate; for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; @@ -212,23 +212,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 = useUnsyncedUpdate ? 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 = useUnsyncedUpdate ? 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++) { diff --git a/src/platform/APM32/pwm_output_dshot_apm32.c b/src/platform/APM32/pwm_output_dshot_apm32.c index 02189569f8..30ae7bdab0 100644 --- a/src/platform/APM32/pwm_output_dshot_apm32.c +++ b/src/platform/APM32/pwm_output_dshot_apm32.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" @@ -187,7 +187,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 @@ -275,7 +275,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; @@ -355,7 +355,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; @@ -373,7 +373,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 9ea68bba43..7374dfe03e 100644 --- a/src/platform/AT32/dshot_bitbang.c +++ b/src/platform/AT32/dshot_bitbang.c @@ -35,12 +35,12 @@ #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 "pwm_output_dshot_shared.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" @@ -93,7 +93,7 @@ const timerHardware_t bbTimerHardware[] = { 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 @@ -243,15 +243,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; } } @@ -364,7 +364,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); @@ -382,7 +382,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); @@ -669,7 +669,7 @@ static void bbPostInit(void) for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { - if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorPwmProtocol, bbMotors[motorIndex].output)) { + if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) { return; } @@ -695,6 +695,8 @@ static motorVTable_t bbVTable = { .convertExternalToMotor = dshotConvertFromExternal, .convertMotorToExternal = dshotConvertToExternal, .shutdown = bbShutdown, + .isMotorIdle = bbDshotIsMotorIdle, + .requestTelemetry = bbDshotRequestTelemetry, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) @@ -707,7 +709,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t dbgPinLo(0); dbgPinLo(1); - motorPwmProtocol = motorConfig->motorPwmProtocol; + motorProtocol = motorConfig->motorProtocol; bbDevice.vTable = bbVTable; motorCount = count; bbStatus = DSHOT_BITBANG_STATUS_OK; @@ -723,7 +725,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t 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 diff --git a/src/platform/AT32/dshot_bitbang_stdperiph.c b/src/platform/AT32/dshot_bitbang_stdperiph.c index 356e4c29ae..e00b56ad99 100644 --- a/src/platform/AT32/dshot_bitbang_stdperiph.c +++ b/src/platform/AT32/dshot_bitbang_stdperiph.c @@ -36,7 +36,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/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 62a4e4ac6b..49779a8752 100644 --- a/src/platform/AT32/pwm_output_at32bsp.c +++ b/src/platform/AT32/pwm_output_at32bsp.c @@ -149,7 +149,7 @@ static motorVTable_t motorPwmVTable = { .convertMotorToExternal = pwmConvertToExternal, }; -motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm) +motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate) { memset(motors, 0, sizeof(motors)); @@ -157,36 +157,36 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl 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; + useUnsyncedUpdate = true; idlePulse = 0; break; - case PWM_TYPE_STANDARD: + case MOTOR_PROTOCOL_STANDARD: sMin = 1e-3f; sLen = 1e-3f; - useUnsyncedPwm = true; + useUnsyncedUpdate = true; idlePulse = 0; break; } motorPwmDevice.vTable.write = pwmWriteStandard; motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate; + motorPwmDevice.vTable.updateComplete = useUnsyncedUpdate ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate; for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; @@ -208,23 +208,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 = useUnsyncedUpdate ? 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 = useUnsyncedUpdate ? 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++) { diff --git a/src/platform/AT32/pwm_output_dshot.c b/src/platform/AT32/pwm_output_dshot.c index ddaf7ed3e9..68a2c23d0b 100644 --- a/src/platform/AT32/pwm_output_dshot.c +++ b/src/platform/AT32/pwm_output_dshot.c @@ -39,9 +39,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 @@ -130,7 +130,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 @@ -360,7 +360,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 @@ -450,7 +450,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); @@ -522,7 +522,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; @@ -550,7 +550,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 65752866c6..b26a0d0647 100644 --- a/src/platform/SIMULATOR/sitl.c +++ b/src/platform/SIMULATOR/sitl.c @@ -627,10 +627,10 @@ static motorDevice_t motorPwmDevice = { } }; -motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount, bool useUnsyncedPwm) +motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount, bool useUnsyncedUpdate) { UNUSED(motorConfig); - UNUSED(useUnsyncedPwm); + UNUSED(useUnsyncedUpdate); printf("Initialized motor count %d\n", motorCount); pwmRawPkt.motorCount = motorCount; diff --git a/src/platform/STM32/dshot_bitbang.c b/src/platform/STM32/dshot_bitbang.c index b6d98d3fc8..d0ae79649e 100644 --- a/src/platform/STM32/dshot_bitbang.c +++ b/src/platform/STM32/dshot_bitbang.c @@ -35,12 +35,12 @@ #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 "pwm_output_dshot_shared.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" @@ -137,7 +137,7 @@ const timerHardware_t bbTimerHardware[] = { 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 +287,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 +408,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 +426,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) { @@ -608,11 +608,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); @@ -714,7 +709,7 @@ static void bbPostInit(void) for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { - if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorPwmProtocol, bbMotors[motorIndex].output)) { + if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) { return; } @@ -740,6 +735,8 @@ static motorVTable_t bbVTable = { .convertExternalToMotor = dshotConvertFromExternal, .convertMotorToExternal = dshotConvertToExternal, .shutdown = bbShutdown, + .isMotorIdle = bbDshotIsMotorIdle, + .requestTelemetry = bbDshotRequestTelemetry, }; dshotBitbangStatus_e dshotBitbangGetStatus(void) @@ -752,7 +749,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t dbgPinLo(0); dbgPinLo(1); - motorPwmProtocol = motorConfig->motorPwmProtocol; + motorProtocol = motorConfig->motorProtocol; bbDevice.vTable = bbVTable; motorCount = count; bbStatus = DSHOT_BITBANG_STATUS_OK; @@ -768,7 +765,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t 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 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..84dce52181 100644 --- a/src/platform/STM32/dshot_bitbang_stdperiph.c +++ b/src/platform/STM32/dshot_bitbang_stdperiph.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/STM32/mk/STM32F4.mk b/src/platform/STM32/mk/STM32F4.mk index 28bf0fecb6..de52550c13 100644 --- a/src/platform/STM32/mk/STM32F4.mk +++ b/src/platform/STM32/mk/STM32F4.mk @@ -175,7 +175,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 \ diff --git a/src/platform/STM32/mk/STM32F7.mk b/src/platform/STM32/mk/STM32F7.mk index b197c92774..ab362b4683 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 \ @@ -191,7 +190,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 41b1ba1a45..f7d2409b3b 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 \ diff --git a/src/platform/STM32/mk/STM32H5.mk b/src/platform/STM32/mk/STM32H5.mk index b8fb8511d2..b85922afff 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 \ diff --git a/src/platform/STM32/mk/STM32H7.mk b/src/platform/STM32/mk/STM32H7.mk index ffa2bf5b6f..9b2e076fd1 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 \ diff --git a/src/platform/STM32/mk/STM32_COMMON.mk b/src/platform/STM32/mk/STM32_COMMON.mk index 8106ccf43d..4f5201794a 100644 --- a/src/platform/STM32/mk/STM32_COMMON.mk +++ b/src/platform/STM32/mk/STM32_COMMON.mk @@ -7,7 +7,11 @@ MCU_COMMON_SRC += \ common/stm32/bus_spi_pinconfig.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 \ + common/stm32/pwm_output_dshot_shared.c \ + common/stm32/dshot_bitbang_shared.c + SIZE_OPTIMISED_SRC += \ common/stm32/config_flash.c \ @@ -16,5 +20,7 @@ SIZE_OPTIMISED_SRC += \ SPEED_OPTIMISED_SRC += \ common/stm32/system.c \ common/stm32/bus_spi_hw.c \ + common/stm32/pwm_output_dshot_shared.c \ + common/stm32/dshot_bitbang_shared.c \ common/stm32/io_impl.c diff --git a/src/platform/STM32/pwm_output.c b/src/platform/STM32/pwm_output.c index 3def265a14..dc86e6daa7 100644 --- a/src/platform/STM32/pwm_output.c +++ b/src/platform/STM32/pwm_output.c @@ -181,7 +181,7 @@ static motorVTable_t motorPwmVTable = { .convertMotorToExternal = pwmConvertToExternal, }; -motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm) +motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate) { memset(motors, 0, sizeof(motors)); @@ -189,36 +189,36 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl 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; + useUnsyncedUpdate = true; idlePulse = 0; break; - case PWM_TYPE_STANDARD: + case MOTOR_PROTOCOL_STANDARD: sMin = 1e-3f; sLen = 1e-3f; - useUnsyncedPwm = true; + useUnsyncedUpdate = true; idlePulse = 0; break; } motorPwmDevice.vTable.write = pwmWriteStandard; motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull; - motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate; + motorPwmDevice.vTable.updateComplete = useUnsyncedUpdate ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate; for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex]; @@ -240,23 +240,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 = useUnsyncedUpdate ? 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 = useUnsyncedUpdate ? 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++) { diff --git a/src/platform/STM32/pwm_output_dshot.c b/src/platform/STM32/pwm_output_dshot.c index 033a10907c..1119471f75 100644 --- a/src/platform/STM32/pwm_output_dshot.c +++ b/src/platform/STM32/pwm_output_dshot.c @@ -42,9 +42,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" #ifdef USE_DSHOT_TELEMETRY @@ -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..1ac173f90d 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" @@ -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/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/main/drivers/dshot_bitbang_impl.h b/src/platform/common/stm32/dshot_bitbang_impl.h similarity index 98% rename from src/main/drivers/dshot_bitbang_impl.h rename to src/platform/common/stm32/dshot_bitbang_impl.h index 0139c24257..9331a732f4 100644 --- a/src/main/drivers/dshot_bitbang_impl.h +++ b/src/platform/common/stm32/dshot_bitbang_impl.h @@ -22,6 +22,7 @@ #include "common/time.h" #include "drivers/motor.h" +#include "drivers/dshot.h" #define USE_DMA_REGISTER_CACHE @@ -275,3 +276,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(uint8_t motorIndex); +bool bbDshotIsMotorIdle(uint8_t 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..a020227aad --- /dev/null +++ b/src/platform/common/stm32/dshot_bitbang_shared.c @@ -0,0 +1,38 @@ +/* + * 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" + +void bbDshotRequestTelemetry(uint8_t motorIndex) +{ + bbMotor_t *const bbmotor = &bbMotors[motorIndex]; + + if (!bbmotor->configured) { + return; + } + bbmotor->protocolControl.requestTelemetry = true; +} + +bool bbDshotIsMotorIdle(uint8_t motorIndex) +{ + bbMotor_t *const bbmotor = &bbMotors[motorIndex]; + return bbmotor->protocolControl.value; +} diff --git a/src/main/drivers/dshot_dpwm.c b/src/platform/common/stm32/dshot_dpwm.c similarity index 88% rename from src/main/drivers/dshot_dpwm.c rename to src/platform/common/stm32/dshot_dpwm.c index a7ab462f51..e0db84c475 100644 --- a/src/main/drivers/dshot_dpwm.c +++ b/src/platform/common/stm32/dshot_dpwm.c @@ -30,8 +30,9 @@ #ifdef USE_DSHOT #include "drivers/pwm_output.h" +#include "pwm_output_dshot_shared.h" #include "drivers/dshot.h" -#include "drivers/dshot_dpwm.h" +#include "dshot_dpwm.h" #include "drivers/motor.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; } } @@ -147,14 +148,16 @@ static motorVTable_t dshotPwmVTable = { .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) +motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate) { UNUSED(idlePulse); - UNUSED(useUnsyncedPwm); + UNUSED(useUnsyncedUpdate); dshotPwmDevice.vTable = dshotPwmVTable; @@ -163,13 +166,13 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl 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 || @@ -190,8 +193,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; diff --git a/src/main/drivers/dshot_dpwm.h b/src/platform/common/stm32/dshot_dpwm.h similarity index 92% rename from src/main/drivers/dshot_dpwm.h rename to src/platform/common/stm32/dshot_dpwm.h index 1c6a7400fb..806bdf1135 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) @@ -159,7 +156,7 @@ typedef struct motorDmaOutput_s { motorDmaOutput_t *getMotorDmaOutput(uint8_t 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 diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/platform/common/stm32/pwm_output_dshot_shared.c similarity index 93% rename from src/main/drivers/pwm_output_dshot_shared.c rename to src/platform/common/stm32/pwm_output_dshot_shared.c index 998112becf..f5788d193b 100644 --- a/src/main/drivers/pwm_output_dshot_shared.c +++ b/src/platform/common/stm32/pwm_output_dshot_shared.c @@ -40,9 +40,9 @@ #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" @@ -60,7 +60,7 @@ 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) @@ -68,6 +68,18 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index) return &dmaMotors[index]; } +bool pwmDshotIsMotorIdle(uint8_t index) +{ + const motorDmaOutput_t *motor = getMotorDmaOutput(index); + return motor->protocolControl.value; +} + +void pwmDshotRequestTelemetry(uint8_t index) +{ + motorDmaOutput_t * const motor = getMotorDmaOutput(index); + motor->protocolControl.requestTelemetry = true; +} + uint8_t getTimerIndex(TIM_TypeDef *timer) { for (int i = 0; i < dmaMotorTimerCount; i++) { @@ -127,12 +139,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 } diff --git a/src/main/drivers/pwm_output_dshot_shared.h b/src/platform/common/stm32/pwm_output_dshot_shared.h similarity index 74% rename from src/main/drivers/pwm_output_dshot_shared.h rename to src/platform/common/stm32/pwm_output_dshot_shared.h index 7c282d5008..4183091f66 100644 --- a/src/main/drivers/pwm_output_dshot_shared.h +++ b/src/platform/common/stm32/pwm_output_dshot_shared.h @@ -20,28 +20,13 @@ #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); @@ -59,6 +44,8 @@ void pwmDshotSetDirectionOutput( #endif ); +void pwmDshotRequestTelemetry(uint8_t index); +bool pwmDshotIsMotorIdle(uint8_t index); bool pwmTelemetryDecode(void); #endif diff --git a/src/test/unit/pg_unittest.cc b/src/test/unit/pg_unittest.cc index 23ce0c7fb8..1b02e4f848 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, + .useUnsyncedUpdate = 0, .useBurstDshot = 0, .useDshotTelemetry = 0, .useDshotEdt = 0,