compute checksum

tinymovr · May 3, 2024 · 54d0023 · 54d0023
1 parent fb247a7
commit 54d0023
Showing 1 changed file with 27 additions and 10 deletions.
diff --git a/firmware/src/sensor/amt22.h b/firmware/src/sensor/amt22.h
@@ -46,6 +46,9 @@ typedef struct
     AMT22SensorConfig config;
     uint8_t errors;
     int32_t angle;
+    uint8_t val_l;
+    uint8_t val_h;
+
 } AMT22Sensor;
 
 void amt22_make_blank_sensor(Sensor *s);
@@ -92,21 +95,35 @@ static inline int32_t amt22_get_raw_angle(const Sensor *s)
 static inline void amt22_update(Sensor *s, bool check_error)
 {
     AMT22Sensor *as = (AMT22Sensor *)s;
-    volatile uint8_t val_l = ssp_read_one(as->config.ssp_struct);
-    volatile uint8_t val_h = ssp_read_one(as->config.ssp_struct);
 
-    const int32_t angle = (((val_h & 0xff) << 8) | (val_l & 0xff)) & 0x3FFF; 
-    if (check_error)
+    const int32_t value = (((as->val_h & 0xff) << 8) | (as->val_l & 0xff)); 
+
+    // TODO: This checksum calculation is sub-optimal
+    bool binaryArray[16];
+
+    for(int i = 0; i < 16; i++) binaryArray[i] = (0x01) & (value >> (i));
+
+    if ((binaryArray[15] == !(binaryArray[13] ^ binaryArray[11] ^ binaryArray[9] ^ binaryArray[7] ^ binaryArray[5] ^ binaryArray[3] ^ binaryArray[1]))
+          && (binaryArray[14] == !(binaryArray[12] ^ binaryArray[10] ^ binaryArray[8] ^ binaryArray[6] ^ binaryArray[4] ^ binaryArray[2] ^ binaryArray[0])))
     {
-        const int32_t delta = as->angle - angle;
-        if ( ((delta > AMT22_MAX_ALLOWED_DELTA) || (delta < -AMT22_MAX_ALLOWED_DELTA)) &&
-            ((delta > AMT22_MAX_ALLOWED_DELTA_ADD) || (delta < AMT22_MIN_ALLOWED_DELTA_ADD)) &&
-            ((delta > AMT22_MAX_ALLOWED_DELTA_SUB) || (delta < AMT22_MIN_ALLOWED_DELTA_SUB)) )
+        const int32_t angle = value & 0x3FFF; 
+        if (check_error)
         {
-            as->errors |= SENSORS_SETUP_ONBOARD_ERRORS_READING_UNSTABLE;
+            const int32_t delta = as->angle - angle;
+            if ( ((delta > AMT22_MAX_ALLOWED_DELTA) || (delta < -AMT22_MAX_ALLOWED_DELTA)) &&
+                ((delta > AMT22_MAX_ALLOWED_DELTA_ADD) || (delta < AMT22_MIN_ALLOWED_DELTA_ADD)) &&
+                ((delta > AMT22_MAX_ALLOWED_DELTA_SUB) || (delta < AMT22_MIN_ALLOWED_DELTA_SUB)) )
+            {
+
+            }
+            else
+            {
+                as->angle = value & 0x3FFF;
+            }
         }
+
     }
-    as->angle = angle;
+
 }