Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Auto detection fix for Kontronik ESCs - verified Kolibri v3.0/v3.5, Kosmik 200 HV v4.17, JivePro 120 HV v1.13 #100

Merged
merged 2 commits into from
Apr 25, 2024
Merged

Auto detection fix for Kontronik ESCs - verified Kolibri v3.0/v3.5, Kosmik 200 HV v4.17, JivePro 120 HV v1.13 #100

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
56fff93
Auto detection fix for legacy Kontronik ESCs - verified Kolibri v3.0/…
bob01 Apr 15, 2024
a0a29af
Auto detection fix for legacy Kontronik ESCs - minor code cleanup - v…
bob01 Apr 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
47 changes: 43 additions & 4 deletions src/main/sensors/esc_sensor.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1012,6 +1012,12 @@ static void uncSensorProcess(timeUs_t currentTimeUs)
* 36-39: CRC32
*
*/
#define KON_FRAME_LENGTH 40
#define KON_FRAME_LENGTH_LEGACY 38
#define KON_CRC_LENGTH 4

static uint8_t kontronikPacketLength = 0; // 40 or 38 byte
static uint8_t kontronikCrcExclude = 0; // 0 or 2

static uint32_t calculateCRC32(const uint8_t *ptr, size_t len)
{
Expand All @@ -1026,6 +1032,11 @@ static uint32_t calculateCRC32(const uint8_t *ptr, size_t len)
return ~crc;
}

static uint32_t kontronikDecodeCRC(uint8_t index)
{
return buffer[index + 3] << 24 | buffer[index + 2] << 16 | buffer[index + 1] << 8 | buffer[index];
}

static bool processKontronikTelemetryStream(uint8_t dataByte)
{
totalByteCount++;
Expand All @@ -1050,7 +1061,36 @@ static bool processKontronikTelemetryStream(uint8_t dataByte)
else
syncCount++;
}
else if (readBytes == 40) {
else if (readBytes == kontronikPacketLength) {
readBytes = 0;
return true;
}
else if (kontronikPacketLength == 0 && readBytes == KON_FRAME_LENGTH_LEGACY) {
// auto detect legacy 38 byte packet...
uint32_t crc = kontronikDecodeCRC(KON_FRAME_LENGTH_LEGACY - KON_CRC_LENGTH);
if (calculateCRC32(buffer, KON_FRAME_LENGTH_LEGACY - 2 - KON_CRC_LENGTH) == crc) {
// ...w/ 32 byte payload (2 bytes excluded)
kontronikPacketLength = KON_FRAME_LENGTH_LEGACY;
kontronikCrcExclude = 2;
readBytes = 0;
return true;
}
else if (calculateCRC32(buffer, KON_FRAME_LENGTH_LEGACY - KON_CRC_LENGTH) == crc) {
// ...w/ 34 byte payload
kontronikPacketLength = KON_FRAME_LENGTH_LEGACY;
kontronikCrcExclude = 0;
readBytes = 0;
return true;
}
}
else if (kontronikPacketLength == 0 && readBytes == KON_FRAME_LENGTH) {
// auto detect 40 byte packet...
uint32_t crc = kontronikDecodeCRC(KON_FRAME_LENGTH - KON_CRC_LENGTH);
if (calculateCRC32(buffer, KON_FRAME_LENGTH - KON_CRC_LENGTH) == crc) {
// ...w/ 36 byte payload
kontronikPacketLength = KON_FRAME_LENGTH;
kontronikCrcExclude = 0;
}
readBytes = 0;
return true;
}
Expand All @@ -1063,9 +1103,8 @@ static void kontronikSensorProcess(timeUs_t currentTimeUs)
// check for any available bytes in the rx buffer
while (serialRxBytesWaiting(escSensorPort)) {
if (processKontronikTelemetryStream(serialRead(escSensorPort))) {
uint32_t crc = buffer[39] << 24 | buffer[38] << 16 | buffer[37] << 8 | buffer[36];

if (calculateCRC32(buffer, 36) == crc) {
uint32_t crc = kontronikDecodeCRC(kontronikPacketLength - KON_CRC_LENGTH);
if (calculateCRC32(buffer, kontronikPacketLength - kontronikCrcExclude - KON_CRC_LENGTH) == crc) {
uint32_t rpm = buffer[7] << 24 | buffer[6] << 16 | buffer[5] << 8 | buffer[4];
uint16_t pwm = buffer[23] << 8 | buffer[22];
uint16_t voltage = buffer[9] << 8 | buffer[8];
Expand Down