From 1f25b44c0362b72489d5536f09b8582860663c24 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 9 Jul 2017 20:16:15 +1200 Subject: [PATCH 1/2] Added display of (KISS) ESC version info for 'dshotprog 6'. --- src/main/fc/cli.c | 64 +++++++++++++++++++++++++++++------ src/main/sensors/esc_sensor.c | 48 ++++++++++++++++---------- src/main/sensors/esc_sensor.h | 3 ++ 3 files changed, 87 insertions(+), 28 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index b56e0f8f0b..0ecf3488e0 100755 --- a/src/main/fc/cli.c +++ b/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); } @@ -2202,6 +2203,21 @@ static int parseOutputIndex(char *pch, bool allowAllEscs) { } #ifdef USE_DSHOT + +#define ESC_INFO_EXPECTED_FRAME_SIZE 15 + +void printEscInfo(const uint8_t *escInfoBytes) +{ + cliPrint("MCU Id: 0x"); + for (int i = 0; i < 12; i++) { + cliPrintf("%02x", escInfoBytes[i]); + } + cliPrintLinef("\nFirmware version: %d.%02d%c", escInfoBytes[12] / 100, escInfoBytes[12] % 100, (const char)((escInfoBytes[13] & 0x1f) + 97)); + + uint8_t escType = (escInfoBytes[13] & 0xe0) >> 5; + cliPrintLinef("ESC type: %d", escType); +} + static void cliDshotProg(char *cmdline) { if (isEmpty(cmdline) || motorConfig()->dev.motorPwmProtocol < PWM_TYPE_DSHOT150) { @@ -2232,17 +2248,45 @@ 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_EXPECTED_FRAME_SIZE]; + if (command == DSHOT_CMD_ESC_INFO) { + delay(10); // Wait for potential ESC telemetry transmission to finish + + startEscDataRead(ESC_INFO_EXPECTED_FRAME_SIZE, escInfoBuffer); + } + pwmWriteDshotCommand(escIndex, command); + + if (command == DSHOT_CMD_ESC_INFO) { + bool escInfoReceived = false; + for (int i = 0; i < 10; i++) { + delay(20); + + if (checkEscDataReadFinished()) { + escInfoReceived = true; + + break; + } + } + + if (escInfoReceived) { + printEscInfo(escInfoBuffer); + } else { + cliPrint("No ESC info received."); + } + } 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; diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 75120fe4ec..f1991f42f9 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -97,12 +97,14 @@ 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; -static uint8_t tlm[ESC_SENSOR_BUFFSIZE] = { 0, }; +#define TELEMETRY_FRAME_SIZE 10 +static uint8_t telemetryBuffer[TELEMETRY_FRAME_SIZE] = { 0, }; + +static volatile uint8_t *buffer; +static volatile uint8_t tlmExpectedFrameSize = 0; static uint8_t tlmFramePosition = 0; static serialPort_t *escSensorPort = NULL; @@ -119,6 +121,17 @@ static bool combinedDataNeedsUpdate = true; static uint16_t totalTimeoutCount = 0; static uint16_t totalCrcErrorCount = 0; +void startEscDataRead(uint8_t frameLength, uint8_t *frameBuffer) +{ + buffer = frameBuffer; + tlmExpectedFrameSize = frameLength; +} + +bool checkEscDataReadFinished(void) +{ + return tlmExpectedFrameSize == 0; +} + bool isEscSensorActive(void) { return escSensorPort != NULL; @@ -166,18 +179,16 @@ 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 (!tlmExpectedFrameSize) { return; } - tlm[tlmFramePosition] = (uint8_t)c; + buffer[tlmFramePosition++] = (uint8_t)c; - if (tlmFramePosition == ESC_SENSOR_BUFFSIZE - 1) { + if (tlmFramePosition == tlmExpectedFrameSize) { tlmFramePosition = 0; - tlmFramePending = false; - } else { - tlmFramePosition++; + tlmExpectedFrameSize = 0; } } @@ -221,21 +232,21 @@ static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen) static uint8_t decodeEscFrame(void) { - if (tlmFramePending) { + if (tlmExpectedFrameSize) { return ESC_SENSOR_FRAME_PENDING; } // Get CRC8 checksum - uint16_t chksum = get_crc8(tlm, ESC_SENSOR_BUFFSIZE - 1); - uint16_t tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1]; // last byte contains CRC value + uint16_t chksum = get_crc8(telemetryBuffer, TELEMETRY_FRAME_SIZE - 1); + 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].voltage = tlm[1] << 8 | tlm[2]; - escSensorData[escSensorMotor].current = tlm[3] << 8 | tlm[4]; - escSensorData[escSensorMotor].consumption = tlm[5] << 8 | tlm[6]; - escSensorData[escSensorMotor].rpm = tlm[7] << 8 | tlm[8]; + escSensorData[escSensorMotor].temperature = telemetryBuffer[0]; + escSensorData[escSensorMotor].voltage = telemetryBuffer[1] << 8 | telemetryBuffer[2]; + escSensorData[escSensorMotor].current = telemetryBuffer[3] << 8 | telemetryBuffer[4]; + escSensorData[escSensorMotor].consumption = telemetryBuffer[5] << 8 | telemetryBuffer[6]; + escSensorData[escSensorMotor].rpm = telemetryBuffer[7] << 8 | telemetryBuffer[8]; combinedDataNeedsUpdate = true; @@ -286,7 +297,8 @@ void escSensorProcess(timeUs_t currentTimeUs) case ESC_SENSOR_TRIGGER_READY: escTriggerTimestamp = currentTimeMs; - tlmFramePending = true; + buffer = telemetryBuffer; + tlmExpectedFrameSize = TELEMETRY_FRAME_SIZE; motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor); motor->requestTelemetry = true; escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING; diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index 45c38ae826..e355d791f5 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -45,3 +45,6 @@ void escSensorProcess(timeUs_t currentTime); escSensorData_t *getEscSensorData(uint8_t motorNumber); +void startEscDataRead(uint8_t frameLength, uint8_t *frameBuffer); +bool checkEscDataReadFinished(void); + From 613e5c3b2a312f50497033309a7710e230013f8d Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 10 Jul 2017 00:07:03 +1200 Subject: [PATCH 2/2] Added checksum verification. --- src/main/fc/cli.c | 116 ++++++++++++++++++++++++++-------- src/main/sensors/esc_sensor.c | 46 +++++++------- src/main/sensors/esc_sensor.h | 5 +- 3 files changed, 117 insertions(+), 50 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 0ecf3488e0..4d1eb1db77 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2204,18 +2204,95 @@ static int parseOutputIndex(char *pch, bool allowAllEscs) { #ifdef USE_DSHOT -#define ESC_INFO_EXPECTED_FRAME_SIZE 15 +#define ESC_INFO_V1_EXPECTED_FRAME_SIZE 15 +#define ESC_INFO_V2_EXPECTED_FRAME_SIZE 21 -void printEscInfo(const uint8_t *escInfoBytes) +#define ESC_INFO_VERSION_POSITION 12 + +void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead) { - cliPrint("MCU Id: 0x"); - for (int i = 0; i < 12; i++) { - cliPrintf("%02x", escInfoBytes[i]); - } - cliPrintLinef("\nFirmware version: %d.%02d%c", escInfoBytes[12] / 100, escInfoBytes[12] % 100, (const char)((escInfoBytes[13] & 0x1f) + 97)); + 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; + } - uint8_t escType = (escInfoBytes[13] & 0xe0) >> 5; - cliPrintLinef("ESC type: %d", escType); + 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) @@ -2251,32 +2328,19 @@ static void cliDshotProg(char *cmdline) cliPrintf("Command %d written.\r\n", command); } else { - uint8_t escInfoBuffer[ESC_INFO_EXPECTED_FRAME_SIZE]; + 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(ESC_INFO_EXPECTED_FRAME_SIZE, escInfoBuffer); + startEscDataRead(escInfoBuffer, ESC_INFO_V2_EXPECTED_FRAME_SIZE); } pwmWriteDshotCommand(escIndex, command); if (command == DSHOT_CMD_ESC_INFO) { - bool escInfoReceived = false; - for (int i = 0; i < 10; i++) { - delay(20); + delay(10); - if (checkEscDataReadFinished()) { - escInfoReceived = true; - - break; - } - } - - if (escInfoReceived) { - printEscInfo(escInfoBuffer); - } else { - cliPrint("No ESC info received."); - } + printEscInfo(escInfoBuffer, getNumberEscBytesRead()); } else { cliPrintf("Command %d written.\r\n", command); } diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index f1991f42f9..3a2d5bfcab 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -104,8 +104,8 @@ typedef enum { static uint8_t telemetryBuffer[TELEMETRY_FRAME_SIZE] = { 0, }; static volatile uint8_t *buffer; -static volatile uint8_t tlmExpectedFrameSize = 0; -static uint8_t tlmFramePosition = 0; +static volatile uint8_t bufferSize = 0; +static volatile uint8_t bufferPosition = 0; static serialPort_t *escSensorPort = NULL; @@ -121,15 +121,21 @@ static bool combinedDataNeedsUpdate = true; static uint16_t totalTimeoutCount = 0; static uint16_t totalCrcErrorCount = 0; -void startEscDataRead(uint8_t frameLength, uint8_t *frameBuffer) +void startEscDataRead(uint8_t *frameBuffer, uint8_t frameLength) { buffer = frameBuffer; - tlmExpectedFrameSize = frameLength; + bufferPosition = 0; + bufferSize = frameLength; } -bool checkEscDataReadFinished(void) +uint8_t getNumberEscBytesRead(void) { - return tlmExpectedFrameSize == 0; + return bufferPosition; +} + +static bool isFrameComplete(void) +{ + return bufferPosition == bufferSize; } bool isEscSensorActive(void) @@ -179,17 +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 (!tlmExpectedFrameSize) { + if (isFrameComplete()) { return; } - buffer[tlmFramePosition++] = (uint8_t)c; - - if (tlmFramePosition == tlmExpectedFrameSize) { - tlmFramePosition = 0; - - tlmExpectedFrameSize = 0; - } + buffer[bufferPosition++] = (uint8_t)c; } bool escSensorInit(void) @@ -211,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; @@ -223,21 +223,24 @@ 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; irequestTelemetry = true; escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING; diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index e355d791f5..8caa7a7015 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -45,6 +45,7 @@ void escSensorProcess(timeUs_t currentTime); escSensorData_t *getEscSensorData(uint8_t motorNumber); -void startEscDataRead(uint8_t frameLength, uint8_t *frameBuffer); -bool checkEscDataReadFinished(void); +void startEscDataRead(uint8_t *frameBuffer, uint8_t frameLength); +uint8_t getNumberEscBytesRead(void); +uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen);