From 89e9d9310d6b2483556665bdac9bbf0bb9f27c98 Mon Sep 17 00:00:00 2001 From: William Clark Date: Thu, 4 Jan 2024 20:20:51 +0000 Subject: [PATCH] accel readings now i16 (1lsb per 1mg) instead of float --- lis3dh.c | 37 +++++++++++++------------------------ lis3dh.h | 16 +++++++++------- main.c | 7 ++++--- 3 files changed, 26 insertions(+), 34 deletions(-) diff --git a/lis3dh.c b/lis3dh.c index 61bd1c0..6aa8ef4 100644 --- a/lis3dh.c +++ b/lis3dh.c @@ -259,50 +259,39 @@ static uint8_t acc_sensitivity(uint8_t mode, uint8_t range) { int lis3dh_read(lis3dh_t *lis3dh) { uint8_t data[6]; - int32_t x, y, z; - uint8_t scale, sens; + uint8_t shift, sens; int err = 0; - scale = acc_shift(lis3dh->cfg.mode); + shift = acc_shift(lis3dh->cfg.mode); sens = acc_sensitivity(lis3dh->cfg.mode, lis3dh->cfg.range); err |= lis3dh->dev.read(REG_OUT_X_L, data, 6); - x = (((int16_t)((data[0] << 8) | data[1])) >> scale) * sens; - y = (((int16_t)((data[2] << 8) | data[3])) >> scale) * sens; - z = (((int16_t)((data[4] << 8) | data[5])) >> scale) * sens; - - lis3dh->acc.x = ((float)x) / 1000.0; - lis3dh->acc.y = ((float)y) / 1000.0; - lis3dh->acc.z = ((float)z) / 1000.0; + lis3dh->acc.x = ((int16_t)(data[1] | data[0] << 8) >> shift) * sens; + lis3dh->acc.y = ((int16_t)(data[3] | data[2] << 8) >> shift) * sens; + lis3dh->acc.z = ((int16_t)(data[5] | data[4] << 8) >> shift) * sens; return err; } /* assume fifo has been configured and poll'd */ int lis3dh_read_fifo(lis3dh_t *lis3dh, struct lis3dh_fifo_data *fifo) { - int32_t x, y, z; - uint8_t scale, sens; uint8_t data[192]; /* max size */ + uint8_t sens; int err = 0; int i, idx; /* FIFO is always 10-bit / normal mode */ - scale = 6; sens = acc_sensitivity(LIS3DH_MODE_NORMAL, lis3dh->cfg.range); fifo->size = lis3dh->cfg.fifo.fth; - err |= lis3dh->dev.read(REG_OUT_X_L, data, 192); + err |= lis3dh->dev.read(REG_OUT_X_L, data, fifo->size * 6); for (i=0, idx=0; isize * 6; i+=6, idx++) { - x = (((int16_t)((data[i + 0] << 8) | data[i + 1])) >> scale) * sens; - y = (((int16_t)((data[i + 2] << 8) | data[i + 3])) >> scale) * sens; - z = (((int16_t)((data[i + 4] << 8) | data[i + 5])) >> scale) * sens; - - fifo->x[idx] = ((float)x) / 1000.0; - fifo->y[idx] = ((float)y) / 1000.0; - fifo->z[idx] = ((float)z) / 1000.0; + fifo->x[idx] = ((int16_t)(data[i+1] | data[i+0] << 8) >> 6) * sens; + fifo->y[idx] = ((int16_t)(data[i+3] | data[i+2] << 8) >> 6) * sens; + fifo->z[idx] = ((int16_t)(data[i+5] | data[i+4] << 8) >> 6) * sens; } return err; @@ -383,9 +372,9 @@ int lis3dh_read_adc(lis3dh_t *lis3dh) { shift = (lis3dh->cfg.mode == LIS3DH_MODE_LP) ? 8 : 6; - lis3dh->adc.adc1 = 1200 + (400 * ((data[1] | data[0] << 8) >> shift) >> (16 - shift)); - lis3dh->adc.adc2 = 1200 + (400 * ((data[3] | data[2] << 8) >> shift) >> (16 - shift)); - lis3dh->adc.adc3 = 1200 + (400 * ((data[5] | data[4] << 8) >> shift) >> (16 - shift)); + lis3dh->adc.adc1 = 1200 + (400 * ((int16_t)(data[1] | data[0] << 8) >> shift) >> (16 - shift)); + lis3dh->adc.adc2 = 1200 + (400 * ((int16_t)(data[3] | data[2] << 8) >> shift) >> (16 - shift)); + lis3dh->adc.adc3 = 1200 + (400 * ((int16_t)(data[5] | data[4] << 8) >> shift) >> (16 - shift)); return err; } diff --git a/lis3dh.h b/lis3dh.h index e38e1c6..d0d7758 100644 --- a/lis3dh.h +++ b/lis3dh.h @@ -245,11 +245,12 @@ struct lis3dh_adc { int16_t adc3; }; -/* accel data not read from FIFO is put here */ +/* accel data read not using FIFO */ +/* 1 lsb = 1 mg */ struct lis3dh_accel { - float x; - float y; - float z; + int16_t x; + int16_t y; + int16_t z; }; /* stores interrupt source registers read from the device */ @@ -270,11 +271,12 @@ struct lis3dh { typedef struct lis3dh lis3dh_t; /* struct for containing the FIFO data */ +/* 1 lsb = 1 mg */ struct lis3dh_fifo_data { uint8_t size; /* up to 32 */ - float x[32]; - float y[32]; - float z[32]; + int16_t x[32]; + int16_t y[32]; + int16_t z[32]; }; int lis3dh_init(lis3dh_t *lis3dh); diff --git a/main.c b/main.c index 1adfe92..dd12326 100644 --- a/main.c +++ b/main.c @@ -77,11 +77,12 @@ int main() { /* above function also writes out the qty of [x y z] sets stored in `fifo' */ for(k=0; k