Add dshot_reversed_motors: per-motor spin direction via DShot commands 20/21

docs/ESC and servo outputs.md

@@ -15,6 +15,18 @@ ESC protocol can be selected in Configurator. No special configuration is requir
 
 Check the ESC documentation for the list of protocols that are supported.
 
+## Motor direction with DShot
+
+A motor that turns the wrong way can be reversed from INAV instead of from the ESC configurator, as long as the ESC runs DShot and understands the spin direction commands (DShot commands 20 and 21 - the same ones turtle mode relies on; BLHeli_32, AM32 and Bluejay do).
+
+`dshot_reversed_motors` is a bitmask: bit 0 is motor 1, bit 1 is motor 2, and so on. A set bit tells that ESC to spin opposite to the direction stored in the ESC. `set dshot_reversed_motors = 5` reverses motors 1 and 3.
+
+The ESC does not store the command, so INAV sends it again on every arm, as soon as the setting changes and every two seconds while disarmed. Plugging in the battery after configuring over USB is covered by that; an ESC that resets in flight falls back to its stored direction, but it will not re-arm while the motor is being driven anyway.
+
+Turtle mode inverts every motor relative to its configured direction and restores the configured directions on disarm.
+
+The setting only works with DShot. With any other protocol it is ignored, and the direction has to be changed in the ESC or by swapping two motor wires.
+
 ## Servo outputs
 
 By default, INAV uses 50Hz servo update rate. If you want to increase it, make sure that servos support

docs/Settings.md

@@ -941,6 +941,16 @@ Sets the DShot beeper tone
 
 ---
 
+### dshot_reversed_motors
+
+Bitmask of motors that are told to spin opposite to the direction stored in their ESC, using the DShot spin direction command: bit 0 is motor 1, bit 1 motor 2, and so on. Fixes a motor that turns the wrong way without touching the ESC configuration. The ESC does not store it, so it is sent again on every arm, whenever this setting changes and every two seconds while disarmed. Needs an ESC that understands DShot commands 20 and 21, the same ones turtle mode uses.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 4095 |
+
+---
+
 ### dterm_lpf2_hz
 
 Dterm pre-differentiation LPF cutoff (Hz). Filters gyro before differentiation to reduce noise amplification. Higher = less delay, more noise. 0 = disabled. Values around 200-250Hz can add smoothing with small delay.

src/main/drivers/pwm_output.c

@@ -64,7 +64,20 @@
 #define DSHOT_COMMAND_DELAY_US 1000
 #define DSHOT_COMMAND_INTERVAL_US 10000
 #define DSHOT_COMMAND_QUEUE_LENGTH 8
-#define DHSOT_COMMAND_QUEUE_SIZE   DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e)
+#define DHSOT_COMMAND_QUEUE_SIZE   DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommandFrame_t)
+
+// One queued command frame: the command each motor receives, sent `repeats` times.
+// Motors carry their own command so one frame can tell some ESCs to spin normal and
+// others reversed; a frame for "every motor" simply repeats the same command.
+typedef struct {
+    uint8_t cmd[MAX_MOTORS];
+    uint8_t repeats;
+} dshotCommandFrame_t;
+
+typedef struct {
+    dshotCommandFrame_t frame;
+    int remainingRepeats;
+} currentExecutingCommand_t;
 #endif
 
 typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function pointer used to write motors
@@ -455,16 +468,6 @@ void pwmRequestMotorTelemetry(int motorIndex)
 }
 
 #ifdef USE_DSHOT
-void sendDShotCommand(dshotCommands_e cmd) {
-    circularBufferPushElement(&commandsCircularBuffer, (uint8_t *) &cmd);
-}
-
-void initDShotCommands(void) {
-    circularBufferInit(&commandsCircularBuffer, commandsBuff,DHSOT_COMMAND_QUEUE_SIZE, sizeof(dshotCommands_e));
-
-    currentExecutingCommand.remainingRepeats = 0;
-}
-
 static int getDShotCommandRepeats(dshotCommands_e cmd) {
     int repeats = 1;
 
@@ -480,18 +483,40 @@ static int getDShotCommandRepeats(dshotCommands_e cmd) {
     return repeats;
 }
 
-static bool executeDShotCommands(void){
-
+void sendDShotCommand(dshotCommands_e cmd) {
+    dshotCommandFrame_t frame;
+    memset(frame.cmd, cmd, sizeof(frame.cmd));
+    frame.repeats = getDShotCommandRepeats(cmd);
+    circularBufferPushElement(&commandsCircularBuffer, (uint8_t *) &frame);
+}
+
+void sendDShotSpinDirection(uint16_t reversedMotorMask) {
+    dshotCommandFrame_t frame;
+    for (int i = 0; i < MAX_MOTORS; i++) {
+        frame.cmd[i] = (reversedMotorMask & (1 << i)) ? DSHOT_CMD_SPIN_DIRECTION_REVERSED : DSHOT_CMD_SPIN_DIRECTION_NORMAL;
+    }
+    frame.repeats = getDShotCommandRepeats(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
+    circularBufferPushElement(&commandsCircularBuffer, (uint8_t *) &frame);
+}
+
+void initDShotCommands(void) {
+    circularBufferInit(&commandsCircularBuffer, commandsBuff,DHSOT_COMMAND_QUEUE_SIZE, sizeof(dshotCommandFrame_t));
+
+    currentExecutingCommand.remainingRepeats = 0;
+}
+
+// LTO must not pull the command sequencing into the ITCM-resident scheduler; the F745
+// targets have 16 KB of it and the per-motor frames pushed it over the edge
+static bool NOINLINE executeDShotCommands(void){
+
     timeUs_t tNow = micros();
 
     if(currentExecutingCommand.remainingRepeats == 0) {
        const int isTherePendingCommands = !circularBufferIsEmpty(&commandsCircularBuffer);
         if (isTherePendingCommands && (tNow - lastCommandSent > DSHOT_COMMAND_INTERVAL_US)){
             //Load the command
-            dshotCommands_e cmd;
-            circularBufferPopHead(&commandsCircularBuffer, (uint8_t *) &cmd);
-            currentExecutingCommand.cmd = cmd;
-            currentExecutingCommand.remainingRepeats = getDShotCommandRepeats(cmd);
+            circularBufferPopHead(&commandsCircularBuffer, (uint8_t *) &currentExecutingCommand.frame);
+            currentExecutingCommand.remainingRepeats = currentExecutingCommand.frame.repeats;
             commandPostDelay = DSHOT_COMMAND_INTERVAL_US;
         } else {
             if (commandPostDelay) {
@@ -506,7 +531,7 @@ static bool executeDShotCommands(void){
     }
     for (uint8_t i = 0; i < getMotorCount(); i++) {
          motors[i].requestTelemetry = true;
-         motors[i].value = currentExecutingCommand.cmd;
+         motors[i].value = currentExecutingCommand.frame.cmd[i];
     }
     if (tNow - lastCommandSent >= DSHOT_COMMAND_DELAY_US) {
         currentExecutingCommand.remainingRepeats--; 

src/main/drivers/pwm_output.h

@@ -31,11 +31,6 @@ typedef enum {
     DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
 } dshotCommands_e;
 
-typedef struct {
-    dshotCommands_e cmd;
-    int remainingRepeats;
-} currentExecutingCommand_t;
-
 void pwmRequestMotorTelemetry(int motorIndex);
 
 ioTag_t pwmGetMotorPinTag(int motorIndex);
@@ -61,7 +56,12 @@ bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn
 void pwmWriteBeeper(bool onoffBeep);
 bool beeperPwmInit(ioTag_t tag, uint16_t frequency);
 
+// Queue one command for every motor
 void sendDShotCommand(dshotCommands_e cmd);
+// Queue one frame in which each motor in the mask gets SPIN_DIRECTION_REVERSED and every
+// other motor SPIN_DIRECTION_NORMAL. The ESC does not store either, so the caller re-sends
+// them whenever the ESC may have restarted
+void sendDShotSpinDirection(uint16_t reversedMotorMask);
 void initDShotCommands(void);
 
 uint32_t getEscUpdateFrequency(void);

src/main/drivers/pwm_output_rp2350.c

@@ -52,6 +52,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 
 #include "build/build_config.h"
 
@@ -192,9 +193,9 @@ static bool    servoInitialized = false;
  */
 static timMotorServoHardware_t rp2350OutputAssignment;
 
-/* Pending DShot command (e.g. spin direction).  Commands are sent 10×. */
-static dshotCommands_e pendingCmd     = 0;
-static int             pendingCmdReps = 0;
+/* Pending DShot command per motor (e.g. spin direction).  Commands are sent 10×. */
+static uint8_t pendingCmd[DSHOT_MAX_MOTORS];
+static int     pendingCmdReps = 0;
 
 /* ── DShot packet construction ───────────────────────────────────────────── */
 
@@ -261,7 +262,7 @@ void pwmCompleteMotorUpdate(void)
         bool telemetry = dshotMotors[i].requestTelemetry;
 
         if (pendingCmdReps > 0) {
-            value    = (uint16_t)pendingCmd;
+            value    = pendingCmd[i];
             telemetry = true;
         }
 
@@ -334,16 +335,24 @@ bool isMotorProtocolDigital(void)
 
 void initDShotCommands(void)
 {
-    pendingCmd     = 0;
+    memset(pendingCmd, 0, sizeof(pendingCmd));
     pendingCmdReps = 0;
 }
 
 void sendDShotCommand(dshotCommands_e cmd)
 {
-    pendingCmd     = cmd;
+    memset(pendingCmd, cmd, sizeof(pendingCmd));
     pendingCmdReps = 10;  /* DShot spec: send each command 10 times */
 }
 
+void sendDShotSpinDirection(uint16_t reversedMotorMask)
+{
+    for (uint i = 0; i < DSHOT_MAX_MOTORS; i++) {
+        pendingCmd[i] = (reversedMotorMask & (1u << i)) ? DSHOT_CMD_SPIN_DIRECTION_REVERSED : DSHOT_CMD_SPIN_DIRECTION_NORMAL;
+    }
+    pendingCmdReps = 10;
+}
+
 /* ── Telemetry / pin tag ─────────────────────────────────────────────────── */
 
 void pwmRequestMotorTelemetry(int motorIndex)

src/main/fc/fc_core.c

@@ -483,7 +483,8 @@ void disarm(disarmReason_t disarmReason)
         DISABLE_STATE(IN_FLIGHT_EMERG_REARM);
 #ifdef USE_DSHOT
         if (FLIGHT_MODE(TURTLE_MODE)) {
-            sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
+            // Back from all-inverted to the configured directions, not to all-normal
+            dshotSpinDirectionApply(false);
             DISABLE_FLIGHT_MODE(TURTLE_MODE);
         }
 #endif
@@ -583,7 +584,8 @@ void tryArm(void)
     const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE);
 #endif
     if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) {
-        sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
+        // Every motor the other way round relative to its configured direction
+        dshotSpinDirectionApply(true);
         ENABLE_ARMING_FLAG(ARMED);
         ENABLE_FLIGHT_MODE(TURTLE_MODE);
         return;
@@ -608,6 +610,14 @@ void tryArm(void)
 
         lastDisarmReason = DISARM_NONE;
 
+#ifdef USE_DSHOT
+        // Some ESCs keep a reversed direction through an FC reboot, so every arm sends it (like Betaflight).
+        // Not on an in-flight rearm: the motors are turning and would ignore it
+        if (!STATE(IN_FLIGHT_EMERG_REARM) && (motorConfig()->dshotReversedMotors || !feature(FEATURE_REVERSIBLE_MOTORS))) {
+            dshotSpinDirectionApply(false);
+        }
+#endif
+
         ENABLE_ARMING_FLAG(ARMED);
         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
         //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
@@ -1080,6 +1090,7 @@ void taskRunRealtimeCallbacks(timeUs_t currentTimeUs)
 #endif
 
 #ifdef USE_DSHOT
+    dshotSpinDirectionUpdate(currentTimeUs);
     pwmCompleteMotorUpdate();
 #endif
 

src/main/fc/settings.yaml

@@ -868,6 +868,13 @@ groups:
         min: 4
         max: 255
         default_value: 14
+      - name: dshot_reversed_motors
+        description: "Bitmask of motors that are told to spin opposite to the direction stored in their ESC, using the DShot spin direction command: bit 0 is motor 1, bit 1 motor 2, and so on. Fixes a motor that turns the wrong way without touching the ESC configuration. The ESC does not store it, so it is sent again on every arm, whenever this setting changes and every two seconds while disarmed. Needs an ESC that understands DShot commands 20 and 21, the same ones turtle mode uses."
+        default_value: 0
+        field: dshotReversedMotors
+        condition: USE_DSHOT
+        min: 0
+        max: 4095
 
   - name: PG_FAILSAFE_CONFIG
     type: failsafeConfig_t

src/main/flight/mixer.c

@@ -53,6 +53,8 @@
 #include "flight/pid.h"
 #include "flight/servos.h"
 
+#include "io/beeper.h"
+
 #include "navigation/navigation.h"
 
 #include "rx/rx.h"
@@ -89,13 +91,16 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
     .neutral = SETTING_3D_NEUTRAL_DEFAULT
 );
 
-PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 11);
+PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 12);
 
 PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT,
     .motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT,
     .mincommand = SETTING_MIN_COMMAND_DEFAULT,
     .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT,            // Most brushless motors that we use are 14 poles
+#ifdef USE_DSHOT
+    .dshotReversedMotors = SETTING_DSHOT_REVERSED_MOTORS_DEFAULT,
+#endif
 );
 PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_TIMER_OVERRIDE_CONFIG, 0);
 
@@ -1003,6 +1008,74 @@ bool areMotorsStopped(void)
     return motor[0] == motorZeroCommand;
 }
 
+#ifdef USE_DSHOT
+/*
+ * DShot commands 20/21 are not stored by the ESC: it forgets them when it restarts, and
+ * without telemetry the FC cannot see an ESC restart - the battery plugged in after USB
+ * is the everyday case. So the configured directions go out again on every arm, as soon
+ * as the setting changes, and every DSHOT_SPIN_DIRECTION_REFRESH_US while disarmed. On
+ * arm the command frames replace the first throttle frames, so an ESC that was listening
+ * has its direction before it gets throttle.
+ */
+#define DSHOT_SPIN_DIRECTION_POLL_US    100000
+#define DSHOT_SPIN_DIRECTION_REFRESH_US 2000000
+
+static uint16_t dshotSpinDirectionSent = 0;         // reversed mask the ESCs last received
+static timeUs_t dshotSpinDirectionSentAtUs = 0;
+static timeUs_t dshotSpinDirectionPolledAtUs = 0;
+
+static uint16_t dshotReversedMotorMask(void)
+{
+    return motorConfig()->dshotReversedMotors & ((1u << motorCount) - 1);
+}
+
+void dshotSpinDirectionApply(bool invert)
+{
+    if (!isMotorProtocolDshot()) {
+        return;
+    }
+
+    const uint16_t allMotors = (1u << motorCount) - 1;
+    const uint16_t mask = invert ? (~dshotReversedMotorMask() & allMotors) : dshotReversedMotorMask();
+
+    sendDShotSpinDirection(mask);
+    dshotSpinDirectionSent = mask;
+    dshotSpinDirectionSentAtUs = micros();
+}
+
+void NOINLINE dshotSpinDirectionUpdate(timeUs_t currentTimeUs)
+{
+    // Called from a busy loop; one evaluation per poll interval is plenty
+    if (currentTimeUs - dshotSpinDirectionPolledAtUs < DSHOT_SPIN_DIRECTION_POLL_US) {
+        return;
+    }
+    dshotSpinDirectionPolledAtUs = currentTimeUs;
+
+    if (!isMotorProtocolDshot()) {
+        return;
+    }
+
+    const uint16_t mask = dshotReversedMotorMask();
+
+    // All normal and the last frame (if any) was all normal: quiet while disarmed, tryArm() still sends it
+    if (mask == 0 && dshotSpinDirectionSent == 0) {
+        return;
+    }
+
+    if (mask == dshotSpinDirectionSent && currentTimeUs - dshotSpinDirectionSentAtUs < DSHOT_SPIN_DIRECTION_REFRESH_US) {
+        return;
+    }
+
+    // Only a stopped motor listens to commands (the motor test counts as running), and an
+    // ESC that is still playing a beacon tone ignores them too
+    if (areMotorsRunning() || currentTimeUs - getLastDshotBeeperCommandTimeUs() < getDShotBeaconGuardDelayUs()) {
+        return;
+    }
+
+    dshotSpinDirectionApply(false);
+}
+#endif
+
 uint16_t getMaxThrottle(void) {
 
     static uint16_t throttle = 0;