From 261f5bdfbf0d76906279c536cdf23b494523e20b Mon Sep 17 00:00:00 2001
From: true <true_git@truecontrol.org>
Date: Thu, 24 Oct 2024 17:23:51 -0700
Subject: [PATCH] ignore accelerometer if it isn't found

---
 nametag8_CH592/user/misc/accel.c | 53 +++++++++++++++++++-------------
 nametag8_CH592/user/misc/accel.h |  2 ++
 2 files changed, 34 insertions(+), 21 deletions(-)

diff --git a/nametag8_CH592/user/misc/accel.c b/nametag8_CH592/user/misc/accel.c
index 89e93ea..501991e 100644
--- a/nametag8_CH592/user/misc/accel.c
+++ b/nametag8_CH592/user/misc/accel.c
@@ -19,6 +19,7 @@
 // user data
 AccelData accel;
 
+uint8_t accel_found = 0;
 int16_t movement;
 
 
@@ -27,6 +28,7 @@ static stmdev_ctx_t dev_ctx;
 
 
 
+
 int32_t accel_i2c_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len)
 {
     (void)(handle);
@@ -50,40 +52,47 @@ void accel_init()
 {
     uint8_t devid;
     uint8_t reset;
+    uint16_t timeout = 1000;
 
     dev_ctx.write_reg = accel_i2c_write;
     dev_ctx.read_reg = accel_i2c_read;
 
     // make sure we've got the right device
     lis2dw12_device_id_get(&dev_ctx, &devid);
-    if (devid != LIS2DW12_ID) {
-        while (1) {
-            // might as well crash the user's badge
-        }
+    if (devid == LIS2DW12_ID) {
+        accel_found = 1;
     }
 
     // reset accelerometer
-    lis2dw12_reset_set(&dev_ctx, PROPERTY_ENABLE);
-    do {
-        lis2dw12_reset_get(&dev_ctx, &reset);
-    } while (reset);
+    if (accel_found) {
+        lis2dw12_reset_set(&dev_ctx, PROPERTY_ENABLE);
+        do {
+            lis2dw12_reset_get(&dev_ctx, &reset);
+            if (!reset) {
+                timeout = 0;
+                accel_found = 1;
+            }
+        } while (timeout);
+    }
 
-    // disable block update
-    // data in output registers is updated immediately; FIFO is disabled
-    lis2dw12_block_data_update_set(&dev_ctx, PROPERTY_DISABLE);
+    if (accel_found) {
+        // disable block update
+        // data in output registers is updated immediately; FIFO is disabled
+        lis2dw12_block_data_update_set(&dev_ctx, PROPERTY_DISABLE);
 
-    // configure scale, power mode
-    lis2dw12_full_scale_set(&dev_ctx, LIS2DW12_2g);
-    lis2dw12_power_mode_set(&dev_ctx, LIS2DW12_CONT_LOW_PWR_LOW_NOISE_4);
+        // configure scale, power mode
+        lis2dw12_full_scale_set(&dev_ctx, LIS2DW12_2g);
+        lis2dw12_power_mode_set(&dev_ctx, LIS2DW12_CONT_LOW_PWR_LOW_NOISE_4);
 
-    // configure filter chain
-    // low pass filter enabled for 6D (not currently used)
-    lis2dw12_filter_path_set(&dev_ctx, LIS2DW12_LPF_ON_OUT);
-    // digital LPF2 filter of output data
-    lis2dw12_filter_bandwidth_set(&dev_ctx, LIS2DW12_ODR_DIV_4);
+        // configure filter chain
+        // low pass filter enabled for 6D (not currently used)
+        lis2dw12_filter_path_set(&dev_ctx, LIS2DW12_LPF_ON_OUT);
+        // digital LPF2 filter of output data
+        lis2dw12_filter_bandwidth_set(&dev_ctx, LIS2DW12_ODR_DIV_4);
 
-    // configure output data rate
-    lis2dw12_data_rate_set(&dev_ctx, LIS2DW12_XL_ODR_200Hz);
+        // configure output data rate
+        lis2dw12_data_rate_set(&dev_ctx, LIS2DW12_XL_ODR_200Hz);
+    }
 }
 
 void accel_poll()
@@ -91,6 +100,8 @@ void accel_poll()
     uint8_t reg = 1;
     uint16_t xyz[3];
 
+    if (!accel_found) return;
+
     while (reg) {
         // read output only if new value is available
         lis2dw12_flag_data_ready_get(&dev_ctx, &reg);
diff --git a/nametag8_CH592/user/misc/accel.h b/nametag8_CH592/user/misc/accel.h
index 26d727a..738db03 100644
--- a/nametag8_CH592/user/misc/accel.h
+++ b/nametag8_CH592/user/misc/accel.h
@@ -27,6 +27,8 @@ typedef struct AccelData {
 
 
 extern AccelData accel;
+
+extern uint8_t accel_found;
 extern uint16_t movement_worst;