Merge pull request #3474 from mikeller/add_esc_version_info

Added display of (KISS) ESC version info for 'dshotprog <ESC index> 6'.

src/main/fc/cli.c

@@ -120,6 +120,7 @@ extern uint8_t __config_end;
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
 #include "sensors/compass.h"
+#include "sensors/esc_sensor.h"
 #include "sensors/gyro.h"
 #include "sensors/sensors.h"
 
@@ -877,12 +878,12 @@ static void cliSerialPassthrough(char *cmdline)
         tok = strtok_r(NULL, " ", &saveptr);
     }
 
-    tfp_printf("Port %d ", id);
+    cliPrintf("Port %d ", id);
     serialPort_t *passThroughPort;
     serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id);
     if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) {
         if (!baud) {
-            tfp_printf("closed, specify baud.\r\n");
+            cliPrint("closed, specify baud.\r\n");
             return;
         }
         if (!mode)
@@ -892,17 +893,17 @@ static void cliSerialPassthrough(char *cmdline)
                                          baud, mode,
                                          SERIAL_NOT_INVERTED);
         if (!passThroughPort) {
-            tfp_printf("could not be opened.\r\n");
+            cliPrint("could not be opened.\r\n");
             return;
         }
-        tfp_printf("opened, baud = %d.\r\n", baud);
+        cliPrintf("opened, baud = %d.\r\n", baud);
     } else {
         passThroughPort = passThroughPortUsage->serialPort;
         // If the user supplied a mode, override the port's mode, otherwise
         // leave the mode unchanged. serialPassthrough() handles one-way ports.
-        tfp_printf("already open.\r\n");
+        cliPrint("already open.\r\n");
         if (mode && passThroughPort->mode != mode) {
-            tfp_printf("mode changed from %d to %d.\r\n",
+            cliPrintf("mode changed from %d to %d.\r\n",
                    passThroughPort->mode, mode);
             serialSetMode(passThroughPort, mode);
         }
@@ -913,7 +914,7 @@ static void cliSerialPassthrough(char *cmdline)
         }
     }
 
-    tfp_printf("forwarding, power cycle to exit.\r\n");
+    cliPrint("Forwarding, power cycle to exit.\r\n");
 
     serialPassthrough(cliPort, passThroughPort, NULL, NULL);
 }
@@ -2212,6 +2213,98 @@ static int parseOutputIndex(char *pch, bool allowAllEscs) {
 }
 
 #ifdef USE_DSHOT
+
+#define ESC_INFO_V1_EXPECTED_FRAME_SIZE 15
+#define ESC_INFO_V2_EXPECTED_FRAME_SIZE 21
+
+#define ESC_INFO_VERSION_POSITION 12
+
+void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead)
+{
+    bool escInfoReceived = false;
+    if (bytesRead > ESC_INFO_VERSION_POSITION) {
+        uint8_t escInfoVersion = 0;
+        uint8_t frameLength = 0;
+        if (escInfoBytes[ESC_INFO_VERSION_POSITION] == 255) {
+            escInfoVersion = 2;
+            frameLength = ESC_INFO_V2_EXPECTED_FRAME_SIZE;
+        } else {
+            escInfoVersion = 1;
+            frameLength = ESC_INFO_V1_EXPECTED_FRAME_SIZE;
+        }
+
+        if (((escInfoVersion == 1) && (bytesRead == ESC_INFO_V1_EXPECTED_FRAME_SIZE))
+            || ((escInfoVersion == 2) && (bytesRead == ESC_INFO_V2_EXPECTED_FRAME_SIZE))) {
+            escInfoReceived = true;
+
+            if (calculateCrc8(escInfoBytes, frameLength - 1) == escInfoBytes[frameLength - 1]) {
+                uint8_t firmwareVersion;
+                char firmwareSubVersion;
+                uint8_t escType;
+                switch (escInfoVersion) {
+                case 1:
+                    firmwareVersion = escInfoBytes[12];
+                    firmwareSubVersion = (char)((escInfoBytes[13] & 0x1f) + 97);
+                    escType = (escInfoBytes[13] & 0xe0) >> 5;
+
+                    break;
+                case 2:
+                    firmwareVersion = escInfoBytes[13];
+                    firmwareSubVersion = (char)escInfoBytes[14];
+                    escType = escInfoBytes[15];
+
+                    break;
+                }
+
+                cliPrint("ESC: ");
+                switch (escType) {
+                case 1:
+                    cliPrintLine("KISS8A");
+
+                    break;
+                case 2:
+                    cliPrintLine("KISS16A");
+
+                    break;
+                case 3:
+                    cliPrintLine("KISS24A");
+
+                    break;
+                case 5:
+                    cliPrintLine("KISS Ultralite");
+
+                    break;
+                default:
+                    cliPrintLine("unknown");
+
+                    break;
+                }
+
+                cliPrint("MCU: 0x");
+                for (int i = 0; i < 12; i++) {
+                    if (i && (i % 3 == 0)) {
+                        cliPrint("-");
+                    }
+                    cliPrintf("%02x", escInfoBytes[i]);
+                }
+                cliPrintLinefeed();
+
+                cliPrintLinef("Firmware: %d.%02d%c", firmwareVersion / 100, firmwareVersion % 100, firmwareSubVersion);
+                if (escInfoVersion == 2) {
+                    cliPrintLinef("Rotation: %s", escInfoBytes[16] ? "reversed" : "normal");
+                    cliPrintLinef("3D: %s", escInfoBytes[17] ? "on" : "off");
+                }
+            } else {
+                cliPrint("Checksum error.");
+            }
+        }
+    }
+
+    if (!escInfoReceived) {
+        cliPrint("No info.");
+    }
+}
+
 static void cliDshotProg(char *cmdline)
 {
     if (isEmpty(cmdline) || motorConfig()->dev.motorPwmProtocol < PWM_TYPE_DSHOT150) {
@@ -2242,17 +2335,32 @@ static void cliDshotProg(char *cmdline)
                         for (unsigned i = 0; i < getMotorCount(); i++) {
                             pwmWriteDshotCommand(i, command);
                         }
+
+                        cliPrintf("Command %d written.\r\n", command);
                     } else {
+                        uint8_t escInfoBuffer[ESC_INFO_V2_EXPECTED_FRAME_SIZE];
+                        if (command == DSHOT_CMD_ESC_INFO) {
+                            delay(10); // Wait for potential ESC telemetry transmission to finish
+
+                            startEscDataRead(escInfoBuffer, ESC_INFO_V2_EXPECTED_FRAME_SIZE);
+                        }
+
                         pwmWriteDshotCommand(escIndex, command);
+
+                        if (command == DSHOT_CMD_ESC_INFO) {
+                            delay(10);
+
+                            printEscInfo(escInfoBuffer, getNumberEscBytesRead());
+                        } else {
+                            cliPrintf("Command %d written.\r\n", command);
+                        }
                     }
 
                     if (command <= 5) {
                         delay(10); // wait for sound output to finish
                     }
-
-                    tfp_printf("Command %d written.\r\n", command);
                 } else {
-                    tfp_printf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1);
+                    cliPrintf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1);
                 }
 
                 break;

src/main/sensors/esc_sensor.c

@@ -97,13 +97,15 @@ typedef enum {
 } escSensorTriggerState_t;
 
 #define ESC_SENSOR_BAUDRATE 115200
-#define ESC_SENSOR_BUFFSIZE 10
 #define ESC_BOOTTIME 5000               // 5 seconds
 #define ESC_REQUEST_TIMEOUT 100         // 100 ms (data transfer takes only 900us)
 
-static volatile bool tlmFramePending = false;
+#define TELEMETRY_FRAME_SIZE 10
-static uint8_t tlm[ESC_SENSOR_BUFFSIZE] = { 0, };
+static uint8_t telemetryBuffer[TELEMETRY_FRAME_SIZE] = { 0, };
-static uint8_t tlmFramePosition = 0;
+
+static volatile uint8_t *buffer;
+static volatile uint8_t bufferSize = 0;
+static volatile uint8_t bufferPosition = 0;
 
 static serialPort_t *escSensorPort = NULL;
 
@@ -119,6 +121,23 @@ static bool combinedDataNeedsUpdate = true;
 static uint16_t totalTimeoutCount = 0;
 static uint16_t totalCrcErrorCount = 0;
 
+void startEscDataRead(uint8_t *frameBuffer, uint8_t frameLength)
+{
+    buffer = frameBuffer;
+    bufferPosition = 0;
+    bufferSize = frameLength;
+}
+
+uint8_t getNumberEscBytesRead(void)
+{
+    return bufferPosition;
+}
+
+static bool isFrameComplete(void)
+{
+    return bufferPosition == bufferSize;
+}
+
 bool isEscSensorActive(void)
 {
     return escSensorPort != NULL;
@@ -166,19 +185,11 @@ static void escSensorDataReceive(uint16_t c)
     // KISS ESC sends some data during startup, ignore this for now (maybe future use)
     // startup data could be firmware version and serialnumber
 
-    if (!tlmFramePending) {
+    if (isFrameComplete()) {
         return;
     }
 
-    tlm[tlmFramePosition] = (uint8_t)c;
+    buffer[bufferPosition++] = (uint8_t)c;
-
-    if (tlmFramePosition == ESC_SENSOR_BUFFSIZE - 1) {
-        tlmFramePosition = 0;
-
-        tlmFramePending = false;
-    } else {
-        tlmFramePosition++;
-    }
 }
 
 bool escSensorInit(void)
@@ -200,7 +211,7 @@ bool escSensorInit(void)
     return escSensorPort != NULL;
 }
 
-static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
+static uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
 {
     uint8_t crc_u = crc;
     crc_u ^= crc_seed;
@@ -212,30 +223,33 @@ static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
     return (crc_u);
 }
 
-static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen)
+uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen)
 {
     uint8_t crc = 0;
-    for (int i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
+    for (int i = 0; i < BufLen; i++) {
-    return (crc);
+        crc = updateCrc8(Buf[i], crc);
+    }
+
+    return crc;
 }
 
 static uint8_t decodeEscFrame(void)
 {
-    if (tlmFramePending) {
+    if (!isFrameComplete()) {
         return ESC_SENSOR_FRAME_PENDING;
     }
 
     // Get CRC8 checksum
-    uint16_t chksum = get_crc8(tlm, ESC_SENSOR_BUFFSIZE - 1);
+    uint16_t chksum = calculateCrc8(telemetryBuffer, TELEMETRY_FRAME_SIZE - 1);
-    uint16_t tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1];     // last byte contains CRC value
+    uint16_t tlmsum = telemetryBuffer[TELEMETRY_FRAME_SIZE - 1];     // last byte contains CRC value
     uint8_t frameStatus;
     if (chksum == tlmsum) {
         escSensorData[escSensorMotor].dataAge = 0;
-        escSensorData[escSensorMotor].temperature = tlm[0];
+        escSensorData[escSensorMotor].temperature = telemetryBuffer[0];
-        escSensorData[escSensorMotor].voltage = tlm[1] << 8 | tlm[2];
+        escSensorData[escSensorMotor].voltage = telemetryBuffer[1] << 8 | telemetryBuffer[2];
-        escSensorData[escSensorMotor].current = tlm[3] << 8 | tlm[4];
+        escSensorData[escSensorMotor].current = telemetryBuffer[3] << 8 | telemetryBuffer[4];
-        escSensorData[escSensorMotor].consumption = tlm[5] << 8 | tlm[6];
+        escSensorData[escSensorMotor].consumption = telemetryBuffer[5] << 8 | telemetryBuffer[6];
-        escSensorData[escSensorMotor].rpm = tlm[7] << 8 | tlm[8];
+        escSensorData[escSensorMotor].rpm = telemetryBuffer[7] << 8 | telemetryBuffer[8];
 
         combinedDataNeedsUpdate = true;
 
@@ -286,7 +300,7 @@ void escSensorProcess(timeUs_t currentTimeUs)
         case ESC_SENSOR_TRIGGER_READY:
             escTriggerTimestamp = currentTimeMs;
 
-            tlmFramePending = true;
+            startEscDataRead(telemetryBuffer, TELEMETRY_FRAME_SIZE);
             motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
             motor->requestTelemetry = true;
             escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;

src/main/sensors/esc_sensor.h

@@ -45,3 +45,7 @@ void escSensorProcess(timeUs_t currentTime);
 
 escSensorData_t *getEscSensorData(uint8_t motorNumber);
 
+void startEscDataRead(uint8_t *frameBuffer, uint8_t frameLength);
+uint8_t getNumberEscBytesRead(void);
+
+uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen);