diff --git a/Makefile b/Makefile index 27d9753..9920208 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,5 @@ CC=gcc -CFLAG=-O0 -std=c89 -I. +CFLAG=-O0 -std=c89 -Wall -Wextra -W -pedantic -I. CFILES=$(wildcard *.c) BIN=mpu6050 diff --git a/i2c.c b/i2c.c index e69de29..676c9b8 100644 --- a/i2c.c +++ b/i2c.c @@ -0,0 +1,82 @@ +/* + +Example I2C use on linux/raspberry pi + +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "i2c.h" + +/* + * Pinout config for this example + * + * MPU6050 SDA => Raspberry Pi GPIO 2 (Physical pin 3) + * MPU6050 SCL => Raspberry Pi GPIO 3 (Physical pin 5) + * + */ + +#define I2C_DEVICE "/dev/i2c-1" +#define I2C_MPU6050_ADDRESS 0x68 + +static int fd; + +int i2c_init(void) { + fd = open(I2C_DEVICE, O_RDWR); + if (fd < 0) { + fprintf(stderr, "i2c_init(): could not open device: %s\n", I2C_DEVICE); + return 1; + } + + if (ioctl(fd, I2C_SLAVE, I2C_MPU6050_ADDRESS) < 0) { + fprintf(stderr, "i2c_init(): 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) { + + if (write(fd, ®, 1) != 1) { + fprintf(stderr, "i2c_read(): error write()\n"); + return 1; + } + + if (read(fd, dst, size) != (int)size) { + fprintf(stderr, "i2c_read(): error read()\n"); + return 1; + } + + return 0; +} + +int i2c_write(uint8_t reg, uint8_t value) { + uint8_t cmd[2]; + + cmd[0] = reg; + cmd[1] = value; + + if (write(fd, cmd, 2) != 2) { + fprintf(stderr, "i2c_write(): error write()\n"); + return 1; + } + + return 0; +} + +int i2c_deinit(void) { + if (fd) { + close(fd); + } + + return 0; +} \ No newline at end of file diff --git a/i2c.h b/i2c.h index db70280..c5f276f 100644 --- a/i2c.h +++ b/i2c.h @@ -1,5 +1,11 @@ #ifndef I2C_H #define I2C_H +#include + +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/main.c b/main.c index 137ec0a..d4e7714 100644 --- a/main.c +++ b/main.c @@ -1,8 +1,121 @@ +#define _GNU_SOURCE +#include #include +#include +#include #include "mpu6050.h" +#include "registers.h" #include "i2c.h" int main() { - printf("Hello\n"); + + uint8_t id; + uint8_t gyro, acc; + uint8_t int_status = 0; + uint8_t data[6]; + int16_t gx, gy, gz; + int16_t ax, ay, az; + float lsb_conv_gyro; + float lsb_conv_acc; + int err = 0; + + if (i2c_init()) { + exit(1); + } + + if (i2c_read(REG_WHO_AM_I, &id, 1)) { + exit(1); + } + + if (id == 0x68) { + puts("Found MPU-6050"); + } + + /* reset all signal paths */ + /* enable gyro, acc and temp */ + if (i2c_write(REG_SIGNAL_PATH_RESET, 0x07)) { + exit(1); + } + + /* set sample rate divisor to 150 */ + /* if LP-filter = 0 or 7 => sample rate = 8 Khz, otherwise 1 Khz */ + if (i2c_write(REG_SMPLRT_DIV, 150)) { + exit(1); + } + + /* set up digital LP-filter */ + if (i2c_write(REG_CONFIG, 0x05)) { + exit(1); + } + + /* set sleep to 0 */ + if (i2c_write(REG_PWR_MGMT1, 0x00)) { + exit(1); + } + + /* enable DATA_RDY interrupt (polling for now) */ + /* active when internal writes to all out data regs are done */ + if (i2c_write(REG_INT_ENABLE, 0x01)) { + exit(1); + } + + gyro = MPU6050_GYRO_FS_250 << 3; + lsb_conv_gyro = 131.f; + if (i2c_write(REG_GYRO_CONFIG, gyro)) { + exit(1); + } + + acc = MPU6050_ACC_FS_2G << 3; + lsb_conv_acc = 16384.f; + if (i2c_write(REG_ACCEL_CONFIG, acc)) { + exit(1); + } + + usleep(200 * 1000); + +LOOP: + do { + err |= i2c_read(REG_INT_STATUS, &int_status, 1); + usleep(1000); + } while(!err && !(int_status & 1)); + + if (i2c_read(REG_GYRO_XOUT_H, data, 6)) { + exit(1); + } + + gx = (data[0] << 8 | data[1]); + gy = (data[2] << 8 | data[3]); + gz = (data[4] << 8 | data[5]); + + printf("[GYRO °/s] x: % -3.4f y: %-3.4f z: %-3.4f ", + (float)(gx)/lsb_conv_gyro, + (float)(gy)/lsb_conv_gyro, + (float)(gz)/lsb_conv_gyro); + + err = 0; + int_status = 0; + + do { + err |= i2c_read(REG_INT_STATUS, &int_status, 1); + usleep(1000); + } while(!err && !(int_status & 1)); + + if (i2c_read(REG_ACCEL_XOUT_H, data, 6)) { + exit(1); + } + + ax = (data[0] << 8 | data[1]); + ay = (data[2] << 8 | data[3]); + az = (data[4] << 8 | data[5]); + + printf("[ACCEL g] x: % -3.4f y: %-3.4f z: %-3.4f\n", + (float)(ax)/lsb_conv_acc, + (float)(ay)/lsb_conv_acc, + (float)(az)/lsb_conv_acc); + +goto LOOP; + + i2c_deinit(); + return 0; } \ No newline at end of file diff --git a/mpu6050.c b/mpu6050.c index e69de29..e860762 100644 --- a/mpu6050.c +++ b/mpu6050.c @@ -0,0 +1,2 @@ +#include "mpu6050.h" +#include "registers.h" \ No newline at end of file diff --git a/mpu6050.h b/mpu6050.h index 96b3619..ad36432 100644 --- a/mpu6050.h +++ b/mpu6050.h @@ -1,5 +1,40 @@ #ifndef MPU6050_H #define MPU6050_H +#include + +/* Gyroscope full-scale range */ +#define MPU6050_GYRO_FS_250 0x00 /* ± 250 °/s */ +#define MPU6050_GYRO_FS_500 0x01 /* ± 500 °/s */ +#define MPU6050_GYRO_FS_1000 0x02 /* ± 1000 °/s */ +#define MPU6050_GYRO_FS_2000 0x03 /* ± 2000 °/s */ + +/* Accelerometer full-scale range */ +#define MPU6050_ACC_FS_2G 0x00 /* ± 2g */ +#define MPU6050_ACC_FS_4G 0x01 /* ± 4g */ +#define MPU6050_ACC_FS_8G 0x02 /* ± 8g */ +#define MPU6050_ACC_FS_16G 0x03 /* ± 16g */ + + +struct mpu6050_acc_config { + uint8_t range; + uint8_t filter; +}; + +struct mpu6050_gyro_config { + uint8_t range; + uint8_t filter; +}; + +struct mpu6050_config { + struct mpu6050_gyro_config gyro; + struct mpu6050_acc_config acc; +}; + +struct mpu6050 { + struct mpu6050_config cfg; +}; + +typedef struct mpu6050 mpu6050_t; #endif \ No newline at end of file