Auto detection fix for Kontronik ESCs (#100)

* Auto detection fix for legacy Kontronik ESCs - verified Kolibri v3.0/v3.5, Kosmik 200 HV v4.17, JivePro 120 HV v1.13 * Auto detection fix for legacy Kontronik ESCs - minor code cleanup - verified w/ Kolibri v3.5
rotorflight · Apr 25, 2024 · 26b8745 · 26b8745
1 parent ef7d769
commit 26b8745
Showing 1 changed file with 43 additions and 4 deletions.
diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c
@@ -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)
 {
@@ -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++;
@@ -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;
     }
@@ -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];