
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"


//Stworzenie obiektu MPU6050
MPU6050 accelgyro(0x68);

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Opcje DMP
//Ustaw wartość TRUE jeżli inicjalizacja DMP przebiegła pomyślnie
bool dmpReady = false;
//Przechowuje aktualny bajt statusu przerwania z MPU
uint8_t mpuIntStatus;
//zwraca status po każdej operacji z urządzeniem
uint8_t devStatus;
//Oczekiwany rozmiar pakietu DMP
uint16_t packetSize;
//licznik wszystkich bajtów, które aktualnie znajdują się w FIFO
uint16_t fifoCount;
// bufor FIFO
uint8_t fifoBuffer[64];


#define OUTPUT_READABLE_QUATERNION

////////////////////////////////////////////////////////////////////////////////////////////////

//zmienne orientacji/ruchu

Quaternion q;
//VectorInt16 aa;
//VectorInt16 aaReal;
//VectorInt16 aaWorld;
//VectorFloat gravity;

//float euler[3];
//float ypr[3];

//PROCEDURA WYKRYCIA PRZERWANIA

volatile bool mpuInterrupt = false;     // wskazuje, czy pin przerwania MPU przeszedł do stanu wysokiego
void dmpDataReady() {
    mpuInterrupt = true;
}


void setup()
{
  //Zainicjowanie portu szeregowego z prędkością transmisji 115200
  Serial.begin(115200);  
  Setup_MPU6050();
}

void Setup_MPU6050()
{
    Wire.begin();
    // zainicjowanie urządzenia
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // weryfikacja połączenia
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
    
    //Zainicjowanie DMP w sensorze MPU 6050
    Setup_MPU6050_DMP();
}

//Konfiguracja układu DMP sensora MPU 6050
void Setup_MPU6050_DMP()
{
  
     //Inicjalizacja DMP
   devStatus = accelgyro.dmpInitialize();
   accelgyro.setXGyroOffset(220);
   accelgyro.setXGyroOffset(76);
   accelgyro.setXGyroOffset(-85); 
   accelgyro.setXGyroOffset(1788);  
   if(devStatus == 0){
    
		accelgyro.setDMPEnabled(true);
		pinMode(PUSH2,INPUT_PULLUP);    
		attachInterrupt(PUSH2, dmpDataReady, RISING);
		mpuIntStatus = accelgyro.getIntStatus();
		dmpReady = true;
		packetSize = accelgyro.dmpGetFIFOPacketSize();
     
	}
	else{
     
		;
    }
  
    
}

void loop()
{

	
    //Aktualizacja sensora MPU 6050
    Update_MPU6050();
}
void Update_MPU6050()
{
  
 
	  Update_MPU6050_DMP();

}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Aktualizacja funkcji DMP sensora MPU6050

void Update_MPU6050_DMP()
{
  
	//Przetwarzanie DMP 

    if (!dmpReady) return;
    
    while (!mpuInterrupt && fifoCount < packetSize) {
      
			;    
      
    }



    mpuInterrupt = false;
    mpuIntStatus = accelgyro.getIntStatus();
    
    //pobranie aktualnej wartości licznika FIFO
    fifoCount = accelgyro.getFIFOCount();
    
    
    if ((mpuIntStatus & 0x10) || fifoCount > 512) {
        // reset, aby była możliwa czysta kontynuacja
        accelgyro.resetFIFO();
    }

else if (mpuIntStatus & 0x02) {
        // oczekiwanie na poprawny dostępny rozmiar danych. Oczekiwanie powinno być BARDZO krótkie
        while (fifoCount < packetSize) fifoCount = accelgyro.getFIFOCount();

        // odczyt pakietu z FIFO
        accelgyro.getFIFOBytes(fifoBuffer, packetSize);
        
        // Ustawienie licznika FIFO w przypadku, gdy jest > 1 dostępny pakiet
        // (to pozwala natychmiastowo odczytać więcej pakietów bez czekania na przerwanie)
        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_QUATERNION
            // wyświetlenie wartości kwaternionów w czytelnym formacie tablicy: w x y z
        accelgyro.dmpGetQuaternion(&q, fifoBuffer);
            
            
        Serial.print("i");Serial.print("\t");
        Serial.print(q.x); Serial.print("\t");
        Serial.print(q.y); Serial.print("\t");
        Serial.print(q.z); Serial.print("\t");
        Serial.print(q.w);
        Serial.print("\n");
            
        #endif
    }
}


