Compare commits
2 Commits
1726159cb8
...
509bae290b
Author | SHA1 | Date | |
---|---|---|---|
509bae290b | |||
f3c07a5c8b |
5
Makefile
5
Makefile
@ -1,4 +1,7 @@
|
|||||||
CC=gcc
|
CC=gcc
|
||||||
CFLAGS=-O2 -std=gnu99 -W -Werror -Wall -Wextra -I.
|
CFLAGS=-O2 -std=gnu99 -W -Werror -Wall -Wextra -I.
|
||||||
all:
|
all:
|
||||||
$(CC) $(CFLAGS) main.c lis3dh.c -o main
|
$(CC) $(CFLAGS) main.c i2c.c lis3dh.c -o lis3dh -lm
|
||||||
|
|
||||||
|
clean:
|
||||||
|
rm -rf lis3dh
|
11
README.md
11
README.md
@ -1,3 +1,14 @@
|
|||||||
# LIS3DH
|
# LIS3DH
|
||||||
|
|
||||||
3-axis accelerometer
|
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
|
||||||
|
```
|
71
i2c.c
Normal file
71
i2c.c
Normal file
@ -0,0 +1,71 @@
|
|||||||
|
/*
|
||||||
|
|
||||||
|
Example I2C use on linux/raspberry pi
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <linux/i2c.h>
|
||||||
|
#include <linux/i2c-dev.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
|
||||||
|
#include "i2c.h"
|
||||||
|
|
||||||
|
#define I2C_DEVICE "/dev/i2c-1"
|
||||||
|
#define I2C_LIS3DH_ADDRESS 0x18
|
||||||
|
|
||||||
|
static int fd;
|
||||||
|
|
||||||
|
int i2c_init(void) {
|
||||||
|
fd = open(I2C_DEVICE, O_RDWR);
|
||||||
|
if (fd < 0) {
|
||||||
|
fprintf(stderr, "could not open device: %s\n", I2C_DEVICE);
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ioctl(fd, I2C_SLAVE, I2C_LIS3DH_ADDRESS) < 0) {
|
||||||
|
fprintf(stderr, "failed to acquire bus/talk to slave\n");
|
||||||
|
close(fd);
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int i2c_read(uint8_t reg, uint8_t *dst, uint32_t size) {
|
||||||
|
uint8_t cmd[2] = {reg, 0x00};
|
||||||
|
write(fd, cmd, 2);
|
||||||
|
|
||||||
|
if (read(fd, dst, size) != (ssize_t)size) {
|
||||||
|
fprintf(stderr, "error read()\n");
|
||||||
|
return 1;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int i2c_write(uint8_t reg, uint8_t value) {
|
||||||
|
uint8_t cmd[2] = {reg, value};
|
||||||
|
|
||||||
|
if (write(fd, cmd, 2) != 2) {
|
||||||
|
fprintf(stderr, "error write()\n");
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int i2c_deinit(void) {
|
||||||
|
if (fd) {
|
||||||
|
close(fd);
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
11
i2c.h
Normal file
11
i2c.h
Normal file
@ -0,0 +1,11 @@
|
|||||||
|
#ifndef I2C_H
|
||||||
|
#define I2C_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
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
|
156
lis3dh.c
156
lis3dh.c
@ -1,11 +1,161 @@
|
|||||||
|
#include <stdio.h>
|
||||||
#include "lis3dh.h"
|
#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) {
|
int lis3dh_init(lis3dh_t *lis3dh) {
|
||||||
lis3dh->cfg.rate = 0;
|
uint8_t result;
|
||||||
|
int err = 0;
|
||||||
|
|
||||||
|
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;
|
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) {
|
int lis3dh_deinit(lis3dh_t *lis3dh) {
|
||||||
lis3dh->cfg.rate = 0;
|
return lis3dh->dev.deinit();
|
||||||
return 0;
|
|
||||||
}
|
}
|
27
lis3dh.h
27
lis3dh.h
@ -16,10 +16,15 @@
|
|||||||
#define LIS3DH_ODR_LP_5376_HZ 0b1001
|
#define LIS3DH_ODR_LP_5376_HZ 0b1001
|
||||||
|
|
||||||
/* range/sens */
|
/* range/sens */
|
||||||
#define LIS3DH_FS_2G = 0b00
|
#define LIS3DH_FS_2G 0b00
|
||||||
#define LIS3DH_FS_4G = 0b01
|
#define LIS3DH_FS_4G 0b01
|
||||||
#define LIS3DH_FS_8G = 0b10
|
#define LIS3DH_FS_8G 0b10
|
||||||
#define LIS3DH_FS_16G = 0b11
|
#define LIS3DH_FS_16G 0b11
|
||||||
|
|
||||||
|
/* modes */
|
||||||
|
#define LIS3DH_MODE_HR 0b00
|
||||||
|
#define LIS3DH_MODE_LP 0b01
|
||||||
|
#define LIS3DH_MODE_NORMAL 0b10
|
||||||
|
|
||||||
|
|
||||||
struct lis3dh_device {
|
struct lis3dh_device {
|
||||||
@ -33,18 +38,26 @@ struct lis3dh_device {
|
|||||||
struct lis3dh_config {
|
struct lis3dh_config {
|
||||||
uint8_t rate; /* ODR */
|
uint8_t rate; /* ODR */
|
||||||
uint8_t range; /* FS */
|
uint8_t range; /* FS */
|
||||||
uint8_t mode; /* LPen */
|
uint8_t mode; /* LPen and HR */
|
||||||
uint8_t hr; /* high resolution */
|
};
|
||||||
|
|
||||||
|
struct lis3dh_acceleration {
|
||||||
|
double x;
|
||||||
|
double y;
|
||||||
|
double z;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct lis3dh {
|
struct lis3dh {
|
||||||
struct lis3dh_device dev;
|
struct lis3dh_device dev;
|
||||||
struct lis3dh_config cfg;
|
struct lis3dh_config cfg;
|
||||||
|
struct lis3dh_acceleration acc;
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef struct lis3dh lis3dh_t;
|
typedef struct lis3dh lis3dh_t;
|
||||||
|
|
||||||
int lis3dh_init(lis3dh_t *lis3dh);
|
int lis3dh_init(lis3dh_t *lis3dh);
|
||||||
int lis3dh_deinit(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
|
#endif
|
41
main.c
41
main.c
@ -1,16 +1,53 @@
|
|||||||
|
#define _GNU_SOURCE
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <math.h>
|
||||||
#include "lis3dh.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() {
|
int main() {
|
||||||
|
|
||||||
lis3dh_t lis;
|
lis3dh_t lis;
|
||||||
|
|
||||||
|
lis.dev.init = i2c_init;
|
||||||
|
lis.dev.read = i2c_read;
|
||||||
|
lis.dev.write = i2c_write;
|
||||||
|
lis.dev.sleep = usleep;
|
||||||
|
lis.dev.deinit = i2c_deinit;
|
||||||
|
|
||||||
|
|
||||||
if (!lis3dh_init(&lis)) {
|
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)) {
|
if (!lis3dh_deinit(&lis)) {
|
||||||
puts("OK");
|
puts("deinit OK");
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
|
Loading…
Reference in New Issue
Block a user