From c0bc0d9da27dafe742095736c5b32f685d96c426 Mon Sep 17 00:00:00 2001
From: Petri Mattila <petri.t.j.mattila@gmail.com>
Date: Fri, 22 Sep 2023 22:46:43 +0100
Subject: [PATCH] Fix ESC telemetry CRC calculations

---
 src/main/sensors/esc_sensor.c | 26 +++++++++++++++++++-------
 1 file changed, 19 insertions(+), 7 deletions(-)

diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c
index 34940c8bb..8e5488373 100644
--- a/src/main/sensors/esc_sensor.c
+++ b/src/main/sensors/esc_sensor.c
@@ -758,15 +758,14 @@ static void hw5SensorProcess(timeUs_t currentTimeUs)
  *
  */
 
-static uint32_t calculateCRC32(const uint8_t *buf, uint8_t length)
+static uint32_t calculateCRC32(const uint8_t *ptr, size_t len)
 {
     uint32_t crc = 0xFFFFFFFF;
 
-    for (int i = 0; i < length; i++) {
-        crc = crc ^ buf[i];
-        for (int j = 0; j < 8; j++) {
-            crc = (crc >> 1) ^ (0xEDB88320 & -(crc & 1));
-        }
+    while (len--) {
+        crc ^= *ptr++;
+        for (int i = 0; i < 8; i++)
+            crc = (crc & 1) ? (crc >> 1) ^ 0xEDB88320 : (crc >> 1);
     }
 
     return ~crc;
@@ -1123,6 +1122,19 @@ static void ztwSensorProcess(timeUs_t currentTimeUs)
  *
  */
 
+static uint16_t calculateCRC16(const uint8_t *ptr, size_t len)
+{
+    uint16_t crc = 0;
+
+    while (len--) {
+        crc ^= *ptr++;
+        for (int i = 0; i < 8; i++)
+            crc = (crc & 1) ? (crc >> 1) ^ 0x8408 : (crc >> 1);
+    }
+
+    return crc;
+}
+
 static bool processUNCTelemetryStream(uint8_t dataByte)
 {
     totalByteCount++;
@@ -1159,7 +1171,7 @@ static void uncSensorProcess(timeUs_t currentTimeUs)
         if (processUNCTelemetryStream(serialRead(escSensorPort))) {
             uint16_t crc = buffer[21] << 8 | buffer[20];
 
-            if (crc16_ccitt_update(0, buffer, 20) == crc) {
+            if (calculateCRC16(buffer, 20) == crc) {
                 uint16_t rpm = buffer[18] << 8 | buffer[17];
                 uint16_t temp = buffer[14];
                 uint16_t power = buffer[15];