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 <stdint.h>
-#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 <stdio.h>
#include <unistd.h>
+#include <math.h>
#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;