william/lis3dh
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

accel readings now i16 (1lsb per 1mg) instead of float

Browse Source
This commit is contained in:
William Clark 2024-01-04 20:20:51 +00:00
parent bd76380a65
commit 89e9d9310d
3 changed files with 26 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
lis3dh.c
View File

@ -259,50 +259,39 @@ static uint8_t acc_sensitivity(uint8_t mode, uint8_t range) {
int lis3dh_read(lis3dh_t *lis3dh) { int lis3dh_read(lis3dh_t *lis3dh) {
uint8_t data[6]; uint8_t data[6];
int32_t x, y, z; uint8_t shift, sens;
uint8_t scale, sens;
int err = 0; int err = 0;
scale = acc_shift(lis3dh->cfg.mode); shift = acc_shift(lis3dh->cfg.mode);
sens = acc_sensitivity(lis3dh->cfg.mode, lis3dh->cfg.range); sens = acc_sensitivity(lis3dh->cfg.mode, lis3dh->cfg.range);
err |= lis3dh->dev.read(REG_OUT_X_L, data, 6); err |= lis3dh->dev.read(REG_OUT_X_L, data, 6);
x = (((int16_t)((data[0] << 8) | data[1])) >> scale) * sens; lis3dh->acc.x = ((int16_t)(data[1] | data[0] << 8) >> shift) * sens;
y = (((int16_t)((data[2] << 8) | data[3])) >> scale) * sens; lis3dh->acc.y = ((int16_t)(data[3] | data[2] << 8) >> shift) * sens;
z = (((int16_t)((data[4] << 8) | data[5])) >> scale) * sens; lis3dh->acc.z = ((int16_t)(data[5] | data[4] << 8) >> shift) * sens;
lis3dh->acc.x = ((float)x) / 1000.0;
lis3dh->acc.y = ((float)y) / 1000.0;
lis3dh->acc.z = ((float)z) / 1000.0;
return err; return err;
} }
/* assume fifo has been configured and poll'd */ /* assume fifo has been configured and poll'd */
int lis3dh_read_fifo(lis3dh_t *lis3dh, struct lis3dh_fifo_data *fifo) { 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 data[192]; /* max size */
uint8_t sens;
int err = 0; int err = 0;
int i, idx; int i, idx;
/* FIFO is always 10-bit / normal mode */ /* FIFO is always 10-bit / normal mode */
scale = 6;
sens = acc_sensitivity(LIS3DH_MODE_NORMAL, lis3dh->cfg.range); sens = acc_sensitivity(LIS3DH_MODE_NORMAL, lis3dh->cfg.range);
fifo->size = lis3dh->cfg.fifo.fth; 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; i<fifo->size * 6; i+=6, idx++) { for (i=0, idx=0; i<fifo->size * 6; i+=6, idx++) {
x = (((int16_t)((data[i + 0] << 8) | data[i + 1])) >> scale) * sens; fifo->x[idx] = ((int16_t)(data[i+1] | data[i+0] << 8) >> 6) * sens;
y = (((int16_t)((data[i + 2] << 8) | data[i + 3])) >> scale) * sens; fifo->y[idx] = ((int16_t)(data[i+3] | data[i+2] << 8) >> 6) * sens;
z = (((int16_t)((data[i + 4] << 8) | data[i + 5])) >> scale) * sens; fifo->z[idx] = ((int16_t)(data[i+5] | data[i+4] << 8) >> 6) * sens;
fifo->x[idx] = ((float)x) / 1000.0;
fifo->y[idx] = ((float)y) / 1000.0;
fifo->z[idx] = ((float)z) / 1000.0;
} }
return err; return err;
@ -383,9 +372,9 @@ int lis3dh_read_adc(lis3dh_t *lis3dh) {
shift = (lis3dh->cfg.mode == LIS3DH_MODE_LP) ? 8 : 6; shift = (lis3dh->cfg.mode == LIS3DH_MODE_LP) ? 8 : 6;
lis3dh->adc.adc1 = 1200 + (400 * ((data[1] | data[0] << 8) >> shift) >> (16 - shift)); lis3dh->adc.adc1 = 1200 + (400 * ((int16_t)(data[1] | data[0] << 8) >> shift) >> (16 - shift));
lis3dh->adc.adc2 = 1200 + (400 * ((data[3] | data[2] << 8) >> shift) >> (16 - shift)); lis3dh->adc.adc2 = 1200 + (400 * ((int16_t)(data[3] | data[2] << 8) >> shift) >> (16 - shift));
lis3dh->adc.adc3 = 1200 + (400 * ((data[5] | data[4] << 8) >> shift) >> (16 - shift)); lis3dh->adc.adc3 = 1200 + (400 * ((int16_t)(data[5] | data[4] << 8) >> shift) >> (16 - shift));
return err; return err;
} }

16
lis3dh.h
View File

@ -245,11 +245,12 @@ struct lis3dh_adc {
int16_t adc3; 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 { struct lis3dh_accel {
float x; int16_t x;
float y; int16_t y;
float z; int16_t z;
}; };
/* stores interrupt source registers read from the device */ /* stores interrupt source registers read from the device */
@ -270,11 +271,12 @@ struct lis3dh {
typedef struct lis3dh lis3dh_t; typedef struct lis3dh lis3dh_t;
/* struct for containing the FIFO data */ /* struct for containing the FIFO data */
/* 1 lsb = 1 mg */
struct lis3dh_fifo_data { struct lis3dh_fifo_data {
uint8_t size; /* up to 32 */ uint8_t size; /* up to 32 */
float x[32]; int16_t x[32];
float y[32]; int16_t y[32];
float z[32]; int16_t z[32];
}; };
int lis3dh_init(lis3dh_t *lis3dh); int lis3dh_init(lis3dh_t *lis3dh);

7
main.c
View File

@ -77,11 +77,12 @@ int main() {
/* above function also writes out the qty of [x y z] sets stored in `fifo' */ /* above function also writes out the qty of [x y z] sets stored in `fifo' */
for(k=0; k<fifo.size; k++) { for(k=0; k<fifo.size; k++) {
printf("x: %04.04f, y: %04.04f z: %04.04f mag: %04.04f\n", printf("x: %d, y: %d z: %d\n",
fifo.x[k], fifo.y[k], fifo.z[k], fifo.x[k], fifo.y[k], fifo.z[k]);
mag(fifo.x[k], fifo.y[k], fifo.z[k]));
} }
printf("mag: %04.04f\n", mag(100.0, 45.0, 120.0));
if (lis3dh_read_adc(&lis)) { if (lis3dh_read_adc(&lis)) {
quit("adc()", &lis); quit("adc()", &lis);