NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.
MPU6050.cpp
00001 #include "MPU6050.h" 00002 00003 MPU6050::MPU6050(PinName sda, PinName scl) : I2C_Sensor(sda, scl, MPU6050_I2C_ADDRESS) 00004 { 00005 // Turns on the MPU6050's gyro and initializes it 00006 // register datasheet: http://www.invensense.com/mems/gyro/documents/RM-MPU-6000A.pdf 00007 writeRegister(MPU6050_RA_PWR_MGMT_1, 0x01); // wake up from sleep and chooses Gyro X-Axis as Clock source (stadard sleeping and with inacurate clock is 0x40) 00008 writeRegister(MPU6050_RA_GYRO_CONFIG, 0x18); // scales gyros range to +-2000dps 00009 writeRegister(MPU6050_RA_ACCEL_CONFIG, 0x00); // scales accelerometers range to +-2g 00010 } 00011 00012 void MPU6050::read() 00013 { 00014 readraw_gyro(); // read raw measurement data 00015 readraw_acc(); 00016 00017 for (int i = 0; i < 3; i++) 00018 data_gyro[i] = (raw_gyro[i] - offset_gyro[i])*0.07; // subtract offset from calibration and multiply unit factor (datasheet s.10) 00019 00020 for (int i = 0; i < 3; i++) 00021 data_acc[i] = raw_acc[i] - offset_acc[i]; // TODO: didn't care about units because IMU-algorithm just uses vector direction 00022 } 00023 00024 int MPU6050::readTemp() 00025 { 00026 char buffer[2]; // 8-Bit pieces of temperature data 00027 00028 readMultiRegister(MPU6050_RA_TEMP_OUT_H, buffer, 2); // read the sensors register for the temperature 00029 return (short) (buffer[0] << 8 | buffer[1]); 00030 } 00031 00032 void MPU6050::readraw_gyro() 00033 { 00034 char buffer[6]; // 8-Bit pieces of axis data 00035 00036 readMultiRegister(MPU6050_RA_GYRO_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7) 00037 00038 raw_gyro[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers 00039 raw_gyro[1] = (short) (buffer[2] << 8 | buffer[3]); 00040 raw_gyro[2] = (short) (buffer[4] << 8 | buffer[5]); 00041 } 00042 00043 void MPU6050::readraw_acc() 00044 { 00045 char buffer[6]; // 8-Bit pieces of axis data 00046 00047 readMultiRegister(MPU6050_RA_ACCEL_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7) 00048 00049 raw_acc[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers 00050 raw_acc[1] = (short) (buffer[2] << 8 | buffer[3]); 00051 raw_acc[2] = (short) (buffer[4] << 8 | buffer[5]); 00052 } 00053 00054 void MPU6050::calibrate(int times, float separation_time) 00055 { 00056 // calibrate sensor with an average of count samples (result of calibration stored in offset[]) 00057 // Calibrate Gyroscope ---------------------------------- 00058 float calib_gyro[3] = {0,0,0}; // temporary array for the sum of calibration measurement 00059 00060 for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time 00061 readraw_gyro(); 00062 for (int j = 0; j < 3; j++) 00063 calib_gyro[j] += raw_gyro[j]; 00064 wait(separation_time); 00065 } 00066 00067 for (int i = 0; i < 3; i++) 00068 offset_gyro[i] = calib_gyro[i]/times; // take the average of the calibration measurements 00069 00070 // Calibrate Accelerometer ------------------------------- 00071 float calib_acc[3] = {0,0,0}; // temporary array for the sum of calibration measurement 00072 00073 for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time 00074 readraw_acc(); 00075 for (int j = 0; j < 3; j++) 00076 calib_acc[j] += raw_acc[j]; 00077 wait(separation_time); 00078 } 00079 00080 for (int i = 0; i < 2; i++) 00081 offset_acc[i] = calib_acc[i]/times; // take the average of the calibration measurements 00082 }
Generated on Tue Jul 12 2022 20:54:01 by 1.7.2