Fix DShot output for reversible motors when disarmed

src/main/CMakeLists.txt

@@ -333,6 +333,8 @@ main_sources(COMMON_SRC
     flight/rate_dynamics.h
     flight/mixer.c
     flight/mixer.h
+    flight/mixer_disarmed_dshot.c
+    flight/mixer_disarmed_dshot.h
     flight/pid.c
     flight/pid.h
     flight/pid_autotune.c

src/main/flight/mixer.c

@@ -48,6 +48,7 @@
 #include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
+#include "flight/mixer_disarmed_dshot.h"
 #include "flight/pid.h"
 #include "flight/servos.h"
 
@@ -375,7 +376,16 @@ void FAST_CODE writeMotors(void)
         if (isMotorProtocolDigital()) {
             // If we use DSHOT we need to convert motorValue to DSHOT ranges
             if (feature(FEATURE_REVERSIBLE_MOTORS)) {
-                if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
+                if (!ARMING_FLAG(ARMED)) {
+                    // mixTable() never runs while disarmed, so reversibleMotorsThrottleState is stale.
+                    motorValue = calculateDisarmedReversibleMotorsDshotValue(
+                        motor[i],
+                        reversibleMotorsConfig()->deadband_low,
+                        reversibleMotorsConfig()->deadband_high,
+                        motorConfig()->mincommand,
+                        getMaxThrottle()
+                    );
+                } else if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
                     motorValue = handleOutputScaling(
                         motor[i],
                         throttleRangeMin,

src/main/flight/mixer.h

@@ -21,19 +21,14 @@
 
 #include "drivers/timer.h"
 
+#include "flight/mixer_dshot_constants.h"
+
 #if defined(TARGET_MOTOR_COUNT)
 #define MAX_SUPPORTED_MOTORS TARGET_MOTOR_COUNT
 #else
 #define MAX_SUPPORTED_MOTORS 12
 #endif
 
-// Digital protocol has fixed values
-#define DSHOT_DISARM_COMMAND      0
-#define DSHOT_MIN_THROTTLE       48
-#define DSHOT_MAX_THROTTLE     2047
-#define DSHOT_3D_DEADBAND_LOW  1047
-#define DSHOT_3D_DEADBAND_HIGH 1048
-
 typedef enum {
     PLATFORM_MULTIROTOR     = 0,
     PLATFORM_AIRPLANE       = 1,

src/main/flight/mixer_disarmed_dshot.c

@@ -0,0 +1,47 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it 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.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include "flight/mixer_disarmed_dshot.h"
+
+#include "common/maths.h"
+#include "flight/mixer_dshot_constants.h"
+
+uint16_t calculateDisarmedReversibleMotorsDshotValue(
+    int16_t motorValue,
+    int16_t deadbandLow,
+    int16_t deadbandHigh,
+    int16_t mincommand,
+    int16_t maxThrottle)
+{
+    if (motorValue >= deadbandHigh) {
+        if (maxThrottle <= deadbandHigh) {
+            return DSHOT_MAX_THROTTLE; // zero-width range would divide by zero
+        }
+        int32_t scaled = (int32_t)scaleRangef(motorValue, deadbandHigh, maxThrottle, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
+        return constrain(scaled, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
+    }
+    if (motorValue <= deadbandLow) {
+        if (deadbandLow <= mincommand) {
+            return DSHOT_MIN_THROTTLE; // zero-width range would divide by zero
+        }
+        int32_t scaled = (int32_t)scaleRangef(motorValue, mincommand, deadbandLow, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW);
+        return constrain(scaled, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW);
+    }
+    return DSHOT_DISARM_COMMAND;
+}

src/main/flight/mixer_disarmed_dshot.h

@@ -0,0 +1,27 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it 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.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+uint16_t calculateDisarmedReversibleMotorsDshotValue(
+    int16_t motorValue,
+    int16_t deadbandLow,
+    int16_t deadbandHigh,
+    int16_t mincommand,
+    int16_t maxThrottle);

src/main/flight/mixer_dshot_constants.h

@@ -0,0 +1,25 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it 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.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+// Digital protocol has fixed values
+#define DSHOT_DISARM_COMMAND      0
+#define DSHOT_MIN_THROTTLE       48
+#define DSHOT_MAX_THROTTLE     2047
+#define DSHOT_3D_DEADBAND_LOW  1047
+#define DSHOT_3D_DEADBAND_HIGH 1048

src/test/unit/CMakeLists.txt

@@ -16,6 +16,9 @@ set_property(SOURCE flight_imu_unittest.cc PROPERTY depends     "build/debug.c"
 
 set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c")
 
+set_property(SOURCE mixer_unittest.cc PROPERTY depends
+    "common/maths.c" "flight/mixer_disarmed_dshot.c")
+
 set_property(SOURCE olc_unittest.cc PROPERTY depends "common/olc.c")
 
 set_property(SOURCE rcdevice_unittest.cc PROPERTY definitions USE_RCDEVICE)