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 #99

Closed
wants to merge 4 commits into from
Closed

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

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
23f9ecf
Fix for current FW Kontronik ESCs - verified Kolibri v3.0/v3.5, Kosmi…
bob01 Apr 14, 2024
2c6fa41
Fix for current FW Kontronik ESCs - updated protocol documentation
bob01 Apr 14, 2024
c965658
Merge branch 'master' into master
bob01 Apr 14, 2024
5ff4856
Auto detection fix for Kontronik ESCs - verified Kolibri v3.0/v3.5, K…
bob01 Apr 14, 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
50 changes: 45 additions & 5 deletions src/main/sensors/esc_sensor.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1008,9 +1008,16 @@ static void uncSensorProcess(timeUs_t currentTimeUs)
* 28-31: Error Flags
* 32: Operational condition
* 33: Timing 0..30
* 34-37: CRC32
* 34-35: Reserved
* 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 @@ -1025,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 @@ -1049,7 +1061,36 @@ static bool processKontronikTelemetryStream(uint8_t dataByte)
else
syncCount++;
}
else if (readBytes == 38) {
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(40 - 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 @@ -1062,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[37] << 24 | buffer[36] << 16 | buffer[35] << 8 | buffer[34];

if (calculateCRC32(buffer, 34) == 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