accel readings now i16 (1lsb per 1mg) instead of float
This commit is contained in:
parent
bd76380a65
commit
89e9d9310d
37
lis3dh.c
37
lis3dh.c
@ -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
16
lis3dh.h
@ -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
7
main.c
@ -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);
|
||||||
|
Loading…
Reference in New Issue
Block a user