Diff: mpu6050.cpp

Revision:

0:529602524696

diff -r 000000000000 -r 529602524696 mpu6050.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mpu6050.cpp	Thu Aug 17 23:31:15 2017 +0000
@@ -0,0 +1,163 @@
+#include "mbed.h"
+#include "mpu6050.h"
+
+MPU6050::MPU6050() : connection(PB_9, PB_8) { // SDA(D14), SCL(D15)   // pinnames.h
+    dev_addr = MPU6050_DEFAULT_ADDRESS;
+}
+
+MPU6050::MPU6050(uint8_t address) : connection(PB_9, PB_8) {
+    dev_addr = address;
+}
+
+MPU6050::MPU6050(uint8_t address, PinName sda, PinName scl) : connection(sda, scl) {
+    dev_addr = address;
+}
+
+void MPU6050::initialize() {
+    setClockSource(MPU6050_CLOCK_PLL_XGYRO);
+    setFullScaleGyroRange(MPU6050_GYRO_FS_250);
+    setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
+    setSleepEnable(false);
+}
+
+bool MPU6050::testConnection() {
+    return (getDeviceID() == 0x68);
+}
+
+void MPU6050::write(char address, char data) {
+    char temp[2];
+    temp[0] = address;
+    temp[1] = data;
+    connection.write((dev_addr << 1), temp, 2);
+}
+
+void MPU6050::writeWord(char address, int16_t data) {
+    char temp[3];
+    temp[0] = address;
+    temp[1] = (char)((data >> 8) & 0x00ff);
+    temp[2] = (char)(data & 0x00ff);
+    connection.write((dev_addr << 1), temp, 3);
+}
+
+char MPU6050::read(char address) {
+    char retval;
+    connection.write((dev_addr << 1), &address, true);
+    connection.read((dev_addr << 1), &retval, 1);
+    return retval;
+}
+
+void MPU6050::read(char address, char* data, int length) {
+    connection.write((dev_addr << 1), &address, true);
+    connection.read((dev_addr << 1), data, length);
+}
+
+///////////////////////////////////////////////////////////
+// CLKSEL[2:0] = (0x6B)PWR_MGMT_1[2:0]
+///////////////////////////////////////////////////////////
+void MPU6050::setClockSource(uint8_t source) {
+    char read_val, write_val;
+    
+    read_val = this->read(MPU6050_PWR_MGMT_1_REG);
+    write_val = (read_val & 0xF8) | (source & 0x07);
+    
+    this->write(MPU6050_PWR_MGMT_1_REG, write_val);
+}
+
+///////////////////////////////////////////////////////////
+// SLEEP = (0x6B)PWR_MGMT_1[6]
+///////////////////////////////////////////////////////////
+void MPU6050::setSleepEnable(bool enabled) {
+    char read_val, write_val;
+    
+    read_val = this->read(MPU6050_PWR_MGMT_1_REG);
+    if (enabled)
+        write_val = read_val | 0x40;
+    else
+        write_val = read_val & 0xBF;
+    
+    this->write(MPU6050_PWR_MGMT_1_REG, write_val);
+}
+
+///////////////////////////////////////////////////////////
+// WHO_AM_I[5:0] = (0x75)WHO_AM_I[6:1]
+///////////////////////////////////////////////////////////
+uint8_t MPU6050::getDeviceID() {
+    return (this->read(MPU6050_WHO_AM_I_REG));
+}
+
+///////////////////////////////////////////////////////////
+// FS_SEL[1:0] = (0x1B)GYRO_CONFIG[4:3]
+///////////////////////////////////////////////////////////
+void MPU6050::setFullScaleGyroRange(int8_t range) {
+    char read_val, write_val;
+    
+    read_val = this->read(MPU6050_GYRO_CONFIG_REG);
+    write_val = (read_val & 0xE7) | ((range << 3) & 0x18);
+    
+    this->write(MPU6050_GYRO_CONFIG_REG, write_val);
+}
+
+///////////////////////////////////////////////////////////
+// AFS_SEL[1:0] = (0x1C)ACCEL_CONFIG[4:3]
+///////////////////////////////////////////////////////////
+void MPU6050::setFullScaleAccelRange(int8_t range) {
+    char read_val, write_val;
+    
+    read_val = this->read(MPU6050_ACCEL_CONFIG_REG);
+    write_val = (read_val & 0xE7) | ((range << 3) & 0x18);
+    
+    this->write(MPU6050_ACCEL_CONFIG_REG, write_val);
+}
+
+//////////////////////////////////////////////////////////////////////
+// XA_OFFSET[14:0] = (0x06)XA_OFFSET_H[7:0] + (0x07)XA_OFFSET_L[7:1]
+//////////////////////////////////////////////////////////////////////
+void MPU6050::setXAccelOffset(int16_t offset) {
+    this->writeWord(MPU6050_XA_OFFS_H_REG, offset);
+}
+
+//////////////////////////////////////////////////////////////////////
+// YA_OFFSET[14:0] = (0x08)YA_OFFSET_H[7:0] + (0x09)YA_OFFSET_L[7:1]
+//////////////////////////////////////////////////////////////////////
+void MPU6050::setYAccelOffset(int16_t offset) {
+    this->writeWord(MPU6050_YA_OFFS_H_REG, offset);
+}
+
+//////////////////////////////////////////////////////////////////////
+// ZA_OFFSET[14:0] = (0x0A)ZA_OFFSET_H[7:0] + (0x0B)ZA_OFFSET_L[7:1]
+//////////////////////////////////////////////////////////////////////
+void MPU6050::setZAccelOffset(int16_t offset) {
+    this->writeWord(MPU6050_ZA_OFFS_H_REG, offset);
+}
+
+//////////////////////////////////////////////////////////////////////
+// XG_OFFSET[15:0] = (0x13)XG_OFFSET_H[7:0] + (0x14)XG_OFFSET_L[7:0]
+//////////////////////////////////////////////////////////////////////
+void MPU6050::setXGyroOffset(int16_t offset) {
+    this->writeWord(MPU6050_XG_OFFS_USRH_REG, offset);
+}
+
+//////////////////////////////////////////////////////////////////////
+// YG_OFFSET[15:0] = (0x15)YG_OFFSET_H[7:0] + (0x16)YG_OFFSET_L[7:0]
+//////////////////////////////////////////////////////////////////////
+void MPU6050::setYGyroOffset(int16_t offset) {
+    this->writeWord(MPU6050_YG_OFFS_USRH_REG, offset);
+}
+
+//////////////////////////////////////////////////////////////////////
+// ZG_OFFSET[15:0] = (0x17)ZG_OFFSET_H[7:0] + (0x18)ZG_OFFSET_L[7:0]
+//////////////////////////////////////////////////////////////////////
+void MPU6050::setZGyroOffset(int16_t offset) {
+    this->writeWord(MPU6050_ZG_OFFS_USRH_REG, offset);
+}
+
+void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
+    char buffer[14];
+    this->read(MPU6050_ACCEL_XOUT_H_REG, buffer, 14);
+    *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
+    *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
+    *az = (((int16_t)buffer[4]) << 8) | buffer[5];
+    *gx = (((int16_t)buffer[8]) << 8) | buffer[9];
+    *gy = (((int16_t)buffer[10]) << 8) | buffer[11];
+    *gz = (((int16_t)buffer[12]) << 8) | buffer[13];
+}
\ No newline at end of file