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, ®); 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;