MPU.cpp

00001 #include "MPU.h"
00002 #include "mbed.h"
00003 
00004 Senzor::Senzor(PinName Sda, PinName Scl): mpu(Sda, Scl){
00005     mpu.frequency(400000);
00006     setPowerManagement (0x00);//enable measurement
00007 
00008     GYRO_CONFIG(0x10);//Set the register bits as 00010000 (1000dps full scale)
00009 
00010     ACCEL_CONFIG(0x10);//Set the register bits as 00010000 (+/- 8g full scale range)
00011 
00012     setBW(0x00);//Set Bandwidth 0 -> 256Hz
00013 }
00014 
00015     char Senzor::testConnection ()
00016 {
00017     char dat;
00018     char reg=WHO_AM_I_REG;
00019     mpu.write(ADDRESS, &reg, 1, true);
00020     mpu.read(ADDRESS, &dat, 1, false);
00021     return dat;
00022 }
00023 void Senzor::setBW (char command)
00024 {
00025     char dat[2];
00026 
00027     dat[0]=CONFIG_REG;
00028     dat[1]=command;
00029 
00030     mpu.write(ADDRESS, dat, 2, false);
00031 }
00032 
00033 void Senzor::setPowerManagement (char command)
00034 {
00035     char dat[2];
00036 
00037     dat[0]=PWR_MGMT_1_REG;
00038     dat[1]=command;
00039 
00040     mpu.write(ADDRESS, dat, 2, false);
00041 
00042 }
00043 
00044 void Senzor::GYRO_CONFIG(char command)
00045 {
00046     char dat[2];
00047 
00048     dat[0] = GYRO_CONFIG_REG; //GYRO_CONFIG register
00049     dat[1] = command;
00050 
00051     mpu.write(ADDRESS, dat, 2, false);
00052 }
00053 
00054 void Senzor::ACCEL_CONFIG(char command)
00055 {
00056     char dat[2];
00057 
00058     dat[0] = ACCELERO_CONFIG_REG; //ACCEL_CONFIG register
00059     dat[1] = command;
00060 
00061     mpu.write(ADDRESS, dat, 2, false);
00062 }
00063 
00064 void Senzor::getAccel(float *accel)
00065 {
00066     char dat[6];
00067     dat[0] = ACCEL_XOUT_H_REG; //ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H, ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L
00068 
00069     mpu.write(ADDRESS, &dat[0], 1, true);
00070     mpu.read(ADDRESS, dat, 6, false);//krece od 3B -> 40
00071 
00072     accel[0] = short(dat[0] << 8 | dat[1])/ 4096.0 * 9.81;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
00073     accel[1] = short(dat[2] << 8 | dat[3])/ 4096.0 * 9.81;// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
00074     accel[2] = short(dat[4] << 8 | dat[5])/ 4096.0 * 9.81;// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
00075 
00076 }
00077 void Senzor::getGyro(float *gyro)//očitavanje žiroskopa i spremanje podataka
00078 {
00079     char dat[6];
00080     dat[0] = GYRO_XOUT_H_REG; //GYRO_XOUT_H,GYRO_XOUT_L,GYRO_YOUT_H,GYRO_YOUT_L,GYRO_ZOUT_H,GYRO_ZOUT_L
00081 
00082     mpu.write(ADDRESS, &dat[0], 1, true);
00083     mpu.read(ADDRESS, dat, 6, false);//krece od 43 -> 48
00084 
00085     gyro[0] = short(dat[0] << 8 | dat[1])/ 1879.3;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
00086     gyro[1] = short(dat[2] << 8 | dat[3])/ 1879.3;// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
00087     gyro[2] = short(dat[4] << 8 | dat[5])/ 1879.3;// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
00088 }
00089 void Senzor::getAccAngle(float *angle)//računanje kuteva x i y pomoću akceleracije
00090 {
00091     float temp[3];
00092     getAccel(temp);
00093     angle[0] = atan (temp[1]/sqrt(pow(temp[0], 2) + pow(temp[2], 2))) * 57.3; //calculate angle x(pitch/roll?) from accellerometer reading
00094     angle[1] = atan (-1*temp[0]/sqrt(pow(temp[1], 2) + pow(temp[2], 2))) * 57.3; //calculate angle y(pitch/roll?) from accellerometer reading
00095     angle[2] = atan (sqrt(pow(temp[0], 2) + pow(temp[1], 2))/temp[2]) * 57.3;//calculate angle z
00096 }
00097 void Senzor::getOffset(float *accOffset, float *gyroOffset, int sample)//računanje odstupanja od stvarne vrijednosti te oduzimanje eventualne pogreške očitanja
00098 {
00099     float gyro[3];
00100     float accAngle[3];
00101     sample=200;
00102 
00103     for (int i = 0; i < 3; i++) {
00104         accOffset[i] = 0.0;     //initialise offsets to 0.0
00105         gyroOffset[i] = 0.0;
00106     }
00107 
00108     for (int i = 0; i < sample; i++) {
00109         getGyro(gyro); //take real life measurements
00110         getAccAngle(accAngle);
00111 
00112         for (int j = 0; j < 3; j++) {
00113             *(accOffset+j) += accAngle[j]/sample;    //average measurements
00114             *(gyroOffset+j) += gyro[j]/sample;
00115         }
00116         wait_ms (10);    //wait between each reading for accuracy
00117     }
00118     for (int k = 0; k < 3; k++) {
00119         gyro[k] -= gyroOffset[k];       //substract offset values
00120         accAngle[k] -= accOffset[k];
00121     }
00122 }
00123 void Senzor::finalAngle (float *angle)
00124 {
00125     float GyroAng[2];
00126     float gyro[3];
00127     float accAngle[3];
00128     float interval;
00129     Timer t;
00130 
00131     interval=t.read();
00132     t.reset();
00133     getGyro(gyro);           //get gyro value in rad/s
00134     getAccAngle(accAngle);   //get angle from accelerometer
00135     t.start();
00136     // računanje stupnjeva Gyro kuteva rad/s*s=rad*57.3=deg
00137     GyroAng[0]=GyroAng[0] + gyro[0]* interval*57.3;
00138     GyroAng[1]=GyroAng[1] + gyro[1] * interval*57.3;
00139     angle[2]= angle[2] + gyro[2] * interval*57.3; //yaw
00140     
00141     angle[0]=0.96*GyroAng[0]+0.04*accAngle[0]*25; //pitch
00142     angle[1] = 0.96 * GyroAng[1] + 0.04 * accAngle[1]*25; //roll
00143 
00144 }
00145 float Senzor::getTemp()//očitavanje i spremanje temperature
00146 {
00147     char dat[2];
00148     float temp;
00149 
00150     dat[0]=TEMP_H_REG;
00151 
00152     mpu.write(ADDRESS, &dat[0], 1, true);
00153     mpu.read(ADDRESS, dat, 2, false);//krece od 41 -> 42
00154 
00155     temp =short(dat[0] << 8 | dat[1]);// 0x41 (TEMP_H_REG) & 0x42 (TEMP_H_REG)
00156 
00157     temp=((double)temp+521.0)/340.0+35.0;
00158 
00159     return temp;
00160 }