File content as of revision 0:34c1f05d8d2c:

#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, &reg, 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;
}