From 54d0023aa02a5b7d1e7975995f1ddb78173da68b Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Fri, 3 May 2024 13:55:42 +0300 Subject: [PATCH] compute checksum --- firmware/src/sensor/amt22.h | 37 +++++++++++++++++++++++++++---------- 1 file changed, 27 insertions(+), 10 deletions(-) diff --git a/firmware/src/sensor/amt22.h b/firmware/src/sensor/amt22.h index b3cc81fb..86a11d6e 100644 --- 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; + }