mpu6050.cpp

00001 #include "mbed.h"
00002 #include "mpu6050.h"
00003 
00004 MPU6050::MPU6050() : connection(PB_9, PB_8) { // SDA(D14), SCL(D15)   // pinnames.h
00005     dev_addr = MPU6050_DEFAULT_ADDRESS;
00006 }
00007 
00008 MPU6050::MPU6050(uint8_t address) : connection(PB_9, PB_8) {
00009     dev_addr = address;
00010 }
00011 
00012 MPU6050::MPU6050(uint8_t address, PinName sda, PinName scl) : connection(sda, scl) {
00013     dev_addr = address;
00014 }
00015 
00016 void MPU6050::initialize() {
00017     setClockSource(MPU6050_CLOCK_PLL_XGYRO);
00018     setFullScaleGyroRange(MPU6050_GYRO_FS_250);
00019     setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
00020     setSleepEnable(false);
00021 }
00022 
00023 bool MPU6050::testConnection() {
00024     return (getDeviceID() == 0x68);
00025 }
00026 
00027 void MPU6050::write(char address, char data) {
00028     char temp[2];
00029     temp[0] = address;
00030     temp[1] = data;
00031     connection.write((dev_addr << 1), temp, 2);
00032 }
00033 
00034 void MPU6050::writeWord(char address, int16_t data) {
00035     char temp[3];
00036     temp[0] = address;
00037     temp[1] = (char)((data >> 8) & 0x00ff);
00038     temp[2] = (char)(data & 0x00ff);
00039     connection.write((dev_addr << 1), temp, 3);
00040 }
00041 
00042 char MPU6050::read(char address) {
00043     char retval;
00044     connection.write((dev_addr << 1), &address, true);
00045     connection.read((dev_addr << 1), &retval, 1);
00046     return retval;
00047 }
00048 
00049 void MPU6050::read(char address, char* data, int length) {
00050     connection.write((dev_addr << 1), &address, true);
00051     connection.read((dev_addr << 1), data, length);
00052 }
00053 
00054 ///////////////////////////////////////////////////////////
00055 // CLKSEL[2:0] = (0x6B)PWR_MGMT_1[2:0]
00056 ///////////////////////////////////////////////////////////
00057 void MPU6050::setClockSource(uint8_t source) {
00058     char read_val, write_val;
00059     
00060     read_val = this->read(MPU6050_PWR_MGMT_1_REG);
00061     write_val = (read_val & 0xF8) | (source & 0x07);
00062     
00063     this->write(MPU6050_PWR_MGMT_1_REG, write_val);
00064 }
00065 
00066 ///////////////////////////////////////////////////////////
00067 // SLEEP = (0x6B)PWR_MGMT_1[6]
00068 ///////////////////////////////////////////////////////////
00069 void MPU6050::setSleepEnable(bool enabled) {
00070     char read_val, write_val;
00071     
00072     read_val = this->read(MPU6050_PWR_MGMT_1_REG);
00073     if (enabled)
00074         write_val = read_val | 0x40;
00075     else
00076         write_val = read_val & 0xBF;
00077     
00078     this->write(MPU6050_PWR_MGMT_1_REG, write_val);
00079 }
00080 
00081 ///////////////////////////////////////////////////////////
00082 // WHO_AM_I[5:0] = (0x75)WHO_AM_I[6:1]
00083 ///////////////////////////////////////////////////////////
00084 uint8_t MPU6050::getDeviceID() {
00085     return (this->read(MPU6050_WHO_AM_I_REG));
00086 }
00087 
00088 ///////////////////////////////////////////////////////////
00089 // FS_SEL[1:0] = (0x1B)GYRO_CONFIG[4:3]
00090 ///////////////////////////////////////////////////////////
00091 void MPU6050::setFullScaleGyroRange(int8_t range) {
00092     char read_val, write_val;
00093     
00094     read_val = this->read(MPU6050_GYRO_CONFIG_REG);
00095     write_val = (read_val & 0xE7) | ((range << 3) & 0x18);
00096     
00097     this->write(MPU6050_GYRO_CONFIG_REG, write_val);
00098 }
00099 
00100 ///////////////////////////////////////////////////////////
00101 // AFS_SEL[1:0] = (0x1C)ACCEL_CONFIG[4:3]
00102 ///////////////////////////////////////////////////////////
00103 void MPU6050::setFullScaleAccelRange(int8_t range) {
00104     char read_val, write_val;
00105     
00106     read_val = this->read(MPU6050_ACCEL_CONFIG_REG);
00107     write_val = (read_val & 0xE7) | ((range << 3) & 0x18);
00108     
00109     this->write(MPU6050_ACCEL_CONFIG_REG, write_val);
00110 }
00111 
00112 //////////////////////////////////////////////////////////////////////
00113 // XA_OFFSET[14:0] = (0x06)XA_OFFSET_H[7:0] + (0x07)XA_OFFSET_L[7:1]
00114 //////////////////////////////////////////////////////////////////////
00115 void MPU6050::setXAccelOffset(int16_t offset) {
00116     this->writeWord(MPU6050_XA_OFFS_H_REG, offset);
00117 }
00118 
00119 //////////////////////////////////////////////////////////////////////
00120 // YA_OFFSET[14:0] = (0x08)YA_OFFSET_H[7:0] + (0x09)YA_OFFSET_L[7:1]
00121 //////////////////////////////////////////////////////////////////////
00122 void MPU6050::setYAccelOffset(int16_t offset) {
00123     this->writeWord(MPU6050_YA_OFFS_H_REG, offset);
00124 }
00125 
00126 //////////////////////////////////////////////////////////////////////
00127 // ZA_OFFSET[14:0] = (0x0A)ZA_OFFSET_H[7:0] + (0x0B)ZA_OFFSET_L[7:1]
00128 //////////////////////////////////////////////////////////////////////
00129 void MPU6050::setZAccelOffset(int16_t offset) {
00130     this->writeWord(MPU6050_ZA_OFFS_H_REG, offset);
00131 }
00132 
00133 //////////////////////////////////////////////////////////////////////
00134 // XG_OFFSET[15:0] = (0x13)XG_OFFSET_H[7:0] + (0x14)XG_OFFSET_L[7:0]
00135 //////////////////////////////////////////////////////////////////////
00136 void MPU6050::setXGyroOffset(int16_t offset) {
00137     this->writeWord(MPU6050_XG_OFFS_USRH_REG, offset);
00138 }
00139 
00140 //////////////////////////////////////////////////////////////////////
00141 // YG_OFFSET[15:0] = (0x15)YG_OFFSET_H[7:0] + (0x16)YG_OFFSET_L[7:0]
00142 //////////////////////////////////////////////////////////////////////
00143 void MPU6050::setYGyroOffset(int16_t offset) {
00144     this->writeWord(MPU6050_YG_OFFS_USRH_REG, offset);
00145 }
00146 
00147 //////////////////////////////////////////////////////////////////////
00148 // ZG_OFFSET[15:0] = (0x17)ZG_OFFSET_H[7:0] + (0x18)ZG_OFFSET_L[7:0]
00149 //////////////////////////////////////////////////////////////////////
00150 void MPU6050::setZGyroOffset(int16_t offset) {
00151     this->writeWord(MPU6050_ZG_OFFS_USRH_REG, offset);
00152 }
00153 
00154 void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
00155     char buffer[14];
00156     this->read(MPU6050_ACCEL_XOUT_H_REG, buffer, 14);
00157     *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
00158     *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
00159     *az = (((int16_t)buffer[4]) << 8) | buffer[5];
00160     *gx = (((int16_t)buffer[8]) << 8) | buffer[9];
00161     *gy = (((int16_t)buffer[10]) << 8) | buffer[11];
00162     *gz = (((int16_t)buffer[12]) << 8) | buffer[13];
00163 }