diff --git a/make/source.mk b/make/source.mk
index 5168b3009e..d98ed91594 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -10,6 +10,9 @@ COMMON_SRC = \
cli/cli.c \
cli/settings.c \
drivers/adc.c \
+ drivers/dshot.c \
+ drivers/dshot_dpwm.c \
+ drivers/dshot_command.c \
drivers/buf_writer.c \
drivers/bus.c \
drivers/bus_i2c_config.c \
@@ -26,6 +29,7 @@ COMMON_SRC = \
drivers/io.c \
drivers/light_led.c \
drivers/mco.c \
+ drivers/motor.c \
drivers/pinio.c \
drivers/resource.c \
drivers/rcc.c \
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
index b5a2017b52..7d4f6bbfae 100644
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -71,7 +71,10 @@ uint8_t cliMode = 0;
#include "drivers/io.h"
#include "drivers/io_impl.h"
#include "drivers/light_led.h"
-#include "drivers/pwm_output.h"
+#include "drivers/motor.h"
+#include "drivers/dshot.h"
+#include "drivers/dshot_dpwm.h"
+#include "drivers/dshot_command.h"
#include "drivers/rangefinder/rangefinder_hcsr04.h"
#include "drivers/sdcard.h"
#include "drivers/sensor.h"
@@ -1424,9 +1427,11 @@ static void cliSerialPassthrough(char *cmdline)
serialSetCtrlLineStateCb(ports[0].port, cbCtrlLine, (void *)(intptr_t)(port1PinioDtr));
}
+// XXX Review ESC pass through under refactored motor handling
#ifdef USE_PWM_OUTPUT
if (escSensorPassthrough) {
- pwmDisableMotors();
+ // pwmDisableMotors();
+ motorDisable();
delay(5);
unsigned motorsCount = getMotorCount();
for (unsigned i = 0; i < motorsCount; i++) {
@@ -3363,7 +3368,7 @@ static void cliRebootEx(rebootTarget_e rebootTarget)
cliPrint("\r\nRebooting");
bufWriterFlush(cliWriter);
waitForSerialPortToFinishTransmitting(cliPort);
- stopPwmAllMotors();
+ motorShutdown();
switch (rebootTarget) {
case REBOOT_TARGET_BOOTLOADER_ROM:
@@ -3664,7 +3669,7 @@ static void executeEscInfoCommand(uint8_t escIndex)
startEscDataRead(escInfoBuffer, ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE);
- pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO, true);
+ dshotCommandWrite(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO, true);
delay(10);
@@ -3672,6 +3677,9 @@ static void executeEscInfoCommand(uint8_t escIndex)
}
#endif // USE_ESC_SENSOR && USE_ESC_SENSOR_INFO
+
+// XXX Review dshotprog command under refactored motor handling
+
static void cliDshotProg(char *cmdline)
{
if (isEmpty(cmdline) || motorConfig()->dev.motorPwmProtocol < PWM_TYPE_DSHOT150) {
@@ -3699,7 +3707,8 @@ static void cliDshotProg(char *cmdline)
int command = atoi(pch);
if (command >= 0 && command < DSHOT_MIN_THROTTLE) {
if (firstCommand) {
- pwmDisableMotors();
+ // pwmDisableMotors();
+ motorDisable();
if (command == DSHOT_CMD_ESC_INFO) {
delay(5); // Wait for potential ESC telemetry transmission to finish
@@ -3711,7 +3720,7 @@ static void cliDshotProg(char *cmdline)
}
if (command != DSHOT_CMD_ESC_INFO) {
- pwmWriteDshotCommand(escIndex, getMotorCount(), command, true);
+ dshotCommandWrite(escIndex, getMotorCount(), command, true);
} else {
#if defined(USE_ESC_SENSOR) && defined(USE_ESC_SENSOR_INFO)
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
@@ -3743,7 +3752,7 @@ static void cliDshotProg(char *cmdline)
pch = strtok_r(NULL, " ", &saveptr);
}
- pwmEnableMotors();
+ motorEnable();
}
#endif // USE_DSHOT
@@ -3875,7 +3884,7 @@ static void cliMotor(char *cmdline)
if (motorValue < PWM_RANGE_MIN || motorValue > PWM_RANGE_MAX) {
cliShowArgumentRangeError("VALUE", 1000, 2000);
} else {
- uint32_t motorOutputValue = convertExternalToMotor(motorValue);
+ uint32_t motorOutputValue = motorConvertFromExternal(motorValue);
if (motorIndex != ALL_MOTORS) {
motor_disarmed[motorIndex] = motorOutputValue;
@@ -5294,7 +5303,6 @@ static void cliDma(char* cmdline)
}
#endif
-
static void cliResource(char *cmdline)
{
char *pch = NULL;
@@ -5910,7 +5918,7 @@ static void cliMsc(char *cmdline)
cliPrint("\r\nRebooting");
bufWriterFlush(cliWriter);
waitForSerialPortToFinishTransmitting(cliPort);
- stopPwmAllMotors();
+ motorShutdown();
systemResetToMsc(timezoneOffsetMinutes);
} else {
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index f3f92ecf9e..6cc2e08bb0 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -35,6 +35,7 @@
#include "drivers/adc.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
+#include "drivers/dshot_command.h"
#include "drivers/camera_control.h"
#include "drivers/light_led.h"
#include "drivers/pinio.h"
@@ -580,7 +581,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableGyroFilterDebug),
- LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource)
+ LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource),
};
#undef LOOKUP_TABLE_ENTRY
diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h
index c74f9fd762..b6a888dc5d 100644
--- a/src/main/cli/settings.h
+++ b/src/main/cli/settings.h
@@ -135,6 +135,7 @@ typedef enum {
#endif
TABLE_GYRO_FILTER_DEBUG,
TABLE_POSITION_ALT_SOURCE,
+
LOOKUP_TABLE_COUNT
} lookupTableIndex_e;
diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index 045f14776e..d0e83a6e1e 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -50,6 +50,7 @@
#include "drivers/system.h"
#include "drivers/time.h"
+#include "drivers/motor.h"
// For rcData, stopAllMotors, stopPwmAllMotors
#include "config/feature.h"
@@ -808,7 +809,7 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing?
stopMotors();
- stopPwmAllMotors();
+ motorShutdown();
delay(200);
systemReset();
diff --git a/src/main/drivers/dshot.c b/src/main/drivers/dshot.c
new file mode 100644
index 0000000000..dcf40f71a9
--- /dev/null
+++ b/src/main/drivers/dshot.c
@@ -0,0 +1,134 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 .
+ *
+ * Author: jflyper
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_DSHOT
+
+#include "build/atomic.h"
+
+#include "common/maths.h"
+#include "common/time.h"
+
+#include "config/feature.h"
+
+#include "drivers/motor.h"
+#include "drivers/timer.h"
+
+#include "drivers/dshot.h"
+#include "drivers/dshot_dpwm.h" // for motorDmaOutput_t, should be gone
+#include "drivers/dshot_command.h"
+#include "drivers/nvic.h"
+#include "drivers/pwm_output.h" // for PWM_TYPE_* and others
+
+#include "fc/rc_controls.h" // for flight3DConfig_t
+
+#include "pg/motor.h"
+
+#include "rx/rx.h"
+
+void dshotInitEndpoints(float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow) {
+ float outputLimitOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) * (1 - outputLimit);
+ *disarm = DSHOT_CMD_MOTOR_STOP;
+ if (featureIsEnabled(FEATURE_3D)) {
+ *outputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
+ *outputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset / 2;
+ *deadbandMotor3dHigh = DSHOT_3D_FORWARD_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_3D_FORWARD_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
+ *deadbandMotor3dLow = DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - outputLimitOffset / 2;
+ } else {
+ *outputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
+ *outputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset;
+ }
+}
+
+float dshotConvertFromExternal(uint16_t externalValue)
+{
+ uint16_t motorValue;
+
+ externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX);
+
+ if (featureIsEnabled(FEATURE_3D)) {
+ if (externalValue == PWM_RANGE_MIDDLE) {
+ motorValue = DSHOT_CMD_MOTOR_STOP;
+ } else if (externalValue < PWM_RANGE_MIDDLE) {
+ motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MIDDLE - 1, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, DSHOT_MIN_THROTTLE);
+ } else {
+ motorValue = scaleRange(externalValue, PWM_RANGE_MIDDLE + 1, PWM_RANGE_MAX, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
+ }
+ } else {
+ motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_CMD_MOTOR_STOP : scaleRange(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
+ }
+
+ return (float)motorValue;
+}
+
+uint16_t dshotConvertToExternal(float motorValue)
+{
+ uint16_t externalValue;
+
+ if (featureIsEnabled(FEATURE_3D)) {
+ if (motorValue == DSHOT_CMD_MOTOR_STOP || motorValue < DSHOT_MIN_THROTTLE) {
+ externalValue = PWM_RANGE_MIDDLE;
+ } else if (motorValue <= DSHOT_3D_FORWARD_MIN_THROTTLE - 1) {
+ externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, PWM_RANGE_MIDDLE - 1, PWM_RANGE_MIN);
+ } else {
+ externalValue = scaleRange(motorValue, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIDDLE + 1, PWM_RANGE_MAX);
+ }
+ } else {
+ externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);
+ }
+
+ return externalValue;
+}
+
+FAST_CODE uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb)
+{
+ uint16_t packet;
+
+ ATOMIC_BLOCK(NVIC_PRIO_DSHOT_DMA) {
+ packet = (pcb->value << 1) | (pcb->requestTelemetry ? 1 : 0);
+ pcb->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
+ }
+
+ // compute checksum
+ unsigned csum = 0;
+ unsigned csum_data = packet;
+ for (int i = 0; i < 3; i++) {
+ csum ^= csum_data; // xor data by nibbles
+ csum_data >>= 4;
+ }
+ // append checksum
+#ifdef USE_DSHOT_TELEMETRY
+ if (useDshotTelemetry) {
+ csum = ~csum;
+ }
+#endif
+ csum &= 0xf;
+ packet = (packet << 4) | csum;
+
+ return packet;
+}
+#endif // USE_DSHOT
diff --git a/src/main/drivers/dshot.h b/src/main/drivers/dshot.h
new file mode 100644
index 0000000000..57d156a036
--- /dev/null
+++ b/src/main/drivers/dshot.h
@@ -0,0 +1,47 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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
+
+#define DSHOT_MIN_THROTTLE 48
+#define DSHOT_MAX_THROTTLE 2047
+#define DSHOT_3D_FORWARD_MIN_THROTTLE 1048
+
+typedef struct dshotProtocolControl_s {
+ uint16_t value;
+ bool requestTelemetry;
+} dshotProtocolControl_t;
+
+void dshotInitEndpoints(float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow);
+float dshotConvertFromExternal(uint16_t externalValue);
+uint16_t dshotConvertToExternal(float motorValue);
+
+FAST_CODE uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb);
+
+#ifdef USE_DSHOT_TELEMETRY
+extern bool useDshotTelemetry;
+extern uint32_t dshotInvalidPacketCount;
+#endif
+
+uint16_t getDshotTelemetry(uint8_t index);
+bool isDshotMotorTelemetryActive(uint8_t motorIndex);
+bool isDshotTelemetryActive(void);
+
+int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex);
diff --git a/src/main/drivers/dshot_command.c b/src/main/drivers/dshot_command.c
new file mode 100644
index 0000000000..dce9dc724d
--- /dev/null
+++ b/src/main/drivers/dshot_command.c
@@ -0,0 +1,298 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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
+#include
+
+#include "platform.h"
+
+#ifdef USE_DSHOT
+
+#include "common/time.h"
+
+#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/dshot_command.h"
+#include "drivers/pwm_output.h"
+
+#define DSHOT_INITIAL_DELAY_US 10000
+#define DSHOT_COMMAND_DELAY_US 1000
+#define DSHOT_ESCINFO_DELAY_US 12000
+#define DSHOT_BEEP_DELAY_US 100000
+#define DSHOT_MAX_COMMANDS 3
+
+typedef enum {
+ DSHOT_COMMAND_STATE_IDLEWAIT, // waiting for motors to go idle
+ DSHOT_COMMAND_STATE_STARTDELAY, // initial delay period before a sequence of commands
+ DSHOT_COMMAND_STATE_ACTIVE, // actively sending the command (with optional repeated output)
+ DSHOT_COMMAND_STATE_POSTDELAY // delay period after the command has been sent
+} dshotCommandState_e;
+
+typedef struct dshotCommandControl_s {
+ dshotCommandState_e state;
+ uint32_t nextCommandCycleDelay;
+ timeUs_t delayAfterCommandUs;
+ uint8_t repeats;
+ uint8_t command[MAX_SUPPORTED_MOTORS];
+} dshotCommandControl_t;
+
+static timeUs_t dshotCommandPidLoopTimeUs = 125; // default to 8KHz (125us) loop to prevent possible div/0
+ // gets set to the actual value when the PID loop is initialized
+
+// XXX Optimization opportunity here.
+// https://github.com/betaflight/betaflight/pull/8534#pullrequestreview-258947278
+// @ledvinap: queue entry is quite large - it may be better to handle empty/full queue using different mechanism (magic value for Head or Tail / explicit element count)
+// Explicit element count will make code below simpler, but care has to be taken to avoid race conditions
+
+static dshotCommandControl_t commandQueue[DSHOT_MAX_COMMANDS + 1];
+static uint8_t commandQueueHead;
+static uint8_t commandQueueTail;
+
+void dshotSetPidLoopTime(uint32_t pidLoopTime)
+{
+ dshotCommandPidLoopTimeUs = pidLoopTime;
+}
+
+static FAST_CODE bool dshotCommandQueueFull()
+{
+ return (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueTail;
+}
+
+FAST_CODE bool dshotCommandQueueEmpty(void)
+{
+ return commandQueueHead == commandQueueTail;
+}
+
+static FAST_CODE bool isLastDshotCommand(void)
+{
+ return ((commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueHead);
+}
+
+FAST_CODE bool dshotCommandIsProcessing(void)
+{
+ if (dshotCommandQueueEmpty()) {
+ return false;
+ }
+ dshotCommandControl_t* command = &commandQueue[commandQueueTail];
+ const bool commandIsProcessing = command->state == DSHOT_COMMAND_STATE_STARTDELAY
+ || command->state == DSHOT_COMMAND_STATE_ACTIVE
+ || (command->state == DSHOT_COMMAND_STATE_POSTDELAY && !isLastDshotCommand());
+ return commandIsProcessing;
+}
+
+static FAST_CODE bool dshotCommandQueueUpdate(void)
+{
+ if (!dshotCommandQueueEmpty()) {
+ commandQueueTail = (commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1);
+ if (!dshotCommandQueueEmpty()) {
+ // There is another command in the queue so update it so it's ready to output in
+ // sequence. It can go directly to the DSHOT_COMMAND_STATE_ACTIVE state and bypass
+ // the DSHOT_COMMAND_STATE_IDLEWAIT and DSHOT_COMMAND_STATE_STARTDELAY states.
+ dshotCommandControl_t* nextCommand = &commandQueue[commandQueueTail];
+ nextCommand->state = DSHOT_COMMAND_STATE_ACTIVE;
+ nextCommand->nextCommandCycleDelay = 0;
+ return true;
+ }
+ }
+ return false;
+}
+
+static FAST_CODE uint32_t dshotCommandCyclesFromTime(timeUs_t delayUs)
+{
+ // Find the minimum number of motor output cycles needed to
+ // provide at least delayUs time delay
+
+ return (delayUs + dshotCommandPidLoopTimeUs - 1) / dshotCommandPidLoopTimeUs;
+}
+
+static dshotCommandControl_t* addCommand()
+{
+ int newHead = (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1);
+ if (newHead == commandQueueTail) {
+ return NULL;
+ }
+ dshotCommandControl_t* control = &commandQueue[commandQueueHead];
+ commandQueueHead = newHead;
+ return control;
+}
+
+static bool allMotorsAreIdle(void)
+{
+ for (unsigned i = 0; i < dshotPwmDevice.count; i++) {
+ const motorDmaOutput_t *motor = getMotorDmaOutput(i);
+ if (motor->protocolControl.value) {
+ return false;
+ }
+ }
+
+ return true;
+}
+
+void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
+{
+ UNUSED(motorCount);
+
+ if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || dshotCommandQueueFull()) {
+ return;
+ }
+
+ uint8_t repeats = 1;
+ timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
+
+ switch (command) {
+ case DSHOT_CMD_SPIN_DIRECTION_1:
+ case DSHOT_CMD_SPIN_DIRECTION_2:
+ case DSHOT_CMD_3D_MODE_OFF:
+ case DSHOT_CMD_3D_MODE_ON:
+ case DSHOT_CMD_SAVE_SETTINGS:
+ case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
+ case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
+ case DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE:
+ case DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY:
+ repeats = 10;
+ break;
+ case DSHOT_CMD_BEACON1:
+ case DSHOT_CMD_BEACON2:
+ case DSHOT_CMD_BEACON3:
+ case DSHOT_CMD_BEACON4:
+ case DSHOT_CMD_BEACON5:
+ delayAfterCommandUs = DSHOT_BEEP_DELAY_US;
+ break;
+ default:
+ break;
+ }
+
+ if (blocking) {
+ delayMicroseconds(DSHOT_INITIAL_DELAY_US - DSHOT_COMMAND_DELAY_US);
+ for (; repeats; repeats--) {
+ delayMicroseconds(DSHOT_COMMAND_DELAY_US);
+
+#ifdef USE_DSHOT_TELEMETRY
+ timeUs_t timeoutUs = micros() + 1000;
+ while (!pwmStartDshotMotorUpdate() &&
+ cmpTimeUs(timeoutUs, micros()) > 0);
+#endif
+ for (uint8_t i = 0; i < dshotPwmDevice.count; i++) {
+ if ((i == index) || (index == ALL_MOTORS)) {
+ motorDmaOutput_t *const motor = getMotorDmaOutput(i);
+ motor->protocolControl.requestTelemetry = true;
+ dshotPwmDevice.vTable.writeInt(i, command);
+ }
+ }
+
+ dshotPwmDevice.vTable.updateComplete();
+ }
+ delayMicroseconds(delayAfterCommandUs);
+ } else {
+ dshotCommandControl_t *commandControl = addCommand();
+ if (commandControl) {
+ commandControl->repeats = repeats;
+ commandControl->delayAfterCommandUs = delayAfterCommandUs;
+ for (unsigned i = 0; i < dshotPwmDevice.count; i++) {
+ if (index == i || index == ALL_MOTORS) {
+ commandControl->command[i] = command;
+ } else {
+ commandControl->command[i] = DSHOT_CMD_MOTOR_STOP;
+ }
+ }
+ if (allMotorsAreIdle()) {
+ // we can skip the motors idle wait state
+ commandControl->state = DSHOT_COMMAND_STATE_STARTDELAY;
+ commandControl->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
+ } else {
+ commandControl->state = DSHOT_COMMAND_STATE_IDLEWAIT;
+ commandControl->nextCommandCycleDelay = 0; // will be set after idle wait completes
+ }
+ }
+ }
+}
+
+uint8_t pwmGetDshotCommand(uint8_t index)
+{
+ return commandQueue[commandQueueTail].command[index];
+}
+
+// This function is used to synchronize the dshot command output timing with
+// the normal motor output timing tied to the PID loop frequency. A "true" result
+// 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)
+{
+ UNUSED(motorCount);
+
+ dshotCommandControl_t* command = &commandQueue[commandQueueTail];
+ switch (command->state) {
+ case DSHOT_COMMAND_STATE_IDLEWAIT:
+ if (allMotorsAreIdle()) {
+ command->state = DSHOT_COMMAND_STATE_STARTDELAY;
+ command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
+ }
+ break;
+
+ case DSHOT_COMMAND_STATE_STARTDELAY:
+ if (command->nextCommandCycleDelay) {
+ --command->nextCommandCycleDelay;
+ return false; // Delay motor output until the start of the command seequence
+ }
+ command->state = DSHOT_COMMAND_STATE_ACTIVE;
+ command->nextCommandCycleDelay = 0; // first iteration of the repeat happens now
+ FALLTHROUGH;
+
+ case DSHOT_COMMAND_STATE_ACTIVE:
+ if (command->nextCommandCycleDelay) {
+ --command->nextCommandCycleDelay;
+ return false; // Delay motor output until the next command repeat
+ }
+
+ command->repeats--;
+ if (command->repeats) {
+ command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_COMMAND_DELAY_US);
+ } else {
+ command->state = DSHOT_COMMAND_STATE_POSTDELAY;
+ command->nextCommandCycleDelay = dshotCommandCyclesFromTime(command->delayAfterCommandUs);
+ if (!isLastDshotCommand() && command->nextCommandCycleDelay > 0) {
+ // Account for the 1 extra motor output loop between commands.
+ // Otherwise the inter-command delay will be DSHOT_COMMAND_DELAY_US + 1 loop.
+ command->nextCommandCycleDelay--;
+ }
+ }
+ break;
+
+ case DSHOT_COMMAND_STATE_POSTDELAY:
+ if (command->nextCommandCycleDelay) {
+ --command->nextCommandCycleDelay;
+ return false; // Delay motor output until the end of the post-command delay
+ }
+ if (dshotCommandQueueUpdate()) {
+ // Will be true if the command queue is not empty and we
+ // want to wait for the next command to start in sequence.
+ return false;
+ }
+ }
+
+ return true;
+}
+#endif // USE_DSHOT
diff --git a/src/main/drivers/dshot_command.h b/src/main/drivers/dshot_command.h
new file mode 100644
index 0000000000..86e6774eed
--- /dev/null
+++ b/src/main/drivers/dshot_command.h
@@ -0,0 +1,70 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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
+
+#define DSHOT_MAX_COMMAND 47
+
+/*
+ DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings
+
+ 3D Mode:
+ 0 = stop
+ 48 (low) - 1047 (high) -> negative direction
+ 1048 (low) - 2047 (high) -> positive direction
+ */
+
+typedef enum {
+ DSHOT_CMD_MOTOR_STOP = 0,
+ DSHOT_CMD_BEACON1,
+ DSHOT_CMD_BEACON2,
+ DSHOT_CMD_BEACON3,
+ DSHOT_CMD_BEACON4,
+ DSHOT_CMD_BEACON5,
+ DSHOT_CMD_ESC_INFO, // V2 includes settings
+ DSHOT_CMD_SPIN_DIRECTION_1,
+ DSHOT_CMD_SPIN_DIRECTION_2,
+ DSHOT_CMD_3D_MODE_OFF,
+ DSHOT_CMD_3D_MODE_ON,
+ DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented
+ DSHOT_CMD_SAVE_SETTINGS,
+ DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
+ DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
+ DSHOT_CMD_LED0_ON, // BLHeli32 only
+ DSHOT_CMD_LED1_ON, // BLHeli32 only
+ DSHOT_CMD_LED2_ON, // BLHeli32 only
+ DSHOT_CMD_LED3_ON, // BLHeli32 only
+ DSHOT_CMD_LED0_OFF, // BLHeli32 only
+ DSHOT_CMD_LED1_OFF, // BLHeli32 only
+ DSHOT_CMD_LED2_OFF, // BLHeli32 only
+ DSHOT_CMD_LED3_OFF, // BLHeli32 only
+ DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF = 30, // KISS audio Stream mode on/Off
+ DSHOT_CMD_SILENT_MODE_ON_OFF = 31, // KISS silent Mode on/Off
+ DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE = 32,
+ DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY = 33,
+ DSHOT_CMD_MAX = 47
+} dshotCommands_e;
+
+void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking);
+void dshotSetPidLoopTime(uint32_t pidLoopTime);
+bool dshotCommandQueueEmpty(void);
+bool dshotCommandIsProcessing(void);
+uint8_t pwmGetDshotCommand(uint8_t index);
+bool dshotCommandOutputIsEnabled(uint8_t motorCount);
diff --git a/src/main/drivers/dshot_dpwm.c b/src/main/drivers/dshot_dpwm.c
new file mode 100644
index 0000000000..b5f2a56934
--- /dev/null
+++ b/src/main/drivers/dshot_dpwm.c
@@ -0,0 +1,197 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 .
+ *
+ * Author: jflyper
+ */
+
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_DSHOT
+
+#include "drivers/pwm_output.h"
+#include "drivers/dshot.h"
+#include "drivers/dshot_dpwm.h"
+#include "drivers/motor.h"
+
+#include "pg/motor.h"
+
+// XXX TODO: Share a single region among dshotDmaBuffer and dshotBurstDmaBuffer
+
+DSHOT_DMA_BUFFER_ATTRIBUTE DSHOT_DMA_BUFFER_UNIT dshotDmaBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_ALLOC_SIZE];
+
+#ifdef USE_DSHOT_DMAR
+DSHOT_DMA_BUFFER_ATTRIBUTE DSHOT_DMA_BUFFER_UNIT dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
+#endif
+
+#ifdef USE_DSHOT_DMAR
+FAST_RAM_ZERO_INIT bool useBurstDshot = false;
+#endif
+#ifdef USE_DSHOT_TELEMETRY
+FAST_RAM_ZERO_INIT bool useDshotTelemetry = false;
+#endif
+
+FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
+
+FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
+{
+ int i;
+ for (i = 0; i < 16; i++) {
+ dmaBuffer[i * stride] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
+ packet <<= 1;
+ }
+ dmaBuffer[i++ * stride] = 0;
+ dmaBuffer[i++ * stride] = 0;
+
+ return DSHOT_DMA_BUFFER_SIZE;
+}
+
+FAST_CODE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
+{
+ int i;
+ for (i = 0; i < 4; i++) {
+ dmaBuffer[i * stride] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first
+ packet <<= 4; // Shift 4 bits
+ }
+ dmaBuffer[i++ * stride] = 0;
+ dmaBuffer[i++ * stride] = 0;
+
+ return PROSHOT_DMA_BUFFER_SIZE;
+}
+
+uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
+{
+ switch (pwmProtocolType) {
+ case(PWM_TYPE_PROSHOT1000):
+ return MOTOR_PROSHOT1000_HZ;
+ case(PWM_TYPE_DSHOT1200):
+ return MOTOR_DSHOT1200_HZ;
+ case(PWM_TYPE_DSHOT600):
+ return MOTOR_DSHOT600_HZ;
+ case(PWM_TYPE_DSHOT300):
+ return MOTOR_DSHOT300_HZ;
+ default:
+ case(PWM_TYPE_DSHOT150):
+ return MOTOR_DSHOT150_HZ;
+ }
+}
+
+static void dshotPwmShutdown(void)
+{
+ // DShot signal is only generated if write to motors happen,
+ // and that is prevented by enabled checking at write.
+ // Hence there's no special processing required here.
+ return;
+}
+
+static void dshotPwmDisableMotors(void)
+{
+ // No special processing required
+ return;
+}
+
+static bool dshotPwmEnableMotors(void)
+{
+ // No special processing required
+ return true;
+}
+
+static FAST_CODE void dshotWriteInt(uint8_t index, uint16_t value)
+{
+ pwmWriteDshotInt(index, value);
+}
+
+static FAST_CODE void dshotWrite(uint8_t index, float value)
+{
+ pwmWriteDshotInt(index, lrintf(value));
+}
+
+static motorVTable_t dshotPwmVTable = {
+ .enable = dshotPwmEnableMotors,
+ .disable = dshotPwmDisableMotors,
+ .updateStart = motorUpdateStartNull, // May be updated after copying
+ .write = dshotWrite,
+ .writeInt = dshotWriteInt,
+ .updateComplete = pwmCompleteDshotMotorUpdate,
+ .convertExternalToMotor = dshotConvertFromExternal,
+ .convertMotorToExternal = dshotConvertToExternal,
+ .shutdown = dshotPwmShutdown,
+};
+
+FAST_RAM_ZERO_INIT motorDevice_t dshotPwmDevice;
+
+motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
+{
+ UNUSED(idlePulse);
+ UNUSED(useUnsyncedPwm);
+
+ dshotPwmDevice.vTable = dshotPwmVTable;
+
+#ifdef USE_DSHOT_TELEMETRY
+ useDshotTelemetry = motorConfig->useDshotTelemetry;
+ dshotPwmDevice.vTable.updateStart = pwmStartDshotMotorUpdate;
+#endif
+
+ switch (motorConfig->motorPwmProtocol) {
+ case PWM_TYPE_PROSHOT1000:
+ loadDmaBuffer = loadDmaBufferProshot;
+ break;
+ case PWM_TYPE_DSHOT1200:
+ case PWM_TYPE_DSHOT600:
+ case PWM_TYPE_DSHOT300:
+ case PWM_TYPE_DSHOT150:
+ loadDmaBuffer = loadDmaBufferDshot;
+#ifdef USE_DSHOT_DMAR
+ if (motorConfig->useBurstDshot) {
+ useBurstDshot = true;
+ }
+#endif
+ break;
+ }
+
+ for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
+ const ioTag_t tag = motorConfig->ioTags[motorIndex];
+ const timerHardware_t *timerHardware = timerGetByTag(tag);
+
+ if (timerHardware == NULL) {
+ /* not enough motors initialised for the mixer or a break in the motors */
+ dshotPwmDevice.vTable.write = motorWriteNull;
+ dshotPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
+ /* TODO: block arming and add reason system cannot arm */
+ return NULL;
+ }
+
+ motors[motorIndex].io = IOGetByTag(tag);
+ IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+
+ pwmDshotMotorHardwareConfig(timerHardware,
+ motorIndex,
+ motorConfig->motorPwmProtocol,
+ motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
+ motors[motorIndex].enabled = true;
+ }
+
+ return &dshotPwmDevice;
+}
+
+#endif // USE_DSHOT
diff --git a/src/main/drivers/dshot_dpwm.h b/src/main/drivers/dshot_dpwm.h
new file mode 100644
index 0000000000..16cd1ffae0
--- /dev/null
+++ b/src/main/drivers/dshot_dpwm.h
@@ -0,0 +1,162 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 .
+ *
+ * Author: jflyper
+ */
+
+#pragma once
+
+#define MOTOR_DSHOT1200_HZ MHZ_TO_HZ(24)
+#define MOTOR_DSHOT600_HZ MHZ_TO_HZ(12)
+#define MOTOR_DSHOT300_HZ MHZ_TO_HZ(6)
+#define MOTOR_DSHOT150_HZ MHZ_TO_HZ(3)
+
+#define MOTOR_BIT_0 7
+#define MOTOR_BIT_1 14
+#define MOTOR_BITLENGTH 20
+
+#define MOTOR_PROSHOT1000_HZ MHZ_TO_HZ(24)
+#define PROSHOT_BASE_SYMBOL 24 // 1uS
+#define PROSHOT_BIT_WIDTH 3
+#define MOTOR_NIBBLE_LENGTH_PROSHOT (PROSHOT_BASE_SYMBOL * 4) // 4uS
+
+#define DSHOT_TELEMETRY_DEADTIME_US (2 * 30 + 10) // 2 * 30uS to switch lines plus 10us grace period
+
+typedef uint8_t loadDmaBufferFn(uint32_t *dmaBuffer, int stride, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation
+extern FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
+FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet);
+FAST_CODE 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);
+
+/* Motor DMA related, moved from pwm_output.h */
+
+#define MAX_DMA_TIMERS 8
+
+#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
+#define PROSHOT_DMA_BUFFER_SIZE 6 /* resolution + frame reset (2us) */
+
+#define DSHOT_TELEMETRY_INPUT_LEN 32
+#define PROSHOT_TELEMETRY_INPUT_LEN 8
+
+// For H7, DMA buffer is placed in a dedicated segment for coherency management
+#ifdef STM32H7
+#define DSHOT_DMA_BUFFER_ATTRIBUTE DMA_RAM
+#else
+#define DSHOT_DMA_BUFFER_ATTRIBUTE // None
+#endif
+
+#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
+#define DSHOT_DMA_BUFFER_UNIT uint32_t
+#else
+#define DSHOT_DMA_BUFFER_UNIT uint8_t
+#endif
+
+#ifdef USE_DSHOT_TELEMETRY
+STATIC_ASSERT(DSHOT_TELEMETRY_INPUT_LEN >= DSHOT_DMA_BUFFER_SIZE, dshotBufferSizeConstrait);
+#define DSHOT_DMA_BUFFER_ALLOC_SIZE DSHOT_TELEMETRY_INPUT_LEN
+#else
+#define DSHOT_DMA_BUFFER_ALLOC_SIZE DSHOT_DMA_BUFFER_SIZE
+#endif
+
+extern DSHOT_DMA_BUFFER_UNIT dshotDmaBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_ALLOC_SIZE];
+
+#ifdef USE_DSHOT_DMAR
+extern DSHOT_DMA_BUFFER_UNIT dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
+#endif
+
+typedef struct {
+ TIM_TypeDef *timer;
+#if defined(USE_DSHOT) && defined(USE_DSHOT_DMAR)
+#if defined(STM32F7) || defined(STM32H7)
+ TIM_HandleTypeDef timHandle;
+ DMA_HandleTypeDef hdma_tim;
+#endif
+#ifdef STM32F3
+ DMA_Channel_TypeDef *dmaBurstRef;
+#else
+ DMA_Stream_TypeDef *dmaBurstRef;
+#endif
+ uint16_t dmaBurstLength;
+ uint32_t *dmaBurstBuffer;
+ timeUs_t inputDirectionStampUs;
+#endif
+ uint16_t timerDmaSources;
+} motorDmaTimer_t;
+
+typedef struct motorDmaOutput_s {
+ dshotProtocolControl_t protocolControl;
+ ioTag_t ioTag;
+ const timerHardware_t *timerHardware;
+#ifdef USE_DSHOT
+ uint16_t timerDmaSource;
+ uint8_t timerDmaIndex;
+ bool configured;
+#ifdef STM32H7
+ TIM_HandleTypeDef TimHandle;
+ DMA_HandleTypeDef hdma_tim;
+#endif
+ uint8_t output;
+ uint8_t index;
+#ifdef USE_DSHOT_TELEMETRY
+ bool useProshot;
+ volatile bool isInput;
+ volatile bool hasTelemetry;
+ uint16_t dshotTelemetryValue;
+ timeDelta_t dshotTelemetryDeadtimeUs;
+ bool dshotTelemetryActive;
+#ifdef USE_HAL_DRIVER
+ LL_TIM_OC_InitTypeDef ocInitStruct;
+ LL_TIM_IC_InitTypeDef icInitStruct;
+ LL_DMA_InitTypeDef dmaInitStruct;
+ uint32_t llChannel;
+#else
+ TIM_OCInitTypeDef ocInitStruct;
+ TIM_ICInitTypeDef icInitStruct;
+ DMA_InitTypeDef dmaInitStruct;
+#endif
+ uint8_t dmaInputLen;
+#endif
+#ifdef STM32F3
+ DMA_Channel_TypeDef *dmaRef;
+#else
+ DMA_Stream_TypeDef *dmaRef;
+#endif
+#endif
+ motorDmaTimer_t *timer;
+ DSHOT_DMA_BUFFER_UNIT *dmaBuffer;
+} motorDmaOutput_t;
+
+motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
+
+bool isMotorProtocolDshot(void);
+
+void pwmWriteDshotInt(uint8_t index, uint16_t value);
+void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
+#ifdef USE_DSHOT_TELEMETRY
+bool pwmStartDshotMotorUpdate(void);
+#endif
+void pwmCompleteDshotMotorUpdate(void);
+
+extern bool useBurstDshot;
+
+extern motorDevice_t dshotPwmDevice;
diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c
new file mode 100644
index 0000000000..96e96a3eb9
--- /dev/null
+++ b/src/main/drivers/motor.c
@@ -0,0 +1,253 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 .
+ *
+ * Author: jflyper
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_MOTOR
+
+#include "common/maths.h"
+
+#include "config/feature.h"
+
+#include "drivers/dshot.h" // for DSHOT_ constants in initEscEndpoints; may be gone in the future
+
+#include "drivers/motor.h"
+#include "drivers/pwm_output.h" // for PWM_TYPE_* and others
+#include "drivers/time.h"
+#include "drivers/dshot_dpwm.h"
+
+#include "fc/rc_controls.h" // for flight3DConfig_t
+
+#include "pg/motor.h"
+
+static FAST_RAM_ZERO_INIT motorDevice_t *motorDevice;
+
+void motorShutdown(void)
+{
+ motorDevice->vTable.shutdown();
+ motorDevice->enabled = false;
+ motorDevice->initialized = false;
+ delayMicroseconds(1500);
+}
+
+void motorWriteAll(float *values)
+{
+#ifdef USE_PWM_OUTPUT
+ if (motorDevice->enabled) {
+#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
+ if (!motorDevice->vTable.updateStart()) {
+ return;
+ }
+#endif
+ for (int i = 0; i < motorDevice->count; i++) {
+ motorDevice->vTable.write(i, values[i]);
+ }
+ motorDevice->vTable.updateComplete();
+ }
+#endif
+}
+
+int motorCount(void)
+{
+ return motorDevice->count;
+}
+
+// This is not motor generic anymore; should be moved to analog pwm module
+static void analogInitEndpoints(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;
+ *outputLow = motorConfig()->minthrottle;
+ *outputHigh = motorConfig()->maxthrottle - ((motorConfig()->maxthrottle - motorConfig()->minthrottle) * (1 - outputLimit));
+ }
+}
+
+// End point initialization is called from mixerInit before motorDevInit; can't use vtable...
+void motorInitEndpoints(float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow)
+{
+ switch (motorConfig()->dev.motorPwmProtocol) {
+#ifdef USE_DSHOT
+ case PWM_TYPE_PROSHOT1000:
+ case PWM_TYPE_DSHOT1200:
+ case PWM_TYPE_DSHOT600:
+ case PWM_TYPE_DSHOT300:
+ case PWM_TYPE_DSHOT150:
+ dshotInitEndpoints(outputLimit, outputLow, outputHigh, disarm, deadbandMotor3dHigh, deadbandMotor3dLow);
+ break;
+#endif
+ default:
+ analogInitEndpoints(outputLimit, outputLow, outputHigh, disarm, deadbandMotor3dHigh, deadbandMotor3dLow);
+ break;
+ }
+}
+
+float motorConvertFromExternal(uint16_t externalValue)
+{
+ return motorDevice->vTable.convertExternalToMotor(externalValue);
+}
+
+uint16_t motorConvertToExternal(float motorValue)
+{
+ return motorDevice->vTable.convertMotorToExternal(motorValue);
+}
+
+static bool isDshot = false; // XXX Should go somewhere else
+
+static bool motorEnableNull(void)
+{
+ return false;
+}
+
+static void motorDisableNull(void)
+{
+}
+
+bool motorUpdateStartNull(void)
+{
+ return true;
+}
+
+void motorWriteNull(uint8_t index, float value)
+{
+ UNUSED(index);
+ UNUSED(value);
+}
+
+static void motorWriteIntNull(uint8_t index, uint16_t value)
+{
+ UNUSED(index);
+ UNUSED(value);
+}
+
+void motorUpdateCompleteNull(void)
+{
+}
+
+static void motorShutdownNull(void)
+{
+}
+
+static float motorConvertFromExternalNull(uint16_t value)
+{
+ UNUSED(value);
+ return 0.0f ;
+}
+
+static uint16_t motorConvertToExternalNull(float value)
+{
+ UNUSED(value);
+ return 0;
+}
+
+static const motorVTable_t motorNullVTable = {
+ .enable = motorEnableNull,
+ .disable = motorDisableNull,
+ .updateStart = motorUpdateStartNull,
+ .write = motorWriteNull,
+ .writeInt = motorWriteIntNull,
+ .updateComplete = motorUpdateCompleteNull,
+ .convertExternalToMotor = motorConvertFromExternalNull,
+ .convertMotorToExternal = motorConvertToExternalNull,
+ .shutdown = motorShutdownNull,
+};
+
+static motorDevice_t motorNullDevice = {
+ .initialized = false,
+ .enabled = false,
+};
+
+void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) {
+ memset(motors, 0, sizeof(motors));
+
+ bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
+
+ switch (motorConfig->motorPwmProtocol) {
+ default:
+ case PWM_TYPE_STANDARD:
+ case PWM_TYPE_ONESHOT125:
+ case PWM_TYPE_ONESHOT42:
+ case PWM_TYPE_MULTISHOT:
+ case PWM_TYPE_BRUSHED:
+ motorDevice = motorPwmDevInit(motorConfig, idlePulse, motorCount, useUnsyncedPwm);
+ break;
+
+#ifdef USE_DSHOT
+ case PWM_TYPE_DSHOT150:
+ case PWM_TYPE_DSHOT300:
+ case PWM_TYPE_DSHOT600:
+ case PWM_TYPE_DSHOT1200:
+ case PWM_TYPE_PROSHOT1000:
+ motorDevice = dshotPwmDevInit(motorConfig, idlePulse, motorCount, useUnsyncedPwm);
+ isDshot = true;
+ break;
+#endif
+
+#if 0 // not yet
+ case PWM_TYPE_DSHOT_UART:
+ //motorDevice = dshotSerialInit(motorConfig, idlePulse, motorCount, useUnsyncedPwm);
+ break;
+#endif
+ }
+
+ if (motorDevice) {
+ motorDevice->count = motorCount;
+ motorDevice->initialized = true;
+ motorDevice->enabled = false;
+ } else {
+ motorNullDevice.vTable = motorNullVTable;
+ motorDevice = &motorNullDevice;
+ }
+}
+
+void motorDisable(void)
+{
+ motorDevice->vTable.disable();
+ motorDevice->enabled = false;
+}
+
+void motorEnable(void)
+{
+ if (motorDevice->initialized && motorDevice->vTable.enable()) {
+ motorDevice->enabled = true;
+ }
+}
+
+bool motorIsEnabled(void)
+{
+ return motorDevice->enabled;
+}
+
+bool isMotorProtocolDshot(void)
+{
+ return isDshot;
+}
+#endif // USE_MOTOR
diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h
new file mode 100644
index 0000000000..f3692471ec
--- /dev/null
+++ b/src/main/drivers/motor.h
@@ -0,0 +1,83 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 .
+ *
+ * Author: jflyper
+ */
+
+#pragma once
+
+typedef enum {
+ PWM_TYPE_STANDARD = 0,
+ PWM_TYPE_ONESHOT125,
+ PWM_TYPE_ONESHOT42,
+ PWM_TYPE_MULTISHOT,
+ PWM_TYPE_BRUSHED,
+#ifdef USE_DSHOT
+ PWM_TYPE_DSHOT150,
+ PWM_TYPE_DSHOT300,
+ PWM_TYPE_DSHOT600,
+ PWM_TYPE_DSHOT1200,
+ PWM_TYPE_PROSHOT1000,
+#endif
+ PWM_TYPE_MAX
+} motorPwmProtocolTypes_e;
+
+
+typedef struct motorVTable_s {
+ // Common
+ float (*convertExternalToMotor)(uint16_t externalValue);
+ uint16_t (*convertMotorToExternal)(float motorValue);
+ bool (*enable)(void);
+ void (*disable)(void);
+ bool (*updateStart)(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;
+} motorDevice_t;
+
+void motorWriteNull(uint8_t index, float value);
+bool motorUpdateStartNull(void);
+void motorUpdateCompleteNull(void);
+
+void motorWriteAll(float *values);
+
+void motorInitEndpoints(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);
+bool isMotorProtocolDshot(void);
+
+void motorDisable(void);
+void motorEnable(void);
+bool motorIsEnabled(void);
+void motorShutdown(void); // Replaces stopPwmAllMotors
diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h
index 85ae54eb7c..f36a8faec5 100644
--- a/src/main/drivers/nvic.h
+++ b/src/main/drivers/nvic.h
@@ -26,6 +26,7 @@
#define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_BARO_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_SONAR_EXTI NVIC_BUILD_PRIORITY(2, 0) // maybe increase slightly
+#define NVIC_PRIO_DSHOT_DMA NVIC_BUILD_PRIORITY(2, 1)
#define NVIC_PRIO_TRANSPONDER_DMA NVIC_BUILD_PRIORITY(3, 0)
#define NVIC_PRIO_MPU_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 5bb4b9fd6e..0f65d88d19 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -26,77 +26,16 @@
#include "platform.h"
#ifdef USE_PWM_OUTPUT
-#include "drivers/time.h"
#include "drivers/io.h"
-#include "pwm_output.h"
-#include "timer.h"
+#include "drivers/motor.h"
#include "drivers/pwm_output.h"
+#include "drivers/time.h"
+#include "drivers/timer.h"
#include "pg/motor.h"
-static FAST_RAM_ZERO_INIT pwmWriteFn *pwmWrite;
-static FAST_RAM_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
-static FAST_RAM_ZERO_INIT pwmCompleteWriteFn *pwmCompleteWrite = NULL;
-#ifdef USE_DSHOT_TELEMETRY
-static FAST_RAM_ZERO_INIT pwmStartWriteFn *pwmStartWrite = NULL;
-#endif
-
-#ifdef USE_DSHOT
-#ifdef STM32H7
-// XXX dshotDmaBuffer can be embedded inside dshotBurstDmaBuffer
-DMA_RAM uint32_t dshotDmaBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_SIZE];
-#ifdef USE_DSHOT_DMAR
-DMA_RAM uint32_t dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
-#endif
-#endif
-FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
-#define DSHOT_INITIAL_DELAY_US 10000
-#define DSHOT_COMMAND_DELAY_US 1000
-#define DSHOT_ESCINFO_DELAY_US 12000
-#define DSHOT_BEEP_DELAY_US 100000
-#define DSHOT_MAX_COMMANDS 3
-
-typedef enum {
- DSHOT_COMMAND_STATE_IDLEWAIT, // waiting for motors to go idle
- DSHOT_COMMAND_STATE_STARTDELAY, // initial delay period before a sequence of commands
- DSHOT_COMMAND_STATE_ACTIVE, // actively sending the command (with optional repeated output)
- DSHOT_COMMAND_STATE_POSTDELAY // delay period after the command has been sent
-} dshotCommandState_e;
-
-typedef struct dshotCommandControl_s {
- dshotCommandState_e state;
- uint32_t nextCommandCycleDelay;
- timeUs_t delayAfterCommandUs;
- uint8_t repeats;
- uint8_t command[MAX_SUPPORTED_MOTORS];
-} dshotCommandControl_t;
-
-static timeUs_t dshotCommandPidLoopTimeUs = 125; // default to 8KHz (125us) loop to prevent possible div/0
- // gets set to the actual value when the PID loop is initialized
-
-static dshotCommandControl_t commandQueue[DSHOT_MAX_COMMANDS + 1];
-static uint8_t commandQueueHead;
-static uint8_t commandQueueTail;
-#endif
-
-#ifdef USE_SERVOS
-static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
-#endif
-
-#ifdef USE_BEEPER
-static pwmOutputPort_t beeperPwm;
-static uint16_t freqBeep = 0;
-#endif
-
-static bool pwmMotorsEnabled = false;
-static bool isDshot = false;
-#ifdef USE_DSHOT_DMAR
-FAST_RAM_ZERO_INIT bool useBurstDshot = false;
-#endif
-#ifdef USE_DSHOT_TELEMETRY
-FAST_RAM_ZERO_INIT bool useDshotTelemetry = false;
-#endif
+FAST_RAM_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{
@@ -169,6 +108,8 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware,
*channel->ccr = 0;
}
+static FAST_RAM_ZERO_INIT motorDevice_t motorPwmDevice;
+
static void pwmWriteUnused(uint8_t index, float value)
{
UNUSED(index);
@@ -181,50 +122,9 @@ static void pwmWriteStandard(uint8_t index, float value)
*motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
}
-#ifdef USE_DSHOT
-static FAST_CODE void pwmWriteDshot(uint8_t index, float value)
+void pwmShutdownPulsesForAllMotors(void)
{
- pwmWriteDshotInt(index, lrintf(value));
-}
-
-static FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
-{
- for (int i = 0; i < 16; i++) {
- dmaBuffer[i * stride] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
- packet <<= 1;
- }
- dmaBuffer[16 * stride] = 0;
- dmaBuffer[17 * stride] = 0;
-
- return DSHOT_DMA_BUFFER_SIZE;
-}
-
-static uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
-{
- for (int i = 0; i < 4; i++) {
- dmaBuffer[i * stride] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first
- packet <<= 4; // Shift 4 bits
- }
- dmaBuffer[4 * stride] = 0;
- dmaBuffer[5 * stride] = 0;
-
- return PROSHOT_DMA_BUFFER_SIZE;
-}
-
-void setDshotPidLoopTime(uint32_t pidLoopTime)
-{
- dshotCommandPidLoopTimeUs = pidLoopTime;
-}
-#endif
-
-void pwmWriteMotor(uint8_t index, float value)
-{
- pwmWrite(index, value);
-}
-
-void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
-{
- for (int index = 0; index < motorCount; index++) {
+ for (int index = 0; index < motorPwmDevice.count; 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;
@@ -234,37 +134,19 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
void pwmDisableMotors(void)
{
- pwmShutdownPulsesForAllMotors(MAX_SUPPORTED_MOTORS);
- pwmMotorsEnabled = false;
+ pwmShutdownPulsesForAllMotors();
}
-void pwmEnableMotors(void)
+static motorVTable_t motorPwmVTable;
+bool pwmEnableMotors(void)
{
/* check motors can be enabled */
- pwmMotorsEnabled = (pwmWrite != &pwmWriteUnused);
+ return (motorPwmVTable.write != &pwmWriteUnused);
}
-bool pwmAreMotorsEnabled(void)
+static void pwmCompleteOneshotMotorUpdate(void)
{
- return pwmMotorsEnabled;
-}
-
-#ifdef USE_DSHOT_TELEMETRY
-static bool pwmStartWriteUnused(uint8_t motorCount)
-{
- UNUSED(motorCount);
- return true;
-}
-#endif
-
-static void pwmCompleteWriteUnused(uint8_t motorCount)
-{
- UNUSED(motorCount);
-}
-
-static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
-{
- for (int index = 0; index < motorCount; index++) {
+ for (int index = 0; index < motorPwmDevice.count; index++) {
if (motors[index].forceOverflow) {
timerForceOverflow(motors[index].channel.tim);
}
@@ -274,23 +156,27 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
}
}
-void pwmCompleteMotorUpdate(uint8_t motorCount)
+static float pwmConvertFromExternal(uint16_t externalValue)
{
- pwmCompleteWrite(motorCount);
+ return (float)externalValue;
}
-#ifdef USE_DSHOT_TELEMETRY
-bool pwmStartMotorUpdate(uint8_t motorCount)
+static uint16_t pwmConvertToExternal(float motorValue)
{
- return pwmStartWrite(motorCount);
+ return (uint16_t)motorValue;
}
-#endif
-void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
+static motorVTable_t motorPwmVTable = {
+ .enable = pwmEnableMotors,
+ .disable = pwmDisableMotors,
+ .shutdown = pwmShutdownPulsesForAllMotors,
+ .convertExternalToMotor = pwmConvertFromExternal,
+ .convertMotorToExternal = pwmConvertToExternal,
+};
+
+motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
{
- memset(motors, 0, sizeof(motors));
-
- bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
+ motorPwmDevice.vTable = motorPwmVTable;
float sMin = 0;
float sLen = 0;
@@ -319,45 +205,11 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
useUnsyncedPwm = true;
idlePulse = 0;
break;
-#ifdef USE_DSHOT
- case PWM_TYPE_PROSHOT1000:
- pwmWrite = &pwmWriteDshot;
- loadDmaBuffer = &loadDmaBufferProshot;
- pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
-#ifdef USE_DSHOT_TELEMETRY
- pwmStartWrite = &pwmStartDshotMotorUpdate;
- useDshotTelemetry = motorConfig->useDshotTelemetry;
-#endif
- isDshot = true;
- break;
- case PWM_TYPE_DSHOT1200:
- case PWM_TYPE_DSHOT600:
- case PWM_TYPE_DSHOT300:
- case PWM_TYPE_DSHOT150:
- pwmWrite = &pwmWriteDshot;
- loadDmaBuffer = &loadDmaBufferDshot;
- pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
-#ifdef USE_DSHOT_TELEMETRY
- pwmStartWrite = &pwmStartDshotMotorUpdate;
- useDshotTelemetry = motorConfig->useDshotTelemetry;
-#endif
- isDshot = true;
-#ifdef USE_DSHOT_DMAR
- if (motorConfig->useBurstDshot) {
- useBurstDshot = true;
- }
-#endif
- break;
-#endif
}
- if (!isDshot) {
- pwmWrite = &pwmWriteStandard;
- pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
-#ifdef USE_DSHOT_TELEMETRY
- pwmStartWrite = pwmStartWriteUnused;
-#endif
- }
+ motorPwmDevice.vTable.write = pwmWriteStandard;
+ motorPwmDevice.vTable.updateStart = motorUpdateStartNull;
+ motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const ioTag_t tag = motorConfig->ioTags[motorIndex];
@@ -365,26 +217,15 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
if (timerHardware == NULL) {
/* not enough motors initialised for the mixer or a break in the motors */
- pwmWrite = &pwmWriteUnused;
- pwmCompleteWrite = &pwmCompleteWriteUnused;
+ motorPwmDevice.vTable.write = &pwmWriteUnused;
+ motorPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
/* TODO: block arming and add reason system cannot arm */
- return;
+ return NULL;
}
motors[motorIndex].io = IOGetByTag(tag);
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
-#ifdef USE_DSHOT
- if (isDshot) {
- pwmDshotMotorHardwareConfig(timerHardware,
- motorIndex,
- motorConfig->motorPwmProtocol,
- motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
- motors[motorIndex].enabled = true;
- continue;
- }
-#endif
-
#if defined(STM32F1)
IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
#else
@@ -422,7 +263,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
motors[motorIndex].enabled = true;
}
- pwmMotorsEnabled = true;
+ return &motorPwmDevice;
}
pwmOutputPort_t *pwmGetMotors(void)
@@ -430,274 +271,9 @@ pwmOutputPort_t *pwmGetMotors(void)
return motors;
}
-bool isMotorProtocolDshot(void)
-{
- return isDshot;
-}
-
-#ifdef USE_DSHOT
-uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
-{
- switch (pwmProtocolType) {
- case(PWM_TYPE_PROSHOT1000):
- return MOTOR_PROSHOT1000_HZ;
- case(PWM_TYPE_DSHOT1200):
- return MOTOR_DSHOT1200_HZ;
- case(PWM_TYPE_DSHOT600):
- return MOTOR_DSHOT600_HZ;
- case(PWM_TYPE_DSHOT300):
- return MOTOR_DSHOT300_HZ;
- default:
- case(PWM_TYPE_DSHOT150):
- return MOTOR_DSHOT150_HZ;
- }
-}
-
-bool allMotorsAreIdle(uint8_t motorCount)
-{
- bool allMotorsIdle = true;
- for (unsigned i = 0; i < motorCount; i++) {
- const motorDmaOutput_t *motor = getMotorDmaOutput(i);
- if (motor->value) {
- allMotorsIdle = false;
- }
- }
-
- return allMotorsIdle;
-}
-
-FAST_CODE bool pwmDshotCommandQueueFull()
-{
- return (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueTail;
-}
-
-FAST_CODE bool pwmDshotCommandIsQueued(void)
-{
- return commandQueueHead != commandQueueTail;
-}
-
-static FAST_CODE bool isLastDshotCommand(void)
-{
- return ((commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueHead);
-}
-
-FAST_CODE bool pwmDshotCommandIsProcessing(void)
-{
- if (!pwmDshotCommandIsQueued()) {
- return false;
- }
- dshotCommandControl_t* command = &commandQueue[commandQueueTail];
- const bool commandIsProcessing = command->state == DSHOT_COMMAND_STATE_STARTDELAY
- || command->state == DSHOT_COMMAND_STATE_ACTIVE
- || (command->state == DSHOT_COMMAND_STATE_POSTDELAY && !isLastDshotCommand());
- return commandIsProcessing;
-}
-
-static FAST_CODE bool pwmDshotCommandQueueUpdate(void)
-{
- if (pwmDshotCommandIsQueued()) {
- commandQueueTail = (commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1);
- if (pwmDshotCommandIsQueued()) {
- // There is another command in the queue so update it so it's ready to output in
- // sequence. It can go directly to the DSHOT_COMMAND_STATE_ACTIVE state and bypass
- // the DSHOT_COMMAND_STATE_IDLEWAIT and DSHOT_COMMAND_STATE_STARTDELAY states.
- dshotCommandControl_t* nextCommand = &commandQueue[commandQueueTail];
- nextCommand->state = DSHOT_COMMAND_STATE_ACTIVE;
- nextCommand->nextCommandCycleDelay = 0;
- return true;
- }
- }
- return false;
-}
-
-static FAST_CODE uint32_t dshotCommandCyclesFromTime(timeUs_t delayUs)
-{
- // Find the minimum number of motor output cycles needed to
- // provide at least delayUs time delay
- uint32_t ret = delayUs / dshotCommandPidLoopTimeUs;
- if (delayUs % dshotCommandPidLoopTimeUs) {
- ret++;
- }
- return ret;
-}
-
-static dshotCommandControl_t* addCommand()
-{
- int newHead = (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1);
- if (newHead == commandQueueTail) {
- return NULL;
- }
- dshotCommandControl_t* control = &commandQueue[commandQueueHead];
- commandQueueHead = newHead;
- return control;
-}
-
-
-void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
-{
- if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandQueueFull()) {
- return;
- }
-
- uint8_t repeats = 1;
- timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
-
- switch (command) {
- case DSHOT_CMD_SPIN_DIRECTION_1:
- case DSHOT_CMD_SPIN_DIRECTION_2:
- case DSHOT_CMD_3D_MODE_OFF:
- case DSHOT_CMD_3D_MODE_ON:
- case DSHOT_CMD_SAVE_SETTINGS:
- case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
- case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
- case DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE:
- case DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY:
- repeats = 10;
- break;
- case DSHOT_CMD_BEACON1:
- case DSHOT_CMD_BEACON2:
- case DSHOT_CMD_BEACON3:
- case DSHOT_CMD_BEACON4:
- case DSHOT_CMD_BEACON5:
- delayAfterCommandUs = DSHOT_BEEP_DELAY_US;
- break;
- default:
- break;
- }
-
- if (blocking) {
- delayMicroseconds(DSHOT_INITIAL_DELAY_US - DSHOT_COMMAND_DELAY_US);
- for (; repeats; repeats--) {
- delayMicroseconds(DSHOT_COMMAND_DELAY_US);
-
-#ifdef USE_DSHOT_TELEMETRY
- timeUs_t currentTimeUs = micros();
- while (!pwmStartDshotMotorUpdate(motorCount) &&
- cmpTimeUs(micros(), currentTimeUs) < 1000);
-#endif
- for (uint8_t i = 0; i < motorCount; i++) {
- if ((i == index) || (index == ALL_MOTORS)) {
- motorDmaOutput_t *const motor = getMotorDmaOutput(i);
- motor->requestTelemetry = true;
- pwmWriteDshotInt(i, command);
- }
- }
-
- pwmCompleteDshotMotorUpdate(0);
- }
- delayMicroseconds(delayAfterCommandUs);
- } else {
- dshotCommandControl_t *commandControl = addCommand();
- if (commandControl) {
- commandControl->repeats = repeats;
- commandControl->delayAfterCommandUs = delayAfterCommandUs;
- for (unsigned i = 0; i < motorCount; i++) {
- if (index == i || index == ALL_MOTORS) {
- commandControl->command[i] = command;
- } else {
- commandControl->command[i] = DSHOT_CMD_MOTOR_STOP;
- }
- }
- if (allMotorsAreIdle(motorCount)) {
- // we can skip the motors idle wait state
- commandControl->state = DSHOT_COMMAND_STATE_STARTDELAY;
- commandControl->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
- } else {
- commandControl->state = DSHOT_COMMAND_STATE_STARTDELAY;
- commandControl->nextCommandCycleDelay = 0; // will be set after idle wait completes
- }
- }
- }
-}
-
-uint8_t pwmGetDshotCommand(uint8_t index)
-{
- return commandQueue[commandQueueTail].command[index];
-}
-
-// This function is used to synchronize the dshot command output timing with
-// the normal motor output timing tied to the PID loop frequency. A "true" result
-// 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 pwmDshotCommandOutputIsEnabled(uint8_t motorCount)
-{
- dshotCommandControl_t* command = &commandQueue[commandQueueTail];
- switch (command->state) {
- case DSHOT_COMMAND_STATE_IDLEWAIT:
- if (allMotorsAreIdle(motorCount)) {
- command->state = DSHOT_COMMAND_STATE_STARTDELAY;
- command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
- }
- break;
-
- case DSHOT_COMMAND_STATE_STARTDELAY:
- if (command->nextCommandCycleDelay-- > 1) {
- return false; // Delay motor output until the start of the command seequence
- }
- command->state = DSHOT_COMMAND_STATE_ACTIVE;
- command->nextCommandCycleDelay = 0; // first iteration of the repeat happens now
- FALLTHROUGH;
-
- case DSHOT_COMMAND_STATE_ACTIVE:
- if (command->nextCommandCycleDelay-- > 1) {
- return false; // Delay motor output until the next command repeat
- }
-
- command->repeats--;
- if (command->repeats) {
- command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_COMMAND_DELAY_US);
- } else {
- command->state = DSHOT_COMMAND_STATE_POSTDELAY;
- command->nextCommandCycleDelay = dshotCommandCyclesFromTime(command->delayAfterCommandUs);
- if (!isLastDshotCommand() && command->nextCommandCycleDelay > 0) {
- // Account for the 1 extra motor output loop between commands.
- // Otherwise the inter-command delay will be DSHOT_COMMAND_DELAY_US + 1 loop.
- command->nextCommandCycleDelay--;
- }
- }
- break;
-
- case DSHOT_COMMAND_STATE_POSTDELAY:
- if (command->nextCommandCycleDelay-- > 1) {
- return false; // Delay motor output until the end of the post-command delay
- }
- if (pwmDshotCommandQueueUpdate()) {
- // Will be true if the command queue is not empty and we
- // want to wait for the next command to start in sequence.
- return false;
- }
- }
-
- return true;
-}
-
-FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor)
-{
- uint16_t packet = (motor->value << 1) | (motor->requestTelemetry ? 1 : 0);
- motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
-
- // compute checksum
- int csum = 0;
- int csum_data = packet;
- for (int i = 0; i < 3; i++) {
- csum ^= csum_data; // xor data by nibbles
- csum_data >>= 4;
- }
- // append checksum
-#ifdef USE_DSHOT_TELEMETRY
- if (useDshotTelemetry) {
- csum = ~csum;
- }
-#endif
- csum &= 0xf;
- packet = (packet << 4) | csum;
-
- return packet;
-}
-#endif // USE_DSHOT
-
#ifdef USE_SERVOS
+static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
+
void pwmWriteServo(uint8_t index, float value)
{
if (index < MAX_SUPPORTED_SERVOS && servos[index].channel.ccr) {
@@ -735,50 +311,5 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
servos[servoIndex].enabled = true;
}
}
-
#endif // USE_SERVOS
-
-#ifdef USE_BEEPER
-void pwmWriteBeeper(bool onoffBeep)
-{
- if (!beeperPwm.io) {
- return;
- }
-
- if (onoffBeep == true) {
- *beeperPwm.channel.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
- beeperPwm.enabled = true;
- } else {
- *beeperPwm.channel.ccr = 0;
- beeperPwm.enabled = false;
- }
-}
-
-void pwmToggleBeeper(void)
-{
- pwmWriteBeeper(!beeperPwm.enabled);
-}
-
-void beeperPwmInit(const ioTag_t tag, uint16_t frequency)
-{
- const timerHardware_t *timer = timerGetByTag(tag);
- IO_t beeperIO = IOGetByTag(tag);
-
- if (beeperIO && timer) {
- beeperPwm.io = beeperIO;
- IOInit(beeperPwm.io, OWNER_BEEPER, RESOURCE_INDEX(0));
-#if defined(STM32F1)
- IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
-#else
- IOConfigGPIOAF(beeperPwm.io, IOCFG_AF_PP, timer->alternateFunction);
-#endif
- freqBeep = frequency;
- pwmOutConfig(&beeperPwm.channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
-
- *beeperPwm.channel.ccr = 0;
- beeperPwm.enabled = false;
- }
-}
-#endif // USE_BEEPER
-#endif //USE_PWM_OUTPUT
-
+#endif // USE_PWM_OUTPUT
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index 6c4a65d89e..fb681f1760 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -25,187 +25,20 @@
#include "common/time.h"
#include "drivers/io_types.h"
+#include "drivers/motor.h"
#include "drivers/timer.h"
#define BRUSHED_MOTORS_PWM_RATE 16000
#define BRUSHLESS_MOTORS_PWM_RATE 480
#define ALL_MOTORS 255
-#define DSHOT_MAX_COMMAND 47
-#define DSHOT_TELEMETRY_INPUT_LEN 32
-#define PROSHOT_TELEMETRY_INPUT_LEN 8
#define MOTOR_OUTPUT_LIMIT_PERCENT_MIN 1
#define MOTOR_OUTPUT_LIMIT_PERCENT_MAX 100
-/*
- DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings
-
- 3D Mode:
- 0 = stop
- 48 (low) - 1047 (high) -> negative direction
- 1048 (low) - 2047 (high) -> positive direction
- */
-
-typedef enum {
- DSHOT_CMD_MOTOR_STOP = 0,
- DSHOT_CMD_BEACON1,
- DSHOT_CMD_BEACON2,
- DSHOT_CMD_BEACON3,
- DSHOT_CMD_BEACON4,
- DSHOT_CMD_BEACON5,
- DSHOT_CMD_ESC_INFO, // V2 includes settings
- DSHOT_CMD_SPIN_DIRECTION_1,
- DSHOT_CMD_SPIN_DIRECTION_2,
- DSHOT_CMD_3D_MODE_OFF,
- DSHOT_CMD_3D_MODE_ON,
- DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented
- DSHOT_CMD_SAVE_SETTINGS,
- DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
- DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
- DSHOT_CMD_LED0_ON, // BLHeli32 only
- DSHOT_CMD_LED1_ON, // BLHeli32 only
- DSHOT_CMD_LED2_ON, // BLHeli32 only
- DSHOT_CMD_LED3_ON, // BLHeli32 only
- DSHOT_CMD_LED0_OFF, // BLHeli32 only
- DSHOT_CMD_LED1_OFF, // BLHeli32 only
- DSHOT_CMD_LED2_OFF, // BLHeli32 only
- DSHOT_CMD_LED3_OFF, // BLHeli32 only
- DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF = 30, // KISS audio Stream mode on/Off
- DSHOT_CMD_SILENT_MODE_ON_OFF = 31, // KISS silent Mode on/Off
- DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE = 32,
- DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY = 33,
- DSHOT_CMD_MAX = 47
-} dshotCommands_e;
-
-#define DSHOT_MIN_THROTTLE 48
-#define DSHOT_MAX_THROTTLE 2047
-
-typedef enum {
- PWM_TYPE_STANDARD = 0,
- PWM_TYPE_ONESHOT125,
- PWM_TYPE_ONESHOT42,
- PWM_TYPE_MULTISHOT,
- PWM_TYPE_BRUSHED,
-#ifdef USE_DSHOT
- PWM_TYPE_DSHOT150,
- PWM_TYPE_DSHOT300,
- PWM_TYPE_DSHOT600,
- PWM_TYPE_DSHOT1200,
- PWM_TYPE_PROSHOT1000,
-#endif
- PWM_TYPE_MAX
-} motorPwmProtocolTypes_e;
-
#define PWM_TIMER_1MHZ MHZ_TO_HZ(1)
-#ifdef USE_DSHOT
-#define MAX_DMA_TIMERS 8
-
-#define MOTOR_DSHOT1200_HZ MHZ_TO_HZ(24)
-#define MOTOR_DSHOT600_HZ MHZ_TO_HZ(12)
-#define MOTOR_DSHOT300_HZ MHZ_TO_HZ(6)
-#define MOTOR_DSHOT150_HZ MHZ_TO_HZ(3)
-
-#define MOTOR_BIT_0 7
-#define MOTOR_BIT_1 14
-#define MOTOR_BITLENGTH 20
-
-#define MOTOR_PROSHOT1000_HZ MHZ_TO_HZ(24)
-#define PROSHOT_BASE_SYMBOL 24 // 1uS
-#define PROSHOT_BIT_WIDTH 3
-#define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS
-
-#define DSHOT_TELEMETRY_DEADTIME_US (2 * 30 + 10) // 2 * 30uS to switch lines plus 10us grace period
-#endif
-
-
-#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
-#define PROSHOT_DMA_BUFFER_SIZE 6 /* resolution + frame reset (2us) */
-
-typedef struct {
- TIM_TypeDef *timer;
-#if defined(USE_DSHOT) && defined(USE_DSHOT_DMAR)
-#if defined(STM32F7) || defined(STM32H7)
- TIM_HandleTypeDef timHandle;
- DMA_HandleTypeDef hdma_tim;
-#endif
-#ifdef STM32F3
- DMA_Channel_TypeDef *dmaBurstRef;
-#else
- DMA_Stream_TypeDef *dmaBurstRef;
-#endif
- uint16_t dmaBurstLength;
-#ifdef STM32H7
- uint32_t *dmaBurstBuffer;
-#else
- uint32_t dmaBurstBuffer[DSHOT_DMA_BUFFER_SIZE * 4];
-#endif
- timeUs_t inputDirectionStampUs;
-#endif
- uint16_t timerDmaSources;
-} motorDmaTimer_t;
-
-typedef struct {
- ioTag_t ioTag;
- const timerHardware_t *timerHardware;
- uint16_t value;
-#ifdef USE_DSHOT
- uint16_t timerDmaSource;
- uint8_t timerDmaIndex;
- bool configured;
-#ifdef STM32H7
- TIM_HandleTypeDef TimHandle;
- DMA_HandleTypeDef hdma_tim;
-#endif
- uint8_t output;
- uint8_t index;
-#ifdef USE_DSHOT_TELEMETRY
- bool useProshot;
- volatile bool isInput;
- volatile bool hasTelemetry;
- uint16_t dshotTelemetryValue;
- timeDelta_t dshotTelemetryDeadtimeUs;
- bool dshotTelemetryActive;
-#ifdef USE_HAL_DRIVER
- LL_TIM_OC_InitTypeDef ocInitStruct;
- LL_TIM_IC_InitTypeDef icInitStruct;
- LL_DMA_InitTypeDef dmaInitStruct;
- uint32_t llChannel;
-#else
- TIM_OCInitTypeDef ocInitStruct;
- TIM_ICInitTypeDef icInitStruct;
- DMA_InitTypeDef dmaInitStruct;
-#endif
- uint8_t dmaInputLen;
-#endif
-#ifdef STM32F3
- DMA_Channel_TypeDef *dmaRef;
-#else
- DMA_Stream_TypeDef *dmaRef;
-#endif
-#endif
- motorDmaTimer_t *timer;
- volatile bool requestTelemetry;
-#ifdef USE_DSHOT_TELEMETRY
- uint32_t dmaBuffer[DSHOT_TELEMETRY_INPUT_LEN];
-#else
-#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
- uint32_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE];
-#elif defined(STM32H7)
- uint32_t *dmaBuffer;
-#else
- uint8_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE];
-#endif
-#endif
-} motorDmaOutput_t;
-
-motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
-
struct timerHardware_s;
-typedef void pwmWriteFn(uint8_t index, float value); // function pointer used to write motors
-typedef void pwmCompleteWriteFn(uint8_t motorCount); // function pointer used after motors are written
-typedef bool pwmStartWriteFn(uint8_t motorCount); // function pointer used before motors are written
typedef struct {
volatile timCCR_t *ccr;
@@ -221,14 +54,10 @@ typedef struct {
IO_t io;
} pwmOutputPort_t;
-extern bool useBurstDshot;
-#ifdef USE_DSHOT_TELEMETRY
-extern bool useDshotTelemetry;
-extern uint32_t dshotInvalidPacketCount;
-#endif
+extern FAST_RAM_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
struct motorDevConfig_s;
-void motorDevInit(const struct motorDevConfig_s *motorDevConfig, uint16_t idlePulse, uint8_t motorCount);
+motorDevice_t *motorPwmDevInit(const struct motorDevConfig_s *motorDevConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm);
typedef struct servoDevConfig_s {
// PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)
@@ -241,61 +70,9 @@ void servoDevInit(const servoDevConfig_t *servoDevConfig);
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
-bool isMotorProtocolDshot(void);
-
-#ifdef USE_DSHOT
-typedef uint8_t loadDmaBufferFn(uint32_t *dmaBuffer, int stride, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation
-
-uint16_t prepareDshotPacket(motorDmaOutput_t *const motor);
-
-#ifdef STM32H7
-extern DMA_RAM uint32_t dshotDmaBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_SIZE];
-#ifdef USE_DSHOT_DMAR
-extern DMA_RAM uint32_t dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
-#endif
-#endif
-
-extern loadDmaBufferFn *loadDmaBuffer;
-
-uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
-void pwmWriteDshotCommandControl(uint8_t index);
-void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking);
-void pwmWriteDshotInt(uint8_t index, uint16_t value);
-void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
-#ifdef USE_DSHOT_TELEMETRY
-bool pwmStartDshotMotorUpdate(uint8_t motorCount);
-#endif
-void pwmCompleteDshotMotorUpdate(uint8_t motorCount);
-
-bool pwmDshotCommandIsQueued(void);
-bool pwmDshotCommandIsProcessing(void);
-uint8_t pwmGetDshotCommand(uint8_t index);
-bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount);
-uint16_t getDshotTelemetry(uint8_t index);
-bool isDshotMotorTelemetryActive(uint8_t motorIndex);
-void setDshotPidLoopTime(uint32_t pidLoopTime);
-#ifdef USE_DSHOT_TELEMETRY_STATS
-int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex);
-#endif
-
-#endif
-
-#ifdef USE_BEEPER
-void pwmWriteBeeper(bool onoffBeep);
-void pwmToggleBeeper(void);
-void beeperPwmInit(const ioTag_t tag, uint16_t frequency);
-#endif
void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion);
-void pwmWriteMotor(uint8_t index, float value);
-void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
-void pwmCompleteMotorUpdate(uint8_t motorCount);
-bool pwmStartMotorUpdate(uint8_t motorCount);
-
void pwmWriteServo(uint8_t index, float value);
pwmOutputPort_t *pwmGetMotors(void);
bool pwmIsSynced(void);
-void pwmDisableMotors(void);
-void pwmEnableMotors(void);
-bool pwmAreMotorsEnabled(void);
diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c
index 3891fb98b5..cc38747661 100644
--- a/src/main/drivers/pwm_output_dshot.c
+++ b/src/main/drivers/pwm_output_dshot.c
@@ -42,6 +42,9 @@
#endif
#include "pwm_output.h"
+#include "drivers/dshot.h"
+#include "drivers/dshot_dpwm.h"
+#include "drivers/dshot_command.h"
#include "pwm_output_dshot_shared.h"
@@ -143,13 +146,11 @@ FAST_CODE void pwmDshotSetDirectionOutput(
}
-void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
+void pwmCompleteDshotMotorUpdate(void)
{
- UNUSED(motorCount);
-
/* If there is a dshot command loaded up, time it correctly with motor update*/
- if (pwmDshotCommandIsQueued()) {
- if (!pwmDshotCommandOutputIsEnabled(motorCount)) {
+ if (!dshotCommandQueueEmpty()) {
+ if (!dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
return;
}
}
@@ -336,6 +337,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
if (useBurstDshot) {
dmaInit(timerHardware->dmaTimUPIrqHandler, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
+ motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
+
#if defined(STM32F3)
DMAINIT.DMA_MemoryBaseAddr = (uint32_t)motor->timer->dmaBurstBuffer;
DMAINIT.DMA_DIR = DMA_DIR_PeripheralDST;
@@ -361,6 +364,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
{
dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+ motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
+
#if defined(STM32F3)
DMAINIT.DMA_MemoryBaseAddr = (uint32_t)motor->dmaBuffer;
DMAINIT.DMA_DIR = DMA_DIR_PeripheralDST;
@@ -398,11 +403,11 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
#endif
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
- dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(2, 1), motor->index);
+ dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
} else
#endif
{
- dmaSetHandler(dmaGetIdentifier(dmaRef), motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(2, 1), motor->index);
+ dmaSetHandler(dmaGetIdentifier(dmaRef), motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
}
TIM_Cmd(timer, ENABLE);
diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c
index e714cc05ad..0bf36f67e3 100644
--- a/src/main/drivers/pwm_output_dshot_hal.c
+++ b/src/main/drivers/pwm_output_dshot_hal.c
@@ -28,8 +28,14 @@
#include "build/debug.h"
+#include "common/time.h"
+
#include "drivers/dma.h"
#include "drivers/dma_reqmap.h"
+#include "drivers/motor.h"
+#include "drivers/dshot.h"
+#include "drivers/dshot_dpwm.h"
+#include "drivers/dshot_command.h"
#include "drivers/io.h"
#include "drivers/nvic.h"
#include "drivers/rcc.h"
@@ -122,15 +128,11 @@ void pwmDshotSetDirectionOutput(
}
-FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
+FAST_CODE void pwmCompleteDshotMotorUpdate(void)
{
- UNUSED(motorCount);
-
/* If there is a dshot command loaded up, time it correctly with motor update*/
- if (pwmDshotCommandIsQueued()) {
- if (!pwmDshotCommandOutputIsEnabled(motorCount)) {
- return;
- }
+ if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
+ return;
}
for (int i = 0; i < dmaMotorTimerCount; i++) {
@@ -317,6 +319,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
if (useBurstDshot) {
dmaInit(timerHardware->dmaTimUPIrqHandler, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
+ motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
+
DMAINIT.Channel = dmaChannel;
DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->timer->dmaBurstBuffer;
DMAINIT.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_FULL;
@@ -326,6 +330,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
{
dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
+ motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
+
DMAINIT.Channel = dmaChannel;
DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->dmaBuffer;
DMAINIT.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_1_4;
@@ -359,11 +365,11 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
#endif
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
- dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(2, 1), motor->index);
+ dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
} else
#endif
{
- dmaSetHandler(dmaGetIdentifier(dmaRef), motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(2, 1), motor->index);
+ dmaSetHandler(dmaGetIdentifier(dmaRef), motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
}
LL_TIM_OC_Init(timer, channel, &OCINIT);
diff --git a/src/main/drivers/pwm_output_dshot_hal_hal.c b/src/main/drivers/pwm_output_dshot_hal_hal.c
index 51ad4209a2..3495d13f5d 100644
--- a/src/main/drivers/pwm_output_dshot_hal_hal.c
+++ b/src/main/drivers/pwm_output_dshot_hal_hal.c
@@ -31,6 +31,9 @@
#include "drivers/io.h"
#include "timer.h"
#include "pwm_output.h"
+#include "drivers/dshot.h"
+#include "drivers/dshot_dpwm.h"
+#include "drivers/dshot_command.h"
#include "drivers/nvic.h"
#include "dma.h"
#include "rcc.h"
@@ -138,7 +141,7 @@ void pwmBurstDMAStart(TIM_HandleTypeDef *htim, uint32_t BurstBaseAddress, uint32
__HAL_TIM_ENABLE_DMA(htim, BurstRequestSrc);
}
-void pwmWriteDshotInt(uint8_t index, uint16_t value)
+FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
{
motorDmaOutput_t *const motor = &dmaMotors[index];
@@ -147,10 +150,10 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value)
}
/*If there is a command ready to go overwrite the value and send that instead*/
- if (pwmDshotCommandIsProcessing()) {
+ if (dshotCommandIsProcessing()) {
value = pwmGetDshotCommand(index);
if (value) {
- motor->requestTelemetry = true;
+ motor->protocolControl.requestTelemetry = true;
}
}
@@ -164,9 +167,9 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value)
return;
}
- motor->value = value;
+ motor->protocolControl.value = value;
- uint16_t packet = prepareDshotPacket(motor);
+ uint16_t packet = prepareDshotPacket(&motor->protocolControl);
uint8_t bufferSize;
#ifdef USE_DSHOT_DMAR
@@ -182,16 +185,12 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value)
}
}
-void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
+void pwmCompleteDshotMotorUpdate(void)
{
- UNUSED(motorCount);
-
// If there is a dshot command loaded up, time it correctly with motor update
- if (pwmDshotCommandIsQueued()) {
- if (!pwmDshotCommandOutputIsEnabled(motorCount)) {
- return;
- }
+ if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
+ return;
}
#ifdef USE_DSHOT_DMAR
@@ -426,12 +425,12 @@ P - High - High -
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
dmaInit(identifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
- dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), timerIndex);
+ dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, timerIndex);
} else
#endif
{
dmaInit(identifier, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
- dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
+ dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motorIndex);
}
// Start the timer channel now.
diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/main/drivers/pwm_output_dshot_shared.c
index 9553239e1d..855206098f 100644
--- a/src/main/drivers/pwm_output_dshot_shared.c
+++ b/src/main/drivers/pwm_output_dshot_shared.c
@@ -43,6 +43,9 @@
#endif
#include "pwm_output.h"
+#include "drivers/dshot.h"
+#include "drivers/dshot_dpwm.h"
+#include "drivers/dshot_command.h"
#include "pwm_output_dshot_shared.h"
@@ -106,7 +109,7 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
}
/*If there is a command ready to go overwrite the value and send that instead*/
- if (pwmDshotCommandIsProcessing()) {
+ if (dshotCommandIsProcessing()) {
value = pwmGetDshotCommand(index);
#ifdef USE_DSHOT_TELEMETRY
// reset telemetry debug statistics every time telemetry is enabled
@@ -116,13 +119,13 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
}
#endif
if (value) {
- motor->requestTelemetry = true;
+ motor->protocolControl.requestTelemetry = true;
}
}
- motor->value = value;
+ motor->protocolControl.value = value;
- uint16_t packet = prepareDshotPacket(motor);
+ uint16_t packet = prepareDshotPacket(&motor->protocolControl);
uint8_t bufferSize;
#ifdef USE_DSHOT_DMAR
@@ -238,7 +241,7 @@ void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool pac
}
#endif // USE_DSHOT_TELEMETRY_STATS
-bool pwmStartDshotMotorUpdate(uint8_t motorCount)
+bool pwmStartDshotMotorUpdate(void)
{
if (!useDshotTelemetry) {
return true;
@@ -246,7 +249,7 @@ bool pwmStartDshotMotorUpdate(uint8_t motorCount)
#ifdef USE_DSHOT_TELEMETRY_STATS
const timeMs_t currentTimeMs = millis();
#endif
- for (int i = 0; i < motorCount; i++) {
+ for (int i = 0; i < dshotPwmDevice.count; i++) {
if (dmaMotors[i].hasTelemetry) {
#ifdef STM32F7
uint32_t edges = LL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef);
@@ -296,7 +299,7 @@ bool pwmStartDshotMotorUpdate(uint8_t motorCount)
}
pwmDshotSetDirectionOutput(&dmaMotors[i], true);
}
- dshotEnableChannels(motorCount);
+ dshotEnableChannels(dshotPwmDevice.count);
return true;
}
@@ -305,6 +308,16 @@ bool isDshotMotorTelemetryActive(uint8_t motorIndex)
return dmaMotors[motorIndex].dshotTelemetryActive;
}
+bool isDshotTelemetryActive(void)
+{
+ for (unsigned i = 0; i < dshotPwmDevice.count; i++) {
+ if (!isDshotMotorTelemetryActive(i)) {
+ return false;
+ }
+ }
+ return true;
+}
+
#ifdef USE_DSHOT_TELEMETRY_STATS
int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex)
{
diff --git a/src/main/drivers/pwm_output_dshot_shared.h b/src/main/drivers/pwm_output_dshot_shared.h
index a2018e1eb5..d2f50b72cb 100644
--- a/src/main/drivers/pwm_output_dshot_shared.h
+++ b/src/main/drivers/pwm_output_dshot_shared.h
@@ -66,7 +66,7 @@ FAST_CODE void pwmDshotSetDirectionOutput(
#endif
);
-bool pwmStartDshotMotorUpdate(uint8_t motorCount);
+bool pwmStartDshotMotorUpdate(void);
#endif
#endif
diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c
index bdd44fcfa3..cad6692782 100644
--- a/src/main/drivers/serial_escserial.c
+++ b/src/main/drivers/serial_escserial.c
@@ -947,7 +947,9 @@ bool escEnablePassthrough(serialPort_t *escPassthroughPort, const motorDevConfig
LED0_OFF;
LED1_OFF;
//StopPwmAllMotors();
- pwmDisableMotors();
+ // XXX Review effect of motor refactor
+ //pwmDisableMotors();
+ motorDisable();
passPort = escPassthroughPort;
uint32_t escBaudrate;
diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c
index 4e0d077754..1481e117ef 100644
--- a/src/main/drivers/sound_beeper.c
+++ b/src/main/drivers/sound_beeper.c
@@ -34,6 +34,52 @@
static IO_t beeperIO = DEFIO_IO(NONE);
static bool beeperInverted = false;
static uint16_t beeperFrequency = 0;
+
+#ifdef USE_PWM_OUTPUT
+static pwmOutputPort_t beeperPwm;
+static uint16_t freqBeep = 0;
+
+static void pwmWriteBeeper(bool on)
+{
+ if (!beeperPwm.io) {
+ return;
+ }
+
+ if (on) {
+ *beeperPwm.channel.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
+ beeperPwm.enabled = true;
+ } else {
+ *beeperPwm.channel.ccr = 0;
+ beeperPwm.enabled = false;
+ }
+}
+
+static void pwmToggleBeeper(void)
+{
+ pwmWriteBeeper(!beeperPwm.enabled);
+}
+
+static void beeperPwmInit(const ioTag_t tag, uint16_t frequency)
+{
+ const timerHardware_t *timer = timerGetByTag(tag);
+ IO_t beeperIO = IOGetByTag(tag);
+
+ if (beeperIO && timer) {
+ beeperPwm.io = beeperIO;
+ IOInit(beeperPwm.io, OWNER_BEEPER, RESOURCE_INDEX(0));
+#if defined(STM32F1)
+ IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
+#else
+ IOConfigGPIOAF(beeperPwm.io, IOCFG_AF_PP, timer->alternateFunction);
+#endif
+ freqBeep = frequency;
+ pwmOutConfig(&beeperPwm.channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
+
+ *beeperPwm.channel.ccr = 0;
+ beeperPwm.enabled = false;
+ }
+}
+#endif
#endif
void systemBeep(bool onoff)
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 49ca8900ab..bad9337ee0 100644
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -33,6 +33,8 @@
#include "config/config_eeprom.h"
#include "config/feature.h"
+#include "drivers/dshot_command.h"
+#include "drivers/motor.h"
#include "drivers/system.h"
#include "fc/config.h"
diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index 75385e0867..dad78747f2 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -40,7 +40,10 @@
#include "config/feature.h"
+#include "drivers/dshot.h"
+#include "drivers/dshot_command.h"
#include "drivers/light_led.h"
+#include "drivers/motor.h"
#include "drivers/sound_beeper.h"
#include "drivers/system.h"
#include "drivers/time.h"
@@ -368,7 +371,7 @@ void disarm(void)
BEEP_OFF;
#ifdef USE_DSHOT
if (isMotorProtocolDshot() && flipOverAfterCrashActive && !featureIsEnabled(FEATURE_3D)) {
- pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
+ dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
}
#endif
flipOverAfterCrashActive = false;
@@ -419,7 +422,7 @@ void tryArm(void)
if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) {
flipOverAfterCrashActive = false;
if (!featureIsEnabled(FEATURE_3D)) {
- pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
+ dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
}
} else {
flipOverAfterCrashActive = true;
@@ -427,7 +430,7 @@ void tryArm(void)
runawayTakeoffCheckDisabled = false;
#endif
if (!featureIsEnabled(FEATURE_3D)) {
- pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false);
+ dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false);
}
}
}
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index 16bfb68249..76d7f5f3f1 100644
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -526,7 +526,7 @@ void init(void)
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
idlePulse = 0; // brushed motors
}
-#ifdef USE_PWM_OUTPUT
+#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. */
@@ -909,8 +909,8 @@ void init(void)
rcdeviceInit();
#endif // USE_RCDEVICE
-#ifdef USE_PWM_OUTPUT
- pwmEnableMotors();
+#ifdef USE_MOTOR
+ motorEnable();
#endif
#ifdef USE_PERSISTENT_STATS
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 8fab75fbd1..62c3bbf8d0 100644
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -37,8 +37,7 @@
#include "pg/motor.h"
#include "pg/rx.h"
-#include "drivers/pwm_output.h"
-#include "drivers/pwm_esc_detect.h"
+#include "drivers/motor.h"
#include "drivers/time.h"
#include "drivers/io.h"
@@ -94,7 +93,6 @@ static motorMixer_t launchControlMixer[MAX_SUPPORTED_MOTORS];
static FAST_RAM_ZERO_INIT int throttleAngleCorrection;
-
static const motorMixer_t mixerQuadX[] = {
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
@@ -334,45 +332,7 @@ void initEscEndpoints(void)
motorOutputLimit = currentPidProfile->motor_output_limit / 100.0f;
}
- // Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet
- switch (motorConfig()->dev.motorPwmProtocol) {
-#ifdef USE_DSHOT
- case PWM_TYPE_PROSHOT1000:
- case PWM_TYPE_DSHOT1200:
- case PWM_TYPE_DSHOT600:
- case PWM_TYPE_DSHOT300:
- case PWM_TYPE_DSHOT150:
- {
- float outputLimitOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) * (1 - motorOutputLimit);
- disarmMotorOutput = DSHOT_CMD_MOTOR_STOP;
- if (featureIsEnabled(FEATURE_3D)) {
- motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
- motorOutputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset / 2;
- deadbandMotor3dHigh = DSHOT_3D_FORWARD_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_3D_FORWARD_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
- deadbandMotor3dLow = DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - outputLimitOffset / 2;
- } else {
- motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
- motorOutputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset;
- }
- }
-
- break;
-#endif
- default:
- if (featureIsEnabled(FEATURE_3D)) {
- float outputLimitOffset = (flight3DConfig()->limit3d_high - flight3DConfig()->limit3d_low) * (1 - motorOutputLimit) / 2;
- disarmMotorOutput = flight3DConfig()->neutral3d;
- motorOutputLow = flight3DConfig()->limit3d_low + outputLimitOffset;
- motorOutputHigh = flight3DConfig()->limit3d_high - outputLimitOffset;
- deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
- deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
- } else {
- disarmMotorOutput = motorConfig()->mincommand;
- motorOutputLow = motorConfig()->minthrottle;
- motorOutputHigh = motorConfig()->maxthrottle - ((motorConfig()->maxthrottle - motorConfig()->minthrottle) * (1 - motorOutputLimit));
- }
- break;
- }
+ motorInitEndpoints(motorOutputLimit, &motorOutputLow, &motorOutputHigh, &disarmMotorOutput, &deadbandMotor3dHigh, &deadbandMotor3dLow);
rcCommandThrottleRange = PWM_RANGE_MAX - PWM_RANGE_MIN;
}
@@ -478,19 +438,7 @@ void mixerResetDisarmedMotors(void)
void writeMotors(void)
{
-#ifdef USE_PWM_OUTPUT
- if (pwmAreMotorsEnabled()) {
-#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
- if (!pwmStartMotorUpdate(motorCount)) {
- return;
- }
-#endif
- for (int i = 0; i < motorCount; i++) {
- pwmWriteMotor(i, motor[i]);
- }
- pwmCompleteMotorUpdate(motorCount);
- }
-#endif
+ motorWriteAll(motor);
}
static void writeAllMotors(int16_t mc)
@@ -508,14 +456,6 @@ void stopMotors(void)
delay(50); // give the timers and ESCs a chance to react.
}
-void stopPwmAllMotors(void)
-{
-#ifdef USE_PWM_OUTPUT
- pwmShutdownPulsesForAllMotors(motorCount);
- delayMicroseconds(1500);
-#endif
-}
-
static FAST_RAM_ZERO_INIT float throttle = 0;
static FAST_RAM_ZERO_INIT float loggingThrottle = 0;
static FAST_RAM_ZERO_INIT float motorOutputMin;
@@ -751,7 +691,7 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t
}
}
-float applyThrottleLimit(float throttle)
+static float applyThrottleLimit(float throttle)
{
if (currentControlRateProfile->throttle_limit_percent < 100) {
const float throttleLimitFactor = currentControlRateProfile->throttle_limit_percent / 100.0f;
@@ -766,7 +706,7 @@ float applyThrottleLimit(float throttle)
return throttle;
}
-void applyMotorStop(void)
+static void applyMotorStop(void)
{
for (int i = 0; i < motorCount; i++) {
motor[i] = disarmMotorOutput;
@@ -774,7 +714,7 @@ void applyMotorStop(void)
}
#ifdef USE_DYN_LPF
-void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
+static void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
{
static timeUs_t lastDynLpfUpdateUs = 0;
static int dynLpfPreviousQuantizedThrottle = -1; // to allow an initial zero throttle to set the filter cutoff
@@ -950,60 +890,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
}
}
-float convertExternalToMotor(uint16_t externalValue)
-{
- uint16_t motorValue;
-#ifdef USE_DSHOT
- if (isMotorProtocolDshot()) {
- externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX);
-
- if (featureIsEnabled(FEATURE_3D)) {
- if (externalValue == PWM_RANGE_MID) {
- motorValue = DSHOT_CMD_MOTOR_STOP;
- } else if (externalValue < PWM_RANGE_MID) {
- motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MID - 1, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, DSHOT_MIN_THROTTLE);
- } else {
- motorValue = scaleRange(externalValue, PWM_RANGE_MID + 1, PWM_RANGE_MAX, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
- }
- } else {
- motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_CMD_MOTOR_STOP : scaleRange(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
- }
- }
- else
-#endif
- {
- motorValue = externalValue;
- }
-
- return (float)motorValue;
-}
-
-uint16_t convertMotorToExternal(float motorValue)
-{
- uint16_t externalValue;
-#ifdef USE_DSHOT
- if (isMotorProtocolDshot()) {
- if (featureIsEnabled(FEATURE_3D)) {
- if (motorValue == DSHOT_CMD_MOTOR_STOP || motorValue < DSHOT_MIN_THROTTLE) {
- externalValue = PWM_RANGE_MID;
- } else if (motorValue <= DSHOT_3D_FORWARD_MIN_THROTTLE - 1) {
- externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, PWM_RANGE_MID - 1, PWM_RANGE_MIN);
- } else {
- externalValue = scaleRange(motorValue, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MID + 1, PWM_RANGE_MAX);
- }
- } else {
- externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);
- }
- }
- else
-#endif
- {
- externalValue = motorValue;
- }
-
- return externalValue;
-}
-
void mixerSetThrottleAngleCorrection(int correctionValue)
{
throttleAngleCorrection = correctionValue;
@@ -1013,15 +899,3 @@ float mixerGetLoggingThrottle(void)
{
return loggingThrottle;
}
-
-#ifdef USE_DSHOT_TELEMETRY
-bool isDshotTelemetryActive(void)
-{
- for (uint8_t i = 0; i < motorCount; i++) {
- if (!isDshotMotorTelemetryActive(i)) {
- return false;
- }
- }
- return true;
-}
-#endif
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 85f50f1908..6469cd4855 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -29,9 +29,6 @@
#define QUAD_MOTOR_COUNT 4
-// Digital protocol has fixed values
-#define DSHOT_3D_FORWARD_MIN_THROTTLE 1048
-
// Note: this is called MultiType/MULTITYPE_* in baseflight.
typedef enum mixerMode
{
@@ -108,15 +105,10 @@ void mixerConfigureOutput(void);
void mixerResetDisarmedMotors(void);
void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation);
-void syncMotors(bool enabled);
-void writeMotors(void);
void stopMotors(void);
-void stopPwmAllMotors(void);
+void writeMotors(void);
-float convertExternalToMotor(uint16_t externalValue);
-uint16_t convertMotorToExternal(float motorValue);
bool mixerIsTricopter(void);
void mixerSetThrottleAngleCorrection(int correctionValue);
float mixerGetLoggingThrottle(void);
-bool isDshotTelemetryActive(void);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index f93f1c25be..fd8b155e48 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -34,6 +34,7 @@
#include "config/config_reset.h"
+#include "drivers/dshot_command.h"
#include "drivers/pwm_output.h"
#include "drivers/sound_beeper.h"
#include "drivers/time.h"
@@ -225,7 +226,7 @@ static void pidSetTargetLooptime(uint32_t pidLooptime)
dT = targetPidLooptime * 1e-6f;
pidFrequency = 1.0f / dT;
#ifdef USE_DSHOT
- setDshotPidLoopTime(targetPidLooptime);
+ dshotSetPidLoopTime(targetPidLooptime);
#endif
}
diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c
index 9e7c047ee6..05ddf774bb 100644
--- a/src/main/flight/rpm_filter.c
+++ b/src/main/flight/rpm_filter.c
@@ -31,6 +31,8 @@
#include "common/filter.h"
#include "common/maths.h"
+#include "drivers/dshot.h"
+
#include "flight/mixer.h"
#include "flight/pid.h"
diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c
index 1e30e66d69..51bbfd0af9 100644
--- a/src/main/io/beeper.c
+++ b/src/main/io/beeper.c
@@ -27,6 +27,7 @@
#include "config/feature.h"
+#include "drivers/dshot_command.h"
#include "drivers/io.h"
#include "drivers/pwm_output.h"
#include "drivers/sound_beeper.h"
@@ -410,7 +411,7 @@ void beeperUpdate(timeUs_t currentTimeUs)
if ((currentTimeUs - getLastDisarmTimeUs() > DSHOT_BEACON_GUARD_DELAY_US) && !isTryingToArm()) {
lastDshotBeaconCommandTimeUs = currentTimeUs;
- pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, false);
+ dshotCommandWrite(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, false);
}
}
#endif
diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c
index e3d19927ad..eff736d050 100644
--- a/src/main/io/serial_4way.c
+++ b/src/main/io/serial_4way.c
@@ -137,7 +137,9 @@ inline void setEscOutput(uint8_t selEsc)
uint8_t esc4wayInit(void)
{
// StopPwmAllMotors();
- pwmDisableMotors();
+ // XXX Review effect of motor refactor
+ //pwmDisableMotors();
+ motorDisable();
escCount = 0;
memset(&escHardware, 0, sizeof(escHardware));
pwmOutputPort_t *pwmMotors = pwmGetMotors();
@@ -161,7 +163,7 @@ void esc4wayRelease(void)
IOConfigGPIO(escHardware[escCount].io, IOCFG_AF_PP);
setEscLo(escCount);
}
- pwmEnableMotors();
+ motorEnable();
}
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 788f82d3ce..16827238a1 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -51,6 +51,7 @@
#include "drivers/flash.h"
#include "drivers/io.h"
#include "drivers/max7456.h"
+#include "drivers/motor.h"
#include "drivers/pwm_output.h"
#include "drivers/sdcard.h"
#include "drivers/serial.h"
@@ -255,7 +256,7 @@ static void mspRebootFn(serialPort_t *serialPort)
{
UNUSED(serialPort);
- stopPwmAllMotors();
+ motorShutdown();
switch (rebootMode) {
case MSP_REBOOT_FIRMWARE:
@@ -963,7 +964,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
continue;
}
- sbufWriteU16(dst, convertMotorToExternal(motor[i]));
+ sbufWriteU16(dst, motorConvertToExternal(motor[i]));
#else
sbufWriteU16(dst, 0);
#endif
@@ -1950,7 +1951,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_MOTOR:
for (int i = 0; i < getMotorCount(); i++) {
- motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src));
+ motor_disarmed[i] = motorConvertFromExternal(sbufReadU16(src));
}
break;
diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c
index ebf271edf8..7cb4475887 100644
--- a/src/main/osd/osd_elements.c
+++ b/src/main/osd/osd_elements.c
@@ -66,7 +66,7 @@
#include "drivers/display.h"
#include "drivers/max7456_symbols.h"
-#include "drivers/pwm_output.h"
+#include "drivers/dshot.h"
#include "drivers/time.h"
#include "drivers/vtx_common.h"
diff --git a/src/main/pg/motor.c b/src/main/pg/motor.c
index f3d7ec089e..04cc7a190b 100644
--- a/src/main/pg/motor.c
+++ b/src/main/pg/motor.c
@@ -50,16 +50,18 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig)
motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
motorConfig->dev.useUnsyncedPwm = true;
} else
-#endif
+#endif // USE_BRUSHED_ESC_AUTODETECT
{
motorConfig->minthrottle = 1070;
motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
motorConfig->dev.motorPwmProtocol = PWM_TYPE_ONESHOT125;
}
-#endif
+#endif // BRUSHED_MOTORS
+
motorConfig->maxthrottle = 2000;
motorConfig->mincommand = 1000;
motorConfig->digitalIdleOffsetValue = 550;
+
#ifdef USE_DSHOT_DMAR
motorConfig->dev.useBurstDshot = ENABLE_DSHOT_DMAR;
#endif
diff --git a/src/main/pg/motor.h b/src/main/pg/motor.h
index da2eebe6b3..ea110bea96 100644
--- a/src/main/pg/motor.h
+++ b/src/main/pg/motor.h
@@ -32,6 +32,7 @@ typedef struct motorDevConfig_s {
uint8_t useBurstDshot;
uint8_t useDshotTelemetry;
ioTag_t ioTags[MAX_SUPPORTED_MOTORS];
+ uint8_t motorTransportProtocol;
} motorDevConfig_t;
typedef struct motorConfig_s {
diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c
index ced989e5c9..2fd83e5137 100644
--- a/src/main/sensors/esc_sensor.c
+++ b/src/main/sensors/esc_sensor.c
@@ -28,6 +28,8 @@
#include "build/debug.h"
+#include "common/time.h"
+
#include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
@@ -36,7 +38,10 @@
#include "common/maths.h"
#include "common/utils.h"
-#include "drivers/pwm_output.h"
+#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"
@@ -291,11 +296,13 @@ static void selectNextMotor(void)
}
}
+// XXX Review ESC sensor under refactored motor handling
+
void escSensorProcess(timeUs_t currentTimeUs)
{
const timeMs_t currentTimeMs = currentTimeUs / 1000;
- if (!escSensorPort || !pwmAreMotorsEnabled()) {
+ if (!escSensorPort || !motorIsEnabled()) {
return;
}
@@ -312,7 +319,7 @@ void escSensorProcess(timeUs_t currentTimeUs)
startEscDataRead(telemetryBuffer, TELEMETRY_FRAME_SIZE);
motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
- motor->requestTelemetry = true;
+ motor->protocolControl.requestTelemetry = true;
escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1);
diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c
index cd72d5e8c1..a2fffa08bf 100644
--- a/src/main/target/SITL/target.c
+++ b/src/main/target/SITL/target.c
@@ -31,6 +31,7 @@
#include "drivers/io.h"
#include "drivers/dma.h"
+#include "drivers/motor.h"
#include "drivers/serial.h"
#include "drivers/serial_tcp.h"
#include "drivers/system.h"
@@ -380,8 +381,7 @@ int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y)
// PWM part
-static bool pwmMotorsEnabled = false;
-static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
+pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
// real value to send
@@ -389,18 +389,6 @@ static int16_t motorsPwm[MAX_SUPPORTED_MOTORS];
static int16_t servosPwm[MAX_SUPPORTED_SERVOS];
static int16_t idlePulse;
-void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount) {
- UNUSED(motorConfig);
- UNUSED(motorCount);
-
- idlePulse = _idlePulse;
-
- for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
- motors[motorIndex].enabled = true;
- }
- pwmMotorsEnabled = true;
-}
-
void servoDevInit(const servoDevConfig_t *servoConfig) {
UNUSED(servoConfig);
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
@@ -408,33 +396,51 @@ void servoDevInit(const servoDevConfig_t *servoConfig) {
}
}
+static motorDevice_t motorPwmDevice; // Forward
+
pwmOutputPort_t *pwmGetMotors(void) {
return motors;
}
-void pwmDisableMotors(void) {
- pwmMotorsEnabled = false;
+static float pwmConvertFromExternal(uint16_t externalValue)
+{
+ return (float)externalValue;
}
-void pwmEnableMotors(void) {
- pwmMotorsEnabled = true;
+static uint16_t pwmConvertToExternal(float motorValue)
+{
+ return (uint16_t)motorValue;
}
-bool pwmAreMotorsEnabled(void) {
- return pwmMotorsEnabled;
+static void pwmDisableMotors(void)
+{
+ motorPwmDevice.enabled = false;
}
-void pwmWriteMotor(uint8_t index, float value) {
+static bool pwmEnableMotors(void)
+{
+ motorPwmDevice.enabled = true;
+
+ return true;
+}
+
+static void pwmWriteMotor(uint8_t index, float value)
+{
motorsPwm[index] = value - idlePulse;
}
-void pwmShutdownPulsesForAllMotors(uint8_t motorCount) {
- UNUSED(motorCount);
- pwmMotorsEnabled = false;
+static void pwmWriteMotorInt(uint8_t index, uint16_t value)
+{
+ pwmWriteMotor(index, (float)value);
}
-void pwmCompleteMotorUpdate(uint8_t motorCount) {
- UNUSED(motorCount);
+static void pwmShutdownPulsesForAllMotors(void)
+{
+ motorPwmDevice.enabled = false;
+}
+
+static void pwmCompleteMotorUpdate(void)
+{
// send to simulator
// for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0]
@@ -458,6 +464,41 @@ void pwmWriteServo(uint8_t index, float value) {
servosPwm[index] = value;
}
+static motorDevice_t motorPwmDevice = {
+ .vTable = {
+ .convertExternalToMotor = pwmConvertFromExternal,
+ .convertMotorToExternal = pwmConvertToExternal,
+ .enable = pwmEnableMotors,
+ .disable = pwmDisableMotors,
+ .updateStart = motorUpdateStartNull,
+ .write = pwmWriteMotor,
+ .writeInt = pwmWriteMotorInt,
+ .updateComplete = pwmCompleteMotorUpdate,
+ .shutdown = pwmShutdownPulsesForAllMotors,
+ }
+};
+
+motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
+{
+ UNUSED(motorConfig);
+ UNUSED(useUnsyncedPwm);
+
+ if (motorCount > 4) {
+ return NULL;
+ }
+
+ idlePulse = _idlePulse;
+
+ 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;
+}
+
// ADC part
uint16_t adcGetChannel(uint8_t channel) {
UNUSED(channel);
diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c
index 76e977d842..fb9210ec42 100644
--- a/src/main/telemetry/srxl.c
+++ b/src/main/telemetry/srxl.c
@@ -64,7 +64,7 @@
#include "telemetry/srxl.h"
#include "drivers/vtx_common.h"
-#include "drivers/pwm_output.h"
+#include "drivers/dshot.h"
#include "io/vtx_tramp.h"
#include "io/vtx_smartaudio.h"
@@ -180,7 +180,7 @@ uint16_t getMotorAveragePeriod(void)
}
#endif
-#if defined( USE_DSHOT_TELEMETRY)
+#if defined(USE_DSHOT_TELEMETRY)
if (useDshotTelemetry) {
uint16_t motors = getMotorCount();
diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc
index 2cf03c8be7..d0d47ca0e6 100644
--- a/src/test/unit/cli_unittest.cc
+++ b/src/test/unit/cli_unittest.cc
@@ -274,7 +274,8 @@ const char rcChannelLetters[] = "AERT12345678abcdefgh";
void parseRcChannels(const char *, rxConfig_t *){}
void mixerLoadMix(int, motorMixer_t *) {}
bool setModeColor(ledModeIndex_e, int, int) { return false; }
-float convertExternalToMotor(uint16_t ){ return 1.0; }
+float motorConvertFromExternal(uint16_t) { return 1.0; }
+void motorShutdown(void) { }
uint8_t getCurrentPidProfileIndex(void){ return 1; }
uint8_t getCurrentControlRateProfileIndex(void){ return 1; }
void changeControlRateProfile(uint8_t) {}
@@ -328,7 +329,6 @@ void schedulerSetCalulateTaskStatistics(bool) {}
void setArmingDisabled(armingDisableFlags_e) {}
void waitForSerialPortToFinishTransmitting(serialPort_t *) {}
-void stopPwmAllMotors(void) {}
void systemResetToBootloader(void) {}
void resetConfigs(void) {}
void systemReset(void) {}
diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc
index 1e00c050dd..b2bedaa67c 100644
--- a/src/test/unit/cms_unittest.cc
+++ b/src/test/unit/cms_unittest.cc
@@ -142,7 +142,7 @@ uint32_t micros(void) { return 0; }
uint32_t millis(void) { return 0; }
void saveConfigAndNotify(void) {}
void stopMotors(void) {}
-void stopPwmAllMotors(void) {}
+void motorShutdown(void) {}
void systemReset(void) {}
void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }