1
Fork of MPU6050 by
MPU6050.cpp
- Committer:
- Stas285
- Date:
- 2017-05-21
- Revision:
- 3:48773a5c8d48
- Parent:
- 2:c68be621f2f1
- Child:
- 4:2d0953219845
File content as of revision 3:48773a5c8d48:
#include "mbed.h" #include "MPU6050.h" #define ADDRESS 0xD1 //68 shifted left: 7-bit to 8-bit address conversion 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,0x08);//gyro +-500deg/s //0x10);//gyro +-1000deg/s write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x00);//accel +-2G //0x08);//accel +-4G write_reg(ADDRESS,MPU6050_CONFIG,0x05);//bandwidth 10Hz, delay 14 mS write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x04);//sample rate 200Hz (1000/5) } 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]))*1.526e-2; //deg/s *gy = float(short(data[2] << 8 | data[3]))*1.526e-2; *gz = float(short(data[4] << 8 | data[5]))*1.526e-2; *ax = float(short(data2[0] << 8 | data2[1]))*5.9875e-4; //cm^2/s *ay = float(short(data2[2] << 8 | data2[3]))*5.9875e-4; *az = float(short(data2[4] << 8 | data2[5]))*5.9875e-4; return true; } return false; } bool MPU6050::readraw(int *gx, int *gy, int *gz,int *ax, int *ay, int *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 = int(short(data[0] << 8 | data[1])); *gy = int(short(data[2] << 8 | data[3])); *gz = int(short(data[4] << 8 | data[5])); *ax = int(short(data2[0] << 8 | data2[1])); *ay = int(short(data2[2] << 8 | data2[3])); *az = int(short(data2[4] << 8 | data2[5])); 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); } } */