Diff: mpu6050.cpp

Revision:

0:abd3a0fd55a9

Child:

1:6f743fe9a027

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mpu6050.cpp	Wed Dec 12 12:42:30 2018 +0000
@@ -0,0 +1,142 @@
+#include "mpu6050.h"
+
+DigitalOut led(LED3);
+
+static double RATE_ACC_LIST[4] = {
+    16384.0,
+    8192.0,
+    4096.0,
+    2048.0,
+};
+
+static double RATE_GYRO_LIST[4] = {
+    131.0,
+    65.5,
+    32.8,
+    16.4,
+};
+
+MPU6050::MPU6050(PinName i2c_sda, PinName i2c_scl) : i2c(i2c_sda, i2c_scl), address(208), rate_accelemeter(16384.0), rate_gyroscope(131.0)
+{
+    setMaxScale(MAX_ACC_2G, MAX_GYRO_250degpersec);
+}
+
+MPU6050::~MPU6050()
+{
+}
+
+char MPU6050::readByte(char reg)
+{
+    int result_write = i2c.write(address, &reg, 1);
+    if(result_write) {
+        // failed
+        //pc.printf("i2c error\r\n");
+        led = 1;
+    }
+    char read_data;
+    int result_read = i2c.read(address, &read_data, 1);
+    if(result_read) {
+        // failed
+        //pc.printf("i2c error\r\n");
+        led = 1;
+    }
+    return read_data;
+}
+
+void MPU6050::writeByte(char reg, char data)
+{
+    char buf[2];
+    buf[0] = reg;
+    buf[1] = data;
+    int result = i2c.write(address, buf, 2);
+    if(result) {
+        // failed
+        //pc.printf("i2c error\r\n");
+        led = 1;
+    }
+}
+
+void MPU6050::readAccelemeter(double &acc_x, double &acc_y, double &acc_z)
+{
+    char h, l;
+    short x, y, z;
+    // read X
+    h = readByte(0x3B);
+    l = readByte(0x3C);
+    x = h << 8 | l;
+    // read Y
+    h = readByte(0x3D);
+    l = readByte(0x3E);
+    y = h << 8 | l;
+    // read Z
+    h = readByte(0x3F);
+    l = readByte(0x40);
+    z = h << 8 | l;
+    acc_x = (double)x / rate_accelemeter;
+    acc_y = (double)y / rate_accelemeter;
+    acc_z = (double)z / rate_accelemeter;
+}
+
+void MPU6050::readGyroscope(double &gyro_x, double &gyro_y, double &gyro_z)
+{
+    char h, l;
+    short x, y, z;
+    // read X
+    h = readByte(0x43);
+    l = readByte(0x44);
+    x = h << 8 | l;
+    // read Y
+    h = readByte(0x45);
+    l = readByte(0x46);
+    y = h << 8 | l;
+    // read Z
+    h = readByte(0x47);
+    l = readByte(0x48);
+    z = h << 8 | l;
+    gyro_x = (double)x / rate_gyroscope;
+    gyro_y = (double)y / rate_gyroscope;
+    gyro_z = (double)z / rate_gyroscope;
+}
+
+void MPU6050::getMaxScale(int &acc, int &gyro)
+{
+    char reg;
+    reg = readByte(0x1C);
+    acc = reg;
+    reg = readByte(0x1B);
+    gyro = reg;
+}
+
+void MPU6050::setMaxScale(int max_acc, int max_gyro)
+{
+    switch(max_acc) {
+    case MAX_ACC_2G:
+        writeByte(0x1C, 0x00);
+        break;
+    case MAX_ACC_4G:
+        writeByte(0x1C, 0x08);
+        break;
+    case MAX_ACC_8G:
+        writeByte(0x1C, 0x10);
+        break;
+    case MAX_ACC_16G:
+        writeByte(0x1C, 0x18);
+        break;
+    }
+    rate_accelemeter = RATE_ACC_LIST[max_acc];
+    switch(max_gyro) {
+    case MAX_GYRO_250degpersec:
+        writeByte(0x1B, 0x00);
+        break;
+    case MAX_GYRO_500degpersec:
+        writeByte(0x1B, 0x08);
+        break;
+    case MAX_GYRO_1000degpersec:
+        writeByte(0x1B, 0x10);
+        break;
+    case MAX_GYRO_2000degpersec:
+        writeByte(0x1B, 0x18);
+        break;
+    }
+    rate_gyroscope = RATE_GYRO_LIST[max_gyro];
+}