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,