fix reading of AMT22

tinymovr · Apr 15, 2024 · 997a42e · 997a42e
1 parent e4512a2
commit 997a42e
Showing 1 changed file with 10 additions and 21 deletions.
diff --git a/firmware/src/sensor/amt22.h b/firmware/src/sensor/amt22.h
@@ -84,32 +84,21 @@ 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 uint16_t read_value = ssp_read_one(as->config.ssp_struct);
+    volatile uint8_t val_l = ssp_read_one(as->config.ssp_struct);
+    volatile uint8_t val_h = ssp_read_one(as->config.ssp_struct);
 
-    int odd_mask = 0x2AAA;  // 0010101010101010, ignoring bits at indexes 14, 15
-    int even_mask = 0x1555;  // 0001010101010101, ignoring bits at indexes 14, 15
-    // 0xAAA8
-    // 0x5554
-
-    const int odd_parity = calculate_parity(read_value, odd_mask);
-    const int even_parity = calculate_parity(read_value, even_mask);
-
-    // Only update angle if parity check passes
-    if (((read_value & 0x01) == odd_parity) && ((read_value & 0x02) == even_parity))
+    const int32_t angle = (((val_h & 0xff) << 8) | (val_l & 0xff)) & 0x3FFF; 
+    if (check_error)
     {
-        const int32_t angle = read_value & 0x3FFF; 
-        if (check_error)
+        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 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)) )
-            {
-                as->errors |= SENSORS_SETUP_ONBOARD_ERRORS_READING_UNSTABLE;
-            }
+            as->errors |= SENSORS_SETUP_ONBOARD_ERRORS_READING_UNSTABLE;
         }
-        as->angle = angle;
     }
+    as->angle = angle;
 }