reading acc/gyro

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
 

i2c.c

@@ -0,0 +1,82 @@
+/* 
+
+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"
+
+/*
+ * 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, &reg, 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;
+}

i2c.h

@@ -1,5 +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

main.c

@@ -1,8 +1,121 @@
+#define _GNU_SOURCE
+#include <unistd.h>
 #include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
 #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;
 }

mpu6050.c

@@ -0,0 +1,2 @@
+#include "mpu6050.h"
+#include "registers.h"

mpu6050.h

@@ -1,5 +1,40 @@
 #ifndef MPU6050_H
 #define MPU6050_H
 
+#include <stdint.h>
+
+/* 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