Merge pull request #3987 from mikeller/added_blheli32_info_decoding
Added support for decoding BLHeli32 ESC info.
This commit is contained in:
commit
23226be980
|
@ -53,6 +53,14 @@ typedef enum {
|
||||||
DSHOT_CMD_SAVE_SETTINGS,
|
DSHOT_CMD_SAVE_SETTINGS,
|
||||||
DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
|
DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
|
||||||
DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
|
DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
|
||||||
|
DSHOT_CMD_LED0_ON, // BLHeli32 only
|
||||||
|
DSHOT_CMD_LED1_ON, // BLHeli32 only
|
||||||
|
DSHOT_CMD_LED2_ON, // BLHeli32 only
|
||||||
|
DSHOT_CMD_LED3_ON, // BLHeli32 only
|
||||||
|
DSHOT_CMD_LED0_OFF, // BLHeli32 only
|
||||||
|
DSHOT_CMD_LED1_OFF, // BLHeli32 only
|
||||||
|
DSHOT_CMD_LED2_OFF, // BLHeli32 only
|
||||||
|
DSHOT_CMD_LED3_OFF, // BLHeli32 only
|
||||||
DSHOT_CMD_MAX = 47
|
DSHOT_CMD_MAX = 47
|
||||||
} dshotCommands_e;
|
} dshotCommands_e;
|
||||||
|
|
||||||
|
|
|
@ -2259,64 +2259,98 @@ static int parseOutputIndex(char *pch, bool allowAllEscs) {
|
||||||
|
|
||||||
#ifdef USE_DSHOT
|
#ifdef USE_DSHOT
|
||||||
|
|
||||||
#define ESC_INFO_V1_EXPECTED_FRAME_SIZE 15
|
#define ESC_INFO_KISS_V1_EXPECTED_FRAME_SIZE 15
|
||||||
#define ESC_INFO_V2_EXPECTED_FRAME_SIZE 21
|
#define ESC_INFO_KISS_V2_EXPECTED_FRAME_SIZE 21
|
||||||
|
#define ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE 64
|
||||||
|
|
||||||
|
enum {
|
||||||
|
ESC_INFO_KISS_V1,
|
||||||
|
ESC_INFO_KISS_V2,
|
||||||
|
ESC_INFO_BLHELI32
|
||||||
|
};
|
||||||
|
|
||||||
#define ESC_INFO_VERSION_POSITION 12
|
#define ESC_INFO_VERSION_POSITION 12
|
||||||
|
|
||||||
void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead)
|
void printEscInfo(const uint8_t *escInfoBuffer, uint8_t bytesRead)
|
||||||
{
|
{
|
||||||
bool escInfoReceived = false;
|
bool escInfoReceived = false;
|
||||||
if (bytesRead > ESC_INFO_VERSION_POSITION) {
|
if (bytesRead > ESC_INFO_VERSION_POSITION) {
|
||||||
uint8_t escInfoVersion = 0;
|
uint8_t escInfoVersion;
|
||||||
uint8_t frameLength = 0;
|
uint8_t frameLength;
|
||||||
if (escInfoBytes[ESC_INFO_VERSION_POSITION] == 255) {
|
if (escInfoBuffer[ESC_INFO_VERSION_POSITION] == 254) {
|
||||||
escInfoVersion = 2;
|
escInfoVersion = ESC_INFO_BLHELI32;
|
||||||
frameLength = ESC_INFO_V2_EXPECTED_FRAME_SIZE;
|
frameLength = ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE;
|
||||||
|
} else if (escInfoBuffer[ESC_INFO_VERSION_POSITION] == 255) {
|
||||||
|
escInfoVersion = ESC_INFO_KISS_V2;
|
||||||
|
frameLength = ESC_INFO_KISS_V2_EXPECTED_FRAME_SIZE;
|
||||||
} else {
|
} else {
|
||||||
escInfoVersion = 1;
|
escInfoVersion = ESC_INFO_KISS_V1;
|
||||||
frameLength = ESC_INFO_V1_EXPECTED_FRAME_SIZE;
|
frameLength = ESC_INFO_KISS_V1_EXPECTED_FRAME_SIZE;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (((escInfoVersion == 1) && (bytesRead == ESC_INFO_V1_EXPECTED_FRAME_SIZE))
|
if (bytesRead == frameLength) {
|
||||||
|| ((escInfoVersion == 2) && (bytesRead == ESC_INFO_V2_EXPECTED_FRAME_SIZE))) {
|
|
||||||
escInfoReceived = true;
|
escInfoReceived = true;
|
||||||
|
|
||||||
if (calculateCrc8(escInfoBytes, frameLength - 1) == escInfoBytes[frameLength - 1]) {
|
if (calculateCrc8(escInfoBuffer, frameLength - 1) == escInfoBuffer[frameLength - 1]) {
|
||||||
uint8_t firmwareVersion = 0;
|
uint8_t firmwareVersion = 0;
|
||||||
char firmwareSubVersion = 0;
|
uint8_t firmwareSubVersion = 0;
|
||||||
uint8_t escType = 0;
|
uint8_t escType = 0;
|
||||||
switch (escInfoVersion) {
|
switch (escInfoVersion) {
|
||||||
case 1:
|
case ESC_INFO_KISS_V1:
|
||||||
firmwareVersion = escInfoBytes[12];
|
firmwareVersion = escInfoBuffer[12];
|
||||||
firmwareSubVersion = (char)((escInfoBytes[13] & 0x1f) + 97);
|
firmwareSubVersion = (escInfoBuffer[13] & 0x1f) + 97;
|
||||||
escType = (escInfoBytes[13] & 0xe0) >> 5;
|
escType = (escInfoBuffer[13] & 0xe0) >> 5;
|
||||||
|
|
||||||
break;
|
break;
|
||||||
case 2:
|
case ESC_INFO_KISS_V2:
|
||||||
firmwareVersion = escInfoBytes[13];
|
firmwareVersion = escInfoBuffer[13];
|
||||||
firmwareSubVersion = (char)escInfoBytes[14];
|
firmwareSubVersion = escInfoBuffer[14];
|
||||||
escType = escInfoBytes[15];
|
escType = escInfoBuffer[15];
|
||||||
|
|
||||||
|
break;
|
||||||
|
case ESC_INFO_BLHELI32:
|
||||||
|
firmwareVersion = escInfoBuffer[13];
|
||||||
|
firmwareSubVersion = escInfoBuffer[14];
|
||||||
|
escType = escInfoBuffer[15];
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
cliPrint("ESC: ");
|
cliPrint("ESC: ");
|
||||||
|
switch (escInfoVersion) {
|
||||||
|
case ESC_INFO_KISS_V1:
|
||||||
|
case ESC_INFO_KISS_V2:
|
||||||
switch (escType) {
|
switch (escType) {
|
||||||
case 1:
|
case 1:
|
||||||
cliPrintLine("KISS8A");
|
cliPrintLine("KISS8A");
|
||||||
|
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
cliPrintLine("KISS16A");
|
cliPrintLine("KISS16A");
|
||||||
|
|
||||||
break;
|
break;
|
||||||
case 3:
|
case 3:
|
||||||
cliPrintLine("KISS24A");
|
cliPrintLine("KISS24A");
|
||||||
|
|
||||||
break;
|
break;
|
||||||
case 5:
|
case 5:
|
||||||
cliPrintLine("KISS Ultralite");
|
cliPrintLine("KISS Ultralite");
|
||||||
|
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
cliPrintLine("unknown");
|
cliPrintLine("unknown");
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
case ESC_INFO_BLHELI32:
|
||||||
|
{
|
||||||
|
char *escType = (char *)(escInfoBuffer + 31);
|
||||||
|
escType[32] = 0;
|
||||||
|
cliPrintLine(escType);
|
||||||
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -2325,14 +2359,64 @@ void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead)
|
||||||
if (i && (i % 3 == 0)) {
|
if (i && (i % 3 == 0)) {
|
||||||
cliPrint("-");
|
cliPrint("-");
|
||||||
}
|
}
|
||||||
cliPrintf("%02x", escInfoBytes[i]);
|
cliPrintf("%02x", escInfoBuffer[i]);
|
||||||
}
|
}
|
||||||
cliPrintLinefeed();
|
cliPrintLinefeed();
|
||||||
|
|
||||||
cliPrintLinef("Firmware: %d.%02d%c", firmwareVersion / 100, firmwareVersion % 100, firmwareSubVersion);
|
switch (escInfoVersion) {
|
||||||
if (escInfoVersion == 2) {
|
case ESC_INFO_KISS_V1:
|
||||||
cliPrintLinef("Rotation: %s", escInfoBytes[16] ? "reversed" : "normal");
|
case ESC_INFO_KISS_V2:
|
||||||
cliPrintLinef("3D: %s", escInfoBytes[17] ? "on" : "off");
|
cliPrintLinef("Firmware: %d.%02d%c", firmwareVersion / 100, firmwareVersion % 100, (char)firmwareSubVersion);
|
||||||
|
|
||||||
|
break;
|
||||||
|
case ESC_INFO_BLHELI32:
|
||||||
|
cliPrintLinef("Firmware: %d.%02d%", firmwareVersion, firmwareSubVersion);
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (escInfoVersion == ESC_INFO_KISS_V2 || escInfoVersion == ESC_INFO_BLHELI32) {
|
||||||
|
cliPrintLinef("Rotation: %s", escInfoBuffer[16] ? "reversed" : "normal");
|
||||||
|
cliPrintLinef("3D: %s", escInfoBuffer[17] ? "on" : "off");
|
||||||
|
if (escInfoVersion == ESC_INFO_BLHELI32) {
|
||||||
|
uint8_t setting = escInfoBuffer[18];
|
||||||
|
cliPrint("Low voltage limit: ");
|
||||||
|
switch (setting) {
|
||||||
|
case 0:
|
||||||
|
cliPrintLine("off");
|
||||||
|
|
||||||
|
break;
|
||||||
|
case 255:
|
||||||
|
cliPrintLine("unsupported");
|
||||||
|
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
cliPrintLinef("%d.%01d", setting / 10, setting % 10);
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
setting = escInfoBuffer[19];
|
||||||
|
cliPrint("Current limit: ");
|
||||||
|
switch (setting) {
|
||||||
|
case 0:
|
||||||
|
cliPrintLine("off");
|
||||||
|
|
||||||
|
break;
|
||||||
|
case 255:
|
||||||
|
cliPrintLine("unsupported");
|
||||||
|
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
cliPrintLinef("%d", setting);
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i = 0; i < 4; i++) {
|
||||||
|
setting = escInfoBuffer[i + 20];
|
||||||
|
cliPrintLinef("LED %d: %s", i, setting ? (setting == 255) ? "unsupported" : "on" : "off");
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
cliPrintLine("Checksum error.");
|
cliPrintLine("Checksum error.");
|
||||||
|
@ -2347,14 +2431,15 @@ void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead)
|
||||||
|
|
||||||
static void executeEscInfoCommand(uint8_t escIndex)
|
static void executeEscInfoCommand(uint8_t escIndex)
|
||||||
{
|
{
|
||||||
uint8_t escInfoBuffer[ESC_INFO_V2_EXPECTED_FRAME_SIZE];
|
|
||||||
cliPrintLinef("Info for ESC %d:", escIndex);
|
cliPrintLinef("Info for ESC %d:", escIndex);
|
||||||
|
|
||||||
startEscDataRead(escInfoBuffer, ESC_INFO_V2_EXPECTED_FRAME_SIZE);
|
uint8_t escInfoBuffer[ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE];
|
||||||
|
|
||||||
|
startEscDataRead(escInfoBuffer, ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE);
|
||||||
|
|
||||||
pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO);
|
pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO);
|
||||||
|
|
||||||
delay(5);
|
delay(10);
|
||||||
|
|
||||||
printEscInfo(escInfoBuffer, getNumberEscBytesRead());
|
printEscInfo(escInfoBuffer, getNumberEscBytesRead());
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue