Added support for DShot ESC settings to CLI.

src/main/drivers/pwm_output.c

@@ -21,6 +21,7 @@
 #include <math.h>
 
 #include "platform.h"
+#include "system.h"
 
 #include "io.h"
 #include "pwm_output.h"
@@ -29,6 +30,8 @@
 #define MULTISHOT_5US_PW    (MULTISHOT_TIMER_MHZ * 5)
 #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
 
+#define DSHOT_MAX_COMMAND 47
+
 static pwmWriteFuncPtr pwmWritePtr;
 static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
 static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
@@ -315,6 +318,27 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
             return MOTOR_DSHOT150_MHZ * 1000000;
     }
 }
+
+void pwmWriteDshotCommand(uint8_t index, uint8_t command)
+{
+    if (command <= DSHOT_MAX_COMMAND) {
+        motorDmaOutput_t *const motor = getMotorDmaOutput(index);
+
+        unsigned repeats;
+        if ((command >= 7 && command <= 10) || command == 12) {
+            repeats = 10;
+        } else {
+            repeats = 1;
+        }
+        for (; repeats; repeats--) {
+            motor->requestTelemetry = true;
+            pwmWritePtr(index, command);
+            pwmCompleteMotorUpdate(0);
+
+            delay(1);
+        }
+    }
+}
 #endif
 
 #ifdef USE_SERVOS

src/main/drivers/pwm_output.h

@@ -141,6 +141,7 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn
 
 #ifdef USE_DSHOT
 uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
+void pwmWriteDshotCommand(uint8_t index, uint8_t command);
 void pwmWriteDigital(uint8_t index, uint16_t value);
 void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
 void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);

src/main/drivers/serial_escserial.h

@@ -30,9 +30,12 @@ typedef enum {
     PROTOCOL_KISS = 2,
     PROTOCOL_KISSALL = 3,
     PROTOCOL_CASTLE = 4,
+    PROTOCOL_DSHOT = 5,
     PROTOCOL_COUNT
 } escProtocol_e;
 
+#define ALL_ESCS 255
+
 serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode);
 
 // serialPort API

src/main/fc/cli.c

@@ -3161,64 +3161,96 @@ static void cliGpsPassthrough(char *cmdline)
 #ifdef USE_ESCSERIAL
 static void cliEscPassthrough(char *cmdline)
 {
-    uint8_t mode = 0;
-    int index = 0;
-    int i = 0;
-    char *pch = NULL;
-    char *saveptr;
-
     if (isEmpty(cmdline)) {
         cliShowParseError();
+
         return;
     }
 
-    pch = strtok_r(cmdline, " ", &saveptr);
+    char *saveptr;
+    char *pch = strtok_r(cmdline, " ", &saveptr);
+    int pos = 0;
+    uint8_t mode = 0;
+    int escNumber = 0;
     while (pch != NULL) {
-        switch (i) {
+        switch (pos) {
             case 0:
-                if(strncasecmp(pch, "sk", strlen(pch)) == 0)
+                if(strncasecmp(pch, "sk", strlen(pch)) == 0) {
-                {
+                    mode = PROTOCOL_SIMONK;
-                    mode = 0;
+                } else if(strncasecmp(pch, "bl", strlen(pch)) == 0) {
+                    mode = PROTOCOL_BLHELI;
+                } else if(strncasecmp(pch, "ki", strlen(pch)) == 0) {
+                    mode = PROTOCOL_KISS;
+                } else if(strncasecmp(pch, "cc", strlen(pch)) == 0) {
+                    mode = PROTOCOL_KISSALL;
                 }
-                else if(strncasecmp(pch, "bl", strlen(pch)) == 0)
+#ifdef USE_DSHOT
-                {
+                else if(strncasecmp(pch, "ds", strlen(pch)) == 0) {
-                    mode = 1;
+                    mode = PROTOCOL_DSHOT;
-                }
+
-                else if(strncasecmp(pch, "ki", strlen(pch)) == 0)
+                    motorControlEnable = false;
-                {
-                    mode = 2;
-                }
-                else if(strncasecmp(pch, "cc", strlen(pch)) == 0)
-                {
-                    mode = 4;
                 }
+#endif
                 else
                 {
                     cliShowParseError();
+
                     return;
                 }
                 break;
             case 1:
-                index = atoi(pch);
+                escNumber = atoi(pch);
-                if(mode == 2 && index == 255)
+                if ((mode == PROTOCOL_KISS || mode == PROTOCOL_DSHOT) && escNumber == ALL_ESCS) {
-                {
+                    printf("Programming on all ESCs.\r\n");
-                    printf("passthrough on all outputs enabled\r\n");
+                } else {
-                }
+                    if ((escNumber >= 0) && (escNumber < getMotorCount())) {
-                else{
+                        printf("Programming on ESC %d.\r\n", escNumber);
-                    if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) {
+                    } else {
-                        printf("passthrough on output %d enabled\r\n", index);
+                        printf("Invalid ESC number, range: 1 to %d.\r\n", getMotorCount());
-                    }
+
-                    else {
-                        printf("invalid output, range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT);
                         return;
                     }
                 }
                 break;
+            default:
+#ifdef USE_DSHOT
+                if (mode == PROTOCOL_DSHOT && motorConfig()->dev.motorPwmProtocol >= PWM_TYPE_DSHOT150) {
+                    int command = atoi(pch);
+                    if (command >= 0 && command < DSHOT_MIN_THROTTLE) {
+                        if (escNumber == ALL_ESCS) {
+                            for (unsigned i = 0; i < getMotorCount(); i++) {
+                                pwmWriteDshotCommand(i, command);
+                            }
+                        } else {
+                            pwmWriteDshotCommand(escNumber, command);
+                        }
+
+                        if (command <= 5) {
+                            delay(10);
+                        }
+
+                        printf("Command %d written.\r\n", command);
+                    } else {
+                        printf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1);
+                    }
+
+                    break;
+                } else
+#endif
+                    cliShowParseError();
+
+                return;
+
         }
-        i++;
+        pos++;
         pch = strtok_r(NULL, " ", &saveptr);
     }
-    escEnablePassthrough(cliPort,index,mode);
+
+    if (mode != PROTOCOL_DSHOT) {
+        escEnablePassthrough(cliPort, escNumber, mode);
+    } else {
+        motorControlEnable = true;
+    }
 }
 #endif
 
@@ -3293,13 +3325,13 @@ static void cliMotor(char *cmdline)
     if (index == 2) {
         if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) {
             cliShowArgumentRangeError("value", 1000, 2000);
-            return;
         } else {
             motor_disarmed[motor_index] = convertExternalToMotor(motor_value);
+
+            cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index]));
         }
     }
 
-    cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index]));
 }
 
 #ifndef MINIMAL_CLI
@@ -4157,7 +4189,7 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("dump", "dump configuration",
         "[master|profile|rates|all] {showdefaults}", cliDump),
 #ifdef USE_ESCSERIAL
-    CLI_COMMAND_DEF("escprog", "passthrough esc to serial", "<mode [sk/bl/ki/cc]> <index>", cliEscPassthrough),
+    CLI_COMMAND_DEF("escprog", "passthrough esc to serial", "<mode [sk/bl/ki/cc/ds]> <index>", cliEscPassthrough),
 #endif
     CLI_COMMAND_DEF("exit", NULL, NULL, cliExit),
     CLI_COMMAND_DEF("feature", "configure features",

src/main/fc/fc_core.h

@@ -24,6 +24,8 @@ extern int16_t magHold;
 extern bool isRXDataNew;
 extern int16_t headFreeModeHold;
 
+extern uint8_t motorControlEnable;
+
 typedef struct throttleCorrectionConfig_s {
     uint16_t throttle_correction_angle;     // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
     uint8_t throttle_correction_value;      // the correction that will be applied at throttle_correction_angle.

src/main/fc/fc_msp.c

@@ -230,7 +230,7 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr
     case PROTOCOL_KISS:
     case PROTOCOL_KISSALL:
     case PROTOCOL_CASTLE:
-        if (escPortIndex < USABLE_TIMER_CHANNEL_COUNT || (escMode == PROTOCOL_KISS && escPortIndex == 255)) {
+        if (escPortIndex < getMotorCount() || (escMode == PROTOCOL_KISS && escPortIndex == ALL_ESCS)) {
             sbufWriteU8(dst, 1);
 
             if (mspPostProcessFn) {

src/main/flight/mixer.h

@@ -37,8 +37,8 @@
 
   3D Mode:
   0 = stop
-  48   (low) - 1047 (high) -> positive direction
+  48   (low) - 1047 (high) -> negative direction
-  1048 (low) - 2047 (high) -> negative direction
+  1048 (low) - 2047 (high) -> positive direction
 */
 
 // Digital protocol has fixed values