diff --git a/Makefile b/Makefile index bc6143d..9b7a132 100644 --- a/Makefile +++ b/Makefile @@ -1,7 +1,7 @@ CC=gcc -CFLAGS=-O2 -std=gnu99 -W -Werror -Wall -Wextra -I. +CFLAGS=-O2 -std=gnu99 -W -Werror -Wall -Wextra -I. all: - $(CC) $(CFLAGS) main.c i2c.c lis3dh.c -o main + $(CC) $(CFLAGS) main.c i2c.c lis3dh.c -o lis3dh -lm clean: - rm -rf main \ No newline at end of file + rm -rf lis3dh \ No newline at end of file diff --git a/README.md b/README.md index 1175de7..14e4a26 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,14 @@ # LIS3DH 3-axis accelerometer + +``` +$ ./lis3dh +init OK +configure OK +poll OK +read OK +x: 0.552, y: -0.864, z: -0.152 +mag: 1.04 +deinit OK +``` \ No newline at end of file diff --git a/lis3dh.py b/doc/lis3dh.py similarity index 100% rename from lis3dh.py rename to doc/lis3dh.py diff --git a/i2c.c b/i2c.c index 7b97f58..33842a6 100644 --- a/i2c.c +++ b/i2c.c @@ -24,16 +24,16 @@ int i2c_init(void) { fd = open(I2C_DEVICE, O_RDWR); if (fd < 0) { fprintf(stderr, "could not open device: %s\n", I2C_DEVICE); - return I2C_ERR; + return 1; } if (ioctl(fd, I2C_SLAVE, I2C_LIS3DH_ADDRESS) < 0) { fprintf(stderr, "failed to acquire bus/talk to slave\n"); close(fd); - return I2C_ERR; + return 1; } - return I2C_OK; + return 0; } @@ -43,11 +43,11 @@ int i2c_read(uint8_t reg, uint8_t *dst, uint32_t size) { if (read(fd, dst, size) != (ssize_t)size) { fprintf(stderr, "error read()\n"); - return I2C_ERR; + return 1; } - return I2C_OK; + return 0; } @@ -56,10 +56,10 @@ int i2c_write(uint8_t reg, uint8_t value) { if (write(fd, cmd, 2) != 2) { fprintf(stderr, "error write()\n"); - return I2C_ERR; + return 1; } - return I2C_OK; + return 0; } int i2c_deinit(void) { @@ -67,5 +67,5 @@ int i2c_deinit(void) { close(fd); } - return I2C_OK; + return 0; } diff --git a/i2c.h b/i2c.h index 14f0680..c5f276f 100644 --- a/i2c.h +++ b/i2c.h @@ -3,12 +3,9 @@ #include -#define I2C_OK 0 -#define I2C_ERR 1 - -int i2c_init (void); -int i2c_read (uint8_t reg, uint8_t *dst, uint32_t size); -int i2c_write (uint8_t reg, uint8_t value); +int i2c_init (void); +int i2c_read (uint8_t reg, uint8_t *dst, uint32_t size); +int i2c_write (uint8_t reg, uint8_t value); int i2c_deinit (void); #endif \ No newline at end of file diff --git a/lis3dh.c b/lis3dh.c index ecbaaa7..4753f72 100644 --- a/lis3dh.c +++ b/lis3dh.c @@ -2,21 +2,160 @@ #include "lis3dh.h" #include "registers.h" +static int lis3dh_reboot(lis3dh_t *lis3dh) { + int err = 0; + + err |= lis3dh->dev.write(REG_CTRL_REG5, 0x80); + lis3dh->dev.sleep(5000); /* sleep 5 ms */ + + return err; +} + int lis3dh_init(lis3dh_t *lis3dh) { uint8_t result; int err = 0; - lis3dh->dev.init(); + if (lis3dh->dev.init() != 0) { + err |= 1; + } err |= lis3dh->dev.read(REG_WHO_AM_I, &result, 1); if (result != 0x33) { err |= 1; } + err |= lis3dh_reboot(lis3dh); + + return err; +} + +int lis3dh_configure(lis3dh_t *lis3dh) { + + uint8_t ctrl_reg1, ctrl_reg4; + int err = 0; + + /* last 0b111 enables Z, Y and X axis */ + ctrl_reg1 = 0 | (lis3dh->cfg.rate << 4) | 0b111; + ctrl_reg4 = 0 | (lis3dh->cfg.range << 4); + + /* set block update */ + ctrl_reg4 |= 0x80; + + /* set high resolution */ + if (lis3dh->cfg.mode == LIS3DH_MODE_HR) { + ctrl_reg4 |= 0x08; + } + + err |= lis3dh->dev.write(REG_CTRL_REG1, ctrl_reg1); + err |= lis3dh->dev.write(REG_CTRL_REG4, ctrl_reg4); + + return err; +} + +int lis3dh_poll(lis3dh_t *lis3dh) { + + uint8_t status; + int err = 0; + + do { + err |= lis3dh->dev.read(REG_STATUS_REG, &status, 1); + } while (!err && !((status >> 3) & 1)); + + return err; +} + +/* the real size of the int you get back from reading the acc u16 + depends on the power mode. + shift down the 16 bit word by this amount: */ +static uint8_t acc_shift(lis3dh_t *lis3dh) { + switch (lis3dh->cfg.mode) { + case LIS3DH_MODE_HR: + return 4; /* i12 */ + case LIS3DH_MODE_NORMAL: + return 6; /* i10 */ + case LIS3DH_MODE_LP: + return 8; /* i8 */ + } + + return 0; +} + +/* returns a scalar that when multiplied with axis reading + turns it to a multiple of mg. */ +static uint8_t acc_sensitivity(lis3dh_t *lis3dh) { + + uint8_t mode = lis3dh->cfg.mode; + + switch (lis3dh->cfg.range) { + case LIS3DH_FS_2G: + if (mode == LIS3DH_MODE_LP) { + return 16; + } else if (mode == LIS3DH_MODE_NORMAL) { + return 4; + } else { + return 1; + } + break; + + case LIS3DH_FS_4G: + if (mode == LIS3DH_MODE_LP) { + return 32; + } else if (mode == LIS3DH_MODE_NORMAL) { + return 8; + } else { + return 2; + } + break; + + case LIS3DH_FS_8G: + if (mode == LIS3DH_MODE_LP) { + return 64; + } else if (mode == LIS3DH_MODE_NORMAL) { + return 16; + } else { + return 4; + } + break; + + case LIS3DH_FS_16G: + if (mode == LIS3DH_MODE_LP) { + return 192; + } else if (mode == LIS3DH_MODE_NORMAL) { + return 148; + } else { + return 12; + } + break; + } + + return 0; +} + +int lis3dh_read(lis3dh_t *lis3dh) { + + uint8_t data[6]; + int16_t x, y, z; + uint8_t scale, sens; + int err = 0; + + scale = acc_shift(lis3dh); + sens = acc_sensitivity(lis3dh); + + /* must set MSbit of the address to multi-read and + have the device auto-increment the address. */ + err |= lis3dh->dev.read(REG_OUT_X_L | 0x80, 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 = (double)x / 1000.0; + lis3dh->acc.y = (double)y / 1000.0; + lis3dh->acc.z = (double)z / 1000.0; + return err; } int lis3dh_deinit(lis3dh_t *lis3dh) { - lis3dh->dev.deinit(); - return 0; + return lis3dh->dev.deinit(); } \ No newline at end of file diff --git a/lis3dh.h b/lis3dh.h index b5a0c99..6598664 100644 --- a/lis3dh.h +++ b/lis3dh.h @@ -16,10 +16,15 @@ #define LIS3DH_ODR_LP_5376_HZ 0b1001 /* range/sens */ -#define LIS3DH_FS_2G = 0b00 -#define LIS3DH_FS_4G = 0b01 -#define LIS3DH_FS_8G = 0b10 -#define LIS3DH_FS_16G = 0b11 +#define LIS3DH_FS_2G 0b00 +#define LIS3DH_FS_4G 0b01 +#define LIS3DH_FS_8G 0b10 +#define LIS3DH_FS_16G 0b11 + +/* modes */ +#define LIS3DH_MODE_HR 0b00 +#define LIS3DH_MODE_LP 0b01 +#define LIS3DH_MODE_NORMAL 0b10 struct lis3dh_device { @@ -33,18 +38,26 @@ struct lis3dh_device { struct lis3dh_config { uint8_t rate; /* ODR */ uint8_t range; /* FS */ - uint8_t mode; /* LPen */ - uint8_t hr; /* high resolution */ + uint8_t mode; /* LPen and HR */ +}; + +struct lis3dh_acceleration { + double x; + double y; + double z; }; struct lis3dh { struct lis3dh_device dev; struct lis3dh_config cfg; + struct lis3dh_acceleration acc; }; typedef struct lis3dh lis3dh_t; int lis3dh_init(lis3dh_t *lis3dh); int lis3dh_deinit(lis3dh_t *lis3dh); - +int lis3dh_configure(lis3dh_t *lis3dh); +int lis3dh_poll(lis3dh_t *lis3dh); +int lis3dh_read(lis3dh_t *lis3dh); #endif \ No newline at end of file diff --git a/main.c b/main.c index 86aa63c..0a7495f 100644 --- a/main.c +++ b/main.c @@ -1,9 +1,17 @@ #define _GNU_SOURCE #include #include +#include #include "lis3dh.h" #include "i2c.h" +double accel_mag(lis3dh_t *lis) { + double d = 0.0; + + d = sqrt( powf(lis->acc.x, 2) + powf(lis->acc.y, 2) + powf(lis->acc.z, 2)); + return d; +} + int main() { lis3dh_t lis; @@ -16,11 +24,30 @@ int main() { if (!lis3dh_init(&lis)) { - puts("OK"); + puts("init OK"); } + lis.cfg.mode = LIS3DH_MODE_NORMAL; + lis.cfg.range = LIS3DH_FS_2G; + lis.cfg.rate = LIS3DH_ODR_100_HZ; + + if (!lis3dh_configure(&lis)) { + puts("configure OK"); + } + + if (!lis3dh_poll(&lis)) { + puts("poll OK"); + } + + if (!lis3dh_read(&lis)) { + puts("read OK"); + } + + printf("x: %.3g, y: %.3g, z: %.3g\n", lis.acc.x, lis.acc.y, lis.acc.z); + printf("mag: %.3g\n", accel_mag(&lis)); + if (!lis3dh_deinit(&lis)) { - puts("OK"); + puts("deinit OK"); } return 0;