MPU6050 simple library
Dependents: MPU6050_test Drone MPU6050_test acelerometro ... more
Diff: MPU6050.cpp
- Revision:
- 0:6d1b9bcd64ca
- Child:
- 1:cf3a9ec7205e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.cpp Mon Sep 19 05:09:07 2016 +0000 @@ -0,0 +1,98 @@ +#include "mbed.h" +#include "MPU6050.h" + +#define ADDRESS 0xD1 + +MPU6050::MPU6050(PinName sda, PinName scl): + _MPU6050(sda, scl) +{ + _MPU6050.frequency(400000); +} + +void MPU6050::start(void) +{ + write_reg(ADDRESS,MPU6050_PWR_MGMT_1,0x00); + write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x10);//gyro +-1000deg/s + write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x08);//accel +-4G + write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x10);//sample rate 500Hz +} + +char MPU6050::getID(void) +{ + char devID; + read_reg(ADDRESS,MPU6050_WHO_AM_I,&devID); + return devID; +} + +bool MPU6050::read(float *gx, float *gy, float *gz,float *ax, float *ay, float *az) +{ + char data[6]; + char data2[6]; + if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) { + read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6); + *gx = float(short(data[0] << 8 | data[1]))*0.0305; + *gy = float(short(data[2] << 8 | data[3]))*0.0305; + *gz = float(short(data[4] << 8 | data[5]))*0.0305; + *ax = float(short(data2[0] << 8 | data2[1]))*0.000122; + *ay = float(short(data2[2] << 8 | data2[3]))*0.000122; + *az = float(short(data2[4] << 8 | data2[5]))*0.000122; + return true; + } + return false; +} + +bool MPU6050::write_reg(int addr_i2c,int addr_reg, char v) +{ + char data[2] = {addr_reg, v}; + return MPU6050::_MPU6050.write(addr_i2c, data, 2) == 0; +} + +bool MPU6050::read_reg(int addr_i2c,int addr_reg, char *v) +{ + char data = addr_reg; + bool result = false; + __disable_irq(); + if ((_MPU6050.write(addr_i2c, &data, 1) == 0) && (_MPU6050.read(addr_i2c, &data, 1) == 0)) { + *v = data; + result = true; + } + __enable_irq(); + return result; +} + + +bool MPU6050::read_data(char sad, char sub, char *buf, int length) +{ + if (length > 1) sub |= 0x80; + + return _MPU6050.write(sad, &sub, 1, true) == 0 && _MPU6050.read(sad, buf, length) == 0; +} + +/* sample code for Nucleo-F042K6 +#include "mbed.h" +#include "MPU6050.h" + +MPU6050 mpu(D7,D8); +Serial pc(USBTX,USBRX); + +float gx,gy,gz,ax,ay,az; + +int main() +{ + pc.baud(115200); + if(mpu.getID()==0x68) { + pc.printf("MPU6050 OK"); + wait(1); + } else { + pc.printf("MPU6050 error ID=0x%x\r\n",mpu.getID()); + while(1) { + } + } + mpu.start(); + while(1) { + mpu.read(&gx,&gy,&gz,&ax,&ay,&az); + pc.printf("gx,gy,gz,ax,ay,az %.1f,%.1f,%.1f,%.2f,%.2f,%.2f\r\n",gx,gy,gz,ax,ay,az); + wait(0.1); + } +} +*/ \ No newline at end of file