mpu6500.cpp

00001 #include "mbed.h"
00002 #include "mpu6500.h"
00003 
00004 //I2C  mpu6500_i2c(PC_9, PA_15); // I2C0_SDA, I2C0_SCL
00005 extern I2C i2c0;
00006 I2C  *mpu6500_i2c;
00007 
00008 void MPU6500_WriteByte(uint8_t MPU6500_reg, uint8_t MPU6500_data)
00009 {
00010     char data_out[2];
00011     data_out[0]=MPU6500_reg;
00012     data_out[1]=MPU6500_data;
00013     mpu6500_i2c->write(MPU6500_slave_addr, data_out, 2, 0);
00014 }
00015 
00016 uint8_t MPU6500_ReadByte(uint8_t MPU6500_reg)
00017 {
00018     char data_out[1], data_in[1];
00019     data_out[0] = MPU6500_reg;
00020     mpu6500_i2c->write(MPU6500_slave_addr, data_out, 1, 1);
00021     mpu6500_i2c->read(MPU6500_slave_addr, data_in, 1, 0);
00022     return (data_in[0]);
00023 }
00024 
00025 void MPU6500::initialize()
00026 {
00027     mpu6500_i2c = &i2c0;
00028     MPU6500_WriteByte(MPU6500_PWR_MGMT_1, 0x00);    // CLK_SEL=0: internal 8MHz, TEMP_DIS=0, SLEEP=0 
00029     MPU6500_WriteByte(MPU6500_SMPLRT_DIV, 0x07);  // Gyro output sample rate = Gyro Output Rate/(1+SMPLRT_DIV)
00030     MPU6500_WriteByte(MPU6500_CONFIG, 0x06);      // set TEMP_OUT_L, DLPF=2 (Fs=1KHz)
00031     MPU6500_WriteByte(MPU6500_GYRO_CONFIG, 0x18); // bit[4:3] 0=+-250d/s,1=+-500d/s,2=+-1000d/s,3=+-2000d/s
00032     MPU6500_WriteByte(MPU6500_ACCEL_CONFIG, 0x01);// bit[4:3] 0=+-2g,1=+-4g,2=+-8g,3=+-16g, ACC_HPF=On (5Hz)
00033 }
00034 
00035 int16_t MPU6500::getAccelXvalue()
00036 {
00037     uint8_t LoByte, HiByte;
00038     LoByte = MPU6500_ReadByte(MPU6500_ACCEL_XOUT_L); // read Accelerometer X_Low  value
00039     HiByte = MPU6500_ReadByte(MPU6500_ACCEL_XOUT_H); // read Accelerometer X_High value
00040     return((HiByte<<8) | LoByte);
00041 }
00042 
00043 int16_t MPU6500::getAccelYvalue()
00044 {
00045     uint8_t LoByte, HiByte;
00046     LoByte = MPU6500_ReadByte(MPU6500_ACCEL_YOUT_L); // read Accelerometer X_Low  value
00047     HiByte = MPU6500_ReadByte(MPU6500_ACCEL_YOUT_H); // read Accelerometer X_High value
00048     return ((HiByte<<8) | LoByte);
00049 }
00050 
00051 int16_t MPU6500::getAccelZvalue()
00052 {
00053     uint8_t LoByte, HiByte;
00054     LoByte = MPU6500_ReadByte(MPU6500_ACCEL_ZOUT_L); // read Accelerometer X_Low  value
00055     HiByte = MPU6500_ReadByte(MPU6500_ACCEL_ZOUT_H); // read Accelerometer X_High value
00056     return ((HiByte<<8) | LoByte);
00057 }
00058 
00059 int16_t MPU6500::getGyroXvalue()
00060 {
00061     uint8_t LoByte, HiByte;
00062     LoByte = MPU6500_ReadByte(MPU6500_GYRO_XOUT_L); // read Accelerometer X_Low  value
00063     HiByte = MPU6500_ReadByte(MPU6500_GYRO_XOUT_H); // read Accelerometer X_High value
00064     return ((HiByte<<8) | LoByte);
00065 }
00066 
00067 int16_t MPU6500::getGyroYvalue()
00068 {
00069     uint8_t LoByte, HiByte;
00070     LoByte = MPU6500_ReadByte(MPU6500_GYRO_YOUT_L); // read Accelerometer X_Low  value
00071     HiByte = MPU6500_ReadByte(MPU6500_GYRO_YOUT_H); // read Accelerometer X_High value
00072     return ((HiByte<<8) | LoByte);
00073 }
00074 
00075 int16_t MPU6500::getGyroZvalue()
00076 {
00077     uint8_t LoByte, HiByte;
00078     LoByte = MPU6500_ReadByte(MPU6500_GYRO_ZOUT_L); // read Accelerometer X_Low  value
00079     HiByte = MPU6500_ReadByte(MPU6500_GYRO_ZOUT_H); // read Accelerometer X_High value
00080     return ((HiByte<<8) | LoByte);
00081 }