Josip Papratović
/
1_MIKROUPRAVLJACI
Programsko rješenje za uređaj koji mjeri nagib i udaljenost.
MPU_Library/MPU.cpp
- Committer:
- jpapratov
- Date:
- 2019-11-16
- Revision:
- 1:024485d1c677
- Parent:
- 0:34c1f05d8d2c
File content as of revision 1:024485d1c677:
#include "MPU.h" #include "mbed.h" Senzor::Senzor(PinName Sda, PinName Scl): mpu(Sda, Scl){ mpu.frequency(400000); setPowerManagement (0x00);//enable measurement GYRO_CONFIG(0x10);//Set the register bits as 00010000 (1000dps full scale) ACCEL_CONFIG(0x10);//Set the register bits as 00010000 (+/- 8g full scale range) setBW(0x00);//Set Bandwidth 0 -> 256Hz } char Senzor::testConnection () { char dat; char reg=WHO_AM_I_REG; mpu.write(ADDRESS, ®, 1, true); mpu.read(ADDRESS, &dat, 1, false); return dat; } void Senzor::setBW (char command) { char dat[2]; dat[0]=CONFIG_REG; dat[1]=command; mpu.write(ADDRESS, dat, 2, false); } void Senzor::setPowerManagement (char command) { char dat[2]; dat[0]=PWR_MGMT_1_REG; dat[1]=command; mpu.write(ADDRESS, dat, 2, false); } void Senzor::GYRO_CONFIG(char command) { char dat[2]; dat[0] = GYRO_CONFIG_REG; //GYRO_CONFIG register dat[1] = command; mpu.write(ADDRESS, dat, 2, false); } void Senzor::ACCEL_CONFIG(char command) { char dat[2]; dat[0] = ACCELERO_CONFIG_REG; //ACCEL_CONFIG register dat[1] = command; mpu.write(ADDRESS, dat, 2, false); } void Senzor::getAccel(float *accel) { char dat[6]; 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 mpu.write(ADDRESS, &dat[0], 1, true); mpu.read(ADDRESS, dat, 6, false);//krece od 3B -> 40 accel[0] = short(dat[0] << 8 | dat[1])/ 4096.0 * 9.81;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) accel[1] = short(dat[2] << 8 | dat[3])/ 4096.0 * 9.81;// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) accel[2] = short(dat[4] << 8 | dat[5])/ 4096.0 * 9.81;// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) } void Senzor::getGyro(float *gyro)//očitavanje žiroskopa i spremanje podataka { char dat[6]; dat[0] = GYRO_XOUT_H_REG; //GYRO_XOUT_H,GYRO_XOUT_L,GYRO_YOUT_H,GYRO_YOUT_L,GYRO_ZOUT_H,GYRO_ZOUT_L mpu.write(ADDRESS, &dat[0], 1, true); mpu.read(ADDRESS, dat, 6, false);//krece od 43 -> 48 gyro[0] = short(dat[0] << 8 | dat[1])/ 1879.3;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) gyro[1] = short(dat[2] << 8 | dat[3])/ 1879.3;// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) gyro[2] = short(dat[4] << 8 | dat[5])/ 1879.3;// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) } void Senzor::getAccAngle(float *angle)//računanje kuteva x i y pomoću akceleracije { float temp[3]; getAccel(temp); angle[0] = atan (temp[1]/sqrt(pow(temp[0], 2) + pow(temp[2], 2))) * 57.3; //calculate angle x(pitch/roll?) from accellerometer reading 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 angle[2] = atan (sqrt(pow(temp[0], 2) + pow(temp[1], 2))/temp[2]) * 57.3;//calculate angle z } void Senzor::getOffset(float *accOffset, float *gyroOffset, int sample)//računanje odstupanja od stvarne vrijednosti te oduzimanje eventualne pogreške očitanja { float gyro[3]; float accAngle[3]; sample=200; for (int i = 0; i < 3; i++) { accOffset[i] = 0.0; //initialise offsets to 0.0 gyroOffset[i] = 0.0; } for (int i = 0; i < sample; i++) { getGyro(gyro); //take real life measurements getAccAngle(accAngle); for (int j = 0; j < 3; j++) { *(accOffset+j) += accAngle[j]/sample; //average measurements *(gyroOffset+j) += gyro[j]/sample; } wait_ms (10); //wait between each reading for accuracy } for (int k = 0; k < 3; k++) { gyro[k] -= gyroOffset[k]; //substract offset values accAngle[k] -= accOffset[k]; } } void Senzor::finalAngle (float *angle) { float GyroAng[2]; float gyro[3]; float accAngle[3]; float interval; Timer t; interval=t.read(); t.reset(); getGyro(gyro); //get gyro value in rad/s getAccAngle(accAngle); //get angle from accelerometer t.start(); // računanje stupnjeva Gyro kuteva rad/s*s=rad*57.3=deg GyroAng[0]=GyroAng[0] + gyro[0]* interval*57.3; GyroAng[1]=GyroAng[1] + gyro[1] * interval*57.3; angle[2]= angle[2] + gyro[2] * interval*57.3; //yaw angle[0]=0.96*GyroAng[0]+0.04*accAngle[0]*25; //pitch angle[1] = 0.96 * GyroAng[1] + 0.04 * accAngle[1]*25; //roll } float Senzor::getTemp()//očitavanje i spremanje temperature { char dat[2]; float temp; dat[0]=TEMP_H_REG; mpu.write(ADDRESS, &dat[0], 1, true); mpu.read(ADDRESS, dat, 2, false);//krece od 41 -> 42 temp =short(dat[0] << 8 | dat[1]);// 0x41 (TEMP_H_REG) & 0x42 (TEMP_H_REG) temp=((double)temp+521.0)/340.0+35.0; return temp; }