Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Added checksum verification.

Browse source
This commit is contained in:
mikeller 2017-07-10 00:07:03 +12:00
parent 1f25b44c03
commit 613e5c3b2a
3 changed files with 117 additions and 50 deletions
Download patch file Download diff file Expand all files Collapse all files

116
src/main/fc/cli.c
View file

@ -2204,18 +2204,95 @@ static int parseOutputIndex(char *pch, bool allowAllEscs) {
#ifdef USE_DSHOT #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"); bool escInfoReceived = false;
for (int i = 0; i < 12; i++) { if (bytesRead > ESC_INFO_VERSION_POSITION) {
cliPrintf("%02x", escInfoBytes[i]); uint8_t escInfoVersion = 0;
} uint8_t frameLength = 0;
cliPrintLinef("\nFirmware version: %d.%02d%c", escInfoBytes[12] / 100, escInfoBytes[12] % 100, (const char)((escInfoBytes[13] & 0x1f) + 97)); 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; if (((escInfoVersion == 1) && (bytesRead == ESC_INFO_V1_EXPECTED_FRAME_SIZE))
cliPrintLinef("ESC type: %d", escType); || ((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) static void cliDshotProg(char *cmdline)
@ -2251,32 +2328,19 @@ static void cliDshotProg(char *cmdline)
cliPrintf("Command %d written.\r\n", command); cliPrintf("Command %d written.\r\n", command);
} else { } else {
uint8_t escInfoBuffer[ESC_INFO_EXPECTED_FRAME_SIZE]; uint8_t escInfoBuffer[ESC_INFO_V2_EXPECTED_FRAME_SIZE];
if (command == DSHOT_CMD_ESC_INFO) { if (command == DSHOT_CMD_ESC_INFO) {
delay(10); // Wait for potential ESC telemetry transmission to finish 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); pwmWriteDshotCommand(escIndex, command);
if (command == DSHOT_CMD_ESC_INFO) { if (command == DSHOT_CMD_ESC_INFO) {
bool escInfoReceived = false; delay(10);
for (int i = 0; i < 10; i++) {
delay(20);
if (checkEscDataReadFinished()) { printEscInfo(escInfoBuffer, getNumberEscBytesRead());
escInfoReceived = true;
break;
}
}
if (escInfoReceived) {
printEscInfo(escInfoBuffer);
} else {
cliPrint("No ESC info received.");
}
} else { } else {
cliPrintf("Command %d written.\r\n", command); cliPrintf("Command %d written.\r\n", command);
} }

46
src/main/sensors/esc_sensor.c
View file

@ -104,8 +104,8 @@ typedef enum {
static uint8_t telemetryBuffer[TELEMETRY_FRAME_SIZE] = { 0, }; static uint8_t telemetryBuffer[TELEMETRY_FRAME_SIZE] = { 0, };
static volatile uint8_t *buffer; static volatile uint8_t *buffer;
static volatile uint8_t tlmExpectedFrameSize = 0; static volatile uint8_t bufferSize = 0;
static uint8_t tlmFramePosition = 0; static volatile uint8_t bufferPosition = 0;
static serialPort_t *escSensorPort = NULL; static serialPort_t *escSensorPort = NULL;
@ -121,15 +121,21 @@ static bool combinedDataNeedsUpdate = true;
static uint16_t totalTimeoutCount = 0; static uint16_t totalTimeoutCount = 0;
static uint16_t totalCrcErrorCount = 0; static uint16_t totalCrcErrorCount = 0;
void startEscDataRead(uint8_t frameLength, uint8_t *frameBuffer) void startEscDataRead(uint8_t *frameBuffer, uint8_t frameLength)
{ {
buffer = frameBuffer; 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) 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) // KISS ESC sends some data during startup, ignore this for now (maybe future use)
// startup data could be firmware version and serialnumber // startup data could be firmware version and serialnumber
if (!tlmExpectedFrameSize) { if (isFrameComplete()) {
return; return;
} }
buffer[tlmFramePosition++] = (uint8_t)c; buffer[bufferPosition++] = (uint8_t)c;
if (tlmFramePosition == tlmExpectedFrameSize) {
tlmFramePosition = 0;
tlmExpectedFrameSize = 0;
}
} }
bool escSensorInit(void) bool escSensorInit(void)
@ -211,7 +211,7 @@ bool escSensorInit(void)
return escSensorPort != NULL; 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; uint8_t crc_u = crc;
crc_u ^= crc_seed; crc_u ^= crc_seed;
@ -223,21 +223,24 @@ static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
return (crc_u); 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; 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) static uint8_t decodeEscFrame(void)
{ {
if (tlmExpectedFrameSize) { if (!isFrameComplete()) {
return ESC_SENSOR_FRAME_PENDING; return ESC_SENSOR_FRAME_PENDING;
} }
// Get CRC8 checksum // Get CRC8 checksum
uint16_t chksum = get_crc8(telemetryBuffer, TELEMETRY_FRAME_SIZE - 1); uint16_t chksum = calculateCrc8(telemetryBuffer, TELEMETRY_FRAME_SIZE - 1);
uint16_t tlmsum = telemetryBuffer[TELEMETRY_FRAME_SIZE - 1]; // last byte contains CRC value uint16_t tlmsum = telemetryBuffer[TELEMETRY_FRAME_SIZE - 1]; // last byte contains CRC value
uint8_t frameStatus; uint8_t frameStatus;
if (chksum == tlmsum) { if (chksum == tlmsum) {
@ -297,8 +300,7 @@ void escSensorProcess(timeUs_t currentTimeUs)
case ESC_SENSOR_TRIGGER_READY: case ESC_SENSOR_TRIGGER_READY:
escTriggerTimestamp = currentTimeMs; escTriggerTimestamp = currentTimeMs;
buffer = telemetryBuffer; startEscDataRead(telemetryBuffer, TELEMETRY_FRAME_SIZE);
tlmExpectedFrameSize = TELEMETRY_FRAME_SIZE;
motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor); motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
motor->requestTelemetry = true; motor->requestTelemetry = true;
escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING; escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;

5
src/main/sensors/esc_sensor.h
View file

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