Fixed motor value translation to / from MSP for DShot.

Fixed 'stopMotors' (used in 'HardfaultHandler' to actually stop motors with DShot. Also made 'disarmMotorOutput' used consistently.

Renamed functions to be more clear.

src/main/blackbox/blackbox.c

@@ -36,6 +36,7 @@
 #include "drivers/sensor.h"
 #include "drivers/compass.h"
 #include "drivers/system.h"
+#include "drivers/pwm_output.h"
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
@@ -895,7 +896,17 @@ void stopInTestMode(void)
  */
 bool inMotorTestMode(void) {
     static uint32_t resetTime = 0;
-    uint16_t inactiveMotorCommand = (feature(FEATURE_3D) ? masterConfig.flight3DConfig.neutral3d : masterConfig.motorConfig.mincommand);
+    uint16_t inactiveMotorCommand;
+    if (feature(FEATURE_3D)) {
+       inactiveMotorCommand = masterConfig.flight3DConfig.neutral3d;
+#ifdef USE_DSHOT
+    } else if (isMotorProtocolDshot()) {
+       inactiveMotorCommand = DSHOT_DISARM_COMMAND;
+#endif
+    } else {
+       inactiveMotorCommand = masterConfig.motorConfig.mincommand;
+    }
+
     int i;
     bool atLeastOneMotorActivated = false;
 

src/main/fc/fc_msp.c

@@ -648,10 +648,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
                 sbufWriteU16(dst, 0);
                 continue;
             }
-            if (isMotorProtocolDshot())
-                sbufWriteU16(dst, constrain((motor[i] / 2) + 1000, 1000, 2000)); // This is to get it working in the configurator
-            else
-                sbufWriteU16(dst, motor[i]);
+
+            sbufWriteU16(dst, convertMotorToExternal(motor[i]));
         }
         break;
     case MSP_RC:
@@ -1320,8 +1318,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
         break;
     case MSP_SET_MOTOR:
-        for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
-            motor_disarmed[i] = sbufReadU16(src);
+        for (i = 0; i < 8; i++) { // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
+            motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src));
+        }
         break;
     case MSP_SET_SERVO_CONFIGURATION:
 #ifdef USE_SERVOS

src/main/flight/mixer.c

@@ -47,6 +47,13 @@
 #include "fc/runtime_config.h"
 
 #include "config/feature.h"
+#include "config/config_master.h"
+
+#define EXTERNAL_DSHOT_CONVERSION_FACTOR 2
+// (minimum output value(1001) - (minimum input value(48) / conversion factor(2))
+#define EXTERNAL_DSHOT_CONVERSION_OFFSET 977
+#define EXTERNAL_CONVERSION_MIN_VALUE 1000
+#define EXTERNAL_CONVERSION_MAX_VALUE 2000
 
 uint8_t motorCount;
 
@@ -234,21 +241,26 @@ static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, d
 static float rcCommandThrottleRange;
 
 bool isMotorProtocolDshot(void) {
+#ifdef USE_DSHOT
     if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT300 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600)
         return true;
     else
+#endif
         return false;
 }
 
 // Add here scaled ESC outputs for digital protol
 void initEscEndpoints(void) {
+#ifdef USE_DSHOT
     if (isMotorProtocolDshot()) {
         disarmMotorOutput = DSHOT_DISARM_COMMAND;
         minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset;
         maxMotorOutputNormal = DSHOT_MAX_THROTTLE;
         deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH;
         deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
-    } else {
+    } else
+#endif
+    {
         disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand;
         minMotorOutputNormal = motorConfig->minthrottle;
         maxMotorOutputNormal = motorConfig->maxthrottle;
@@ -359,7 +371,7 @@ void mixerResetDisarmedMotors(void)
     int i;
     // set disarmed motor values
     for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
-        motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand;
+        motor_disarmed[i] = disarmMotorOutput;
 }
 
 void writeMotors(void)
@@ -373,7 +385,7 @@ void writeMotors(void)
     }
 }
 
-void writeAllMotors(int16_t mc)
+static void writeAllMotors(int16_t mc)
 {
     // Sends commands to all motors
     for (uint8_t i = 0; i < motorCount; i++) {
@@ -385,7 +397,7 @@ void writeAllMotors(int16_t mc)
 
 void stopMotors(void)
 {
-    writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand);
+    writeAllMotors(disarmMotorOutput);
 
     delay(50); // give the timers and ESCs a chance to react.
 }
@@ -511,14 +523,31 @@ void mixTable(pidProfile_t *pidProfile)
     // Disarmed mode
     if (!ARMING_FLAG(ARMED)) {
         for (i = 0; i < motorCount; i++) {
-            if (isMotorProtocolDshot()) {
-                motor[i] = (motor_disarmed[i] < motorOutputMin) ? disarmMotorOutput : motor_disarmed[i]; // Prevent getting into special reserved range
-
-                if (motor_disarmed[i] != disarmMotorOutput)
-                    motor[i] = (motor_disarmed[i] - 1000) * 2; // TODO - Perhaps needs rescaling as it will never reach 2047 during motor tests
-            } else {
             motor[i] = motor_disarmed[i];
         }
     }
-    }
+}
+
+uint16_t convertExternalToMotor(uint16_t externalValue)
+{
+    uint16_t motorValue = externalValue;
+#ifdef USE_DSHOT
+    if (isMotorProtocolDshot()) {
+        motorValue = externalValue <= EXTERNAL_CONVERSION_MIN_VALUE ? DSHOT_DISARM_COMMAND : constrain((externalValue - EXTERNAL_DSHOT_CONVERSION_OFFSET) * EXTERNAL_DSHOT_CONVERSION_FACTOR, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
+    }
+#endif
+
+    return motorValue;
+}
+
+uint16_t convertMotorToExternal(uint16_t motorValue)
+{
+    uint16_t externalValue = motorValue;
+#ifdef USE_DSHOT
+    if (isMotorProtocolDshot()) {
+        externalValue = motorValue < DSHOT_MIN_THROTTLE ? EXTERNAL_CONVERSION_MIN_VALUE : constrain((motorValue / EXTERNAL_DSHOT_CONVERSION_FACTOR) + EXTERNAL_DSHOT_CONVERSION_OFFSET, EXTERNAL_CONVERSION_MIN_VALUE + 1, EXTERNAL_CONVERSION_MAX_VALUE);
+    }
+#endif
+
+    return externalValue;
 }

src/main/flight/mixer.h

@@ -104,7 +104,6 @@ void mixerUseConfigs(
         airplaneConfig_t *airplaneConfigToUse,
         struct rxConfig_s *rxConfigToUse);
 
-void writeAllMotors(int16_t mc);
 void mixerLoadMix(int index, motorMixer_t *customMixers);
 void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
 
@@ -117,4 +116,7 @@ void syncMotors(bool enabled);
 void writeMotors(void);
 void stopMotors(void);
 void stopPwmAllMotors(void);
+
 bool isMotorProtocolDshot(void);
+uint16_t convertExternalToMotor(uint16_t externalValue);
+uint16_t convertMotorToExternal(uint16_t motorValue);

src/main/io/serial_cli.c

@@ -3095,11 +3095,11 @@ static void cliMotor(char *cmdline)
             cliShowArgumentRangeError("value", 1000, 2000);
             return;
         } else {
-            motor_disarmed[motor_index] = motor_value;
+            motor_disarmed[motor_index] = convertExternalToMotor(motor_value);
         }
     }
 
-    cliPrintf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]);
+    cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index]));
 }
 
 static void cliPlaySound(char *cmdline)

src/main/target/COLIBRI_RACE/i2c_bst.c

@@ -1141,7 +1141,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
             break;
         case BST_SET_MOTOR:
             for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8
-                motor_disarmed[i] = bstRead16();
+                motor_disarmed[i] = convertExternalToMotor(bstRead16());
             break;
         case BST_SET_SERVO_CONFIGURATION:
 #ifdef USE_SERVOS