#define F_CPU 8000000UL									/* Define CPU clock Frequency e.g. here its 8MHz */
#include <avr/io.h>										/* Include AVR std. library file */
#include <util/delay.h>									/* Include delay header file */
#include <inttypes.h>									/* Include integer type header file */
#include <stdlib.h>										/* Include standard library file */
#include <stdio.h>										/* Include standard library file */
#include "calman.h"
#include "MPU6050_res_define.h"							/* Include MPU6050 register define file */
#include "I2C_Master_H_file.h"							/* Include I2C Master header file */
#include "USART_RS232_H_file.h"							/* Include USART header file */

float Acc_x,Acc_y,Acc_z,Temperature,Gyro_x,Gyro_y,Gyro_z, arx, ary, arz, grx, gry,grz;
float dt = 0.01;

void Gyro_Init()										/* Gyro initialization function */
{
	_delay_ms(150);										/* Power up time >100ms */
	I2C_Start_Wait(0xD0);								/* Start with device write address */
	I2C_Write(SMPLRT_DIV);								/* Write to sample rate register */
	I2C_Write(0x07);									/* 1KHz sample rate */
	I2C_Stop();

	I2C_Start_Wait(0xD0);
	I2C_Write(PWR_MGMT_1);								/* Write to power management register */
	I2C_Write(0x01);									/* X axis gyroscope reference frequency */
	I2C_Stop();

	I2C_Start_Wait(0xD0);
	I2C_Write(CONFIG);									/* Write to Configuration register */
	I2C_Write(0x00);									/* Fs = 8KHz */
	I2C_Stop();

	I2C_Start_Wait(0xD0);
	I2C_Write(GYRO_CONFIG);								/* Write to Gyro configuration register */
	I2C_Write(0x18);									/* Full scale range +/- 2000 degree/C */
	I2C_Stop();

	I2C_Start_Wait(0xD0);
	I2C_Write(INT_ENABLE);								/* Write to interrupt enable register */
	I2C_Write(0x01);
	I2C_Stop();
}

void MPU_Start_Loc()
{
	I2C_Start_Wait(0xD0);								/* I2C start with device write address */
	I2C_Write(ACCEL_XOUT_H);							/* Write start location address from where to read */
	I2C_Repeated_Start(0xD1);							/* I2C start with device read address */
}

void Read_RawValue()
{
	MPU_Start_Loc();									/* Read Gyro values */
	Acc_x = ~((((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Ack())-1);
	Acc_y = ~((((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Ack())-1);
	Acc_z = ~((((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Ack())-1);
	Temperature = (((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Ack());
	Gyro_x = ~((((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Ack())-1);
	Gyro_y = ~((((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Ack())-1);
	Gyro_z = ~((((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Nack())-1);
	I2C_Stop();
}

void calman_init_roll(void)
{
	//stan
	stan.theta=0.0;
	stan.omega=0.0;
	stan.pomiar_theta=0.0;
	stan.pomiar_omega=0.0;
	//kalman
	kalm.Q=0.055;//0.00000009; //mniejsza liczba zmniejsza wrazliwosc na przyspieszenia
	kalm.R=1.0;             //
	kalm.theta=0.0;
	kalm.omega_previous=0.0;
	kalm.omega=0.0;
	kalm.g_bias=0.0;
	kalm.P11=kalm.R;
	kalm.P13=0.0;
	kalm.P21=0.0;
	kalm.P31=0.0;
	kalm.P33=0.0;
	kalm.K1=0.0;
	kalm.K2=0.0;
	kalm.K3=0.0;
}
void calman_init_pitch(void)
{
	//stan
	stan1.theta=0.0;
	stan1.omega=0.0;
	stan1.pomiar_theta=0.0;
	stan1.pomiar_omega=0.0;
	//kalman
	kalm1.Q=0.055;//0.00000009; //mniejsza liczba zmniejsza wrazliwosc na przyspieszenia
	kalm1.R=1.0;            //
	kalm1.theta=0.0;
	kalm1.omega_previous=0.0;
	kalm1.omega=0.0;
	kalm1.g_bias=0.0;
	kalm1.P11=kalm.R;
	kalm1.P13=0.0;
	kalm1.P21=0.0;
	kalm1.P31=0.0;
	kalm1.P33=0.0;
	kalm1.K1=0.0;
	kalm1.K2=0.0;
	kalm1.K3=0.0;
}
/*
ustawienia dla Q
0.0001 szybka reakcja na plyniecie zyroskopu duza wrazliwosc na przyspieszenie układu
0.00001 -podobnie
0.000001-w miare szybka kompensacja dryftu +-15st wrazliowsc na przyspieszenia
0.0000001-wolna kompencajca dryftu +- 3 st wrazliwosci na przyspieszenia
*/
float calman_filtr_roll(float akcel,float gyro)
{
	stan.pomiar_theta=akcel;//(float)deg_roll_pitch[ROLL];          //odczyt kata z akcel do filtra
	stan.pomiar_omega=(0.0-gyro)*0.061052;//(0.0-value_gyro_axis[ROLL])*0.061052;  // przy rozdzielczosci 2000st/sec
	
	kalm.theta=((stan.pomiar_omega-kalm.g_bias)*dt)+kalm.theta;
	
	kalm.omega=stan.pomiar_omega-kalm.g_bias;
	
	//kalm.omega_previous=kalm.omega;
	
	//Pk=Ap_{k-1}A^T+Q
	kalm.P11 = kalm.P11-kalm.P31*dt+kalm.P33*dt*dt-kalm.P13*dt+kalm.Q;
	kalm.P13=kalm.P13-kalm.P33*dt;
	kalm.P21=kalm.P33*dt-kalm.P31;
	kalm.P31=kalm.P31-kalm.P33*dt;
	kalm.P33=kalm.P33+kalm.Q;


	//KOREKCJA

	//Kk=PkH^T(HPk^T+R)^-1

	kalm.K1=kalm.P11*(1.0/(kalm.P11+kalm.R));
	kalm.K2=kalm.P21*(1.0/(kalm.P11+kalm.R));
	kalm.K3=kalm.P31*(1.0/(kalm.P11+kalm.R));

	//xk=xk-K(zk-Hxk)

	kalm.theta=kalm.theta+kalm.K1*(stan.pomiar_theta-kalm.theta);
	kalm.omega=kalm.omega+kalm.K2*(stan.pomiar_theta-kalm.theta);
	kalm.g_bias=kalm.g_bias+kalm.K3*(stan.pomiar_theta-kalm.theta);

	//Pk=(1-Kkh)Pk

	kalm.P11=(1.0-kalm.K1)*kalm.P11;
	kalm.P13=(1.0-kalm.K1)*kalm.P13;
	kalm.P21=kalm.P21-kalm.P11*kalm.K2;
	kalm.P31=kalm.P31-kalm.P11*kalm.K3;
	kalm.P33=kalm.P33-kalm.P13*kalm.K3;

	//aktualizacja wektora stanu

	stan.theta=kalm.theta;
	stan.omega=kalm.omega;

	return stan.theta;
}

float calman_filtr_pitch(float akcel,float gyro)
{
        stan1.pomiar_theta=akcel;//(float)deg_roll_pitch[ROLL];         //odczyt kata z akcel do filtra
        stan1.pomiar_omega=(0.0-gyro)*0.061052;//(0.0-value_gyro_axis[ROLL])*0.061052;  // przy rozdzielczosci 2000st/sec
       
        kalm1.theta=((stan1.pomiar_omega-kalm1.g_bias)*dt)+kalm1.theta;
       
        kalm1.omega=stan1.pomiar_omega-kalm1.g_bias;
       
        //kalm.omega_previous=kalm.omega;
       
        //Pk=Ap_{k-1}A^T+Q
        kalm1.P11 = kalm1.P11-kalm1.P31*dt+kalm1.P33*dt*dt-kalm1.P13*dt+kalm1.Q;    
        kalm1.P13=kalm1.P13-kalm1.P33*dt;
        kalm1.P21=kalm1.P33*dt-kalm1.P31;
        kalm1.P31=kalm1.P31-kalm1.P33*dt;
        kalm1.P33=kalm1.P33+kalm1.Q;   
       
        //KOREKCJA
       
        //Kk=PkH^T(HPk^T+R)^-1
       
        kalm1.K1=kalm1.P11*(1.0/(kalm1.P11+kalm1.R));
        kalm1.K2=kalm1.P21*(1.0/(kalm1.P11+kalm1.R));
        kalm1.K3=kalm1.P31*(1.0/(kalm1.P11+kalm1.R));
       
        //xk=xk-K(zk-Hxk)
       
        kalm1.theta=kalm1.theta+kalm1.K1*(stan1.pomiar_theta-kalm1.theta);
        kalm1.omega=kalm1.omega+kalm1.K2*(stan1.pomiar_theta-kalm1.theta);
        kalm1.g_bias=kalm1.g_bias+kalm1.K3*(stan1.pomiar_theta-kalm1.theta);
       
        //Pk=(1-Kkh)Pk
       
        kalm1.P11=(1.0-kalm1.K1)*kalm1.P11;
        kalm1.P13=(1.0-kalm1.K1)*kalm1.P13;
        kalm1.P21=kalm1.P21-kalm1.P11*kalm1.K2;  //?
        kalm1.P31=kalm1.P31-kalm1.P11*kalm1.K3; //?
        kalm1.P33=kalm1.P33-kalm1.P13*kalm1.K3; //?
       
        //aktualizacja wektora stanu
       
        stan1.theta=kalm1.theta;
        stan1.omega=kalm1.omega;
       
        return stan1.theta;
}

void timer_init(){
	TCCR1A = ((1 << COM1A1) | (1<<COM1B1) | (1 << WGM11)); // clear OC1A on match + WGM mode 14
	TCCR1B = ((1 << WGM12) | (1 << WGM13) | (1 << CS10)); // WGM mode 14 (Fast PWM)
	
	ICR1  = 19999;  //set ICR1 to produce 50Hz frequency
	//(8000000 / 8 / 20000 = 50hz)
}

void servo_angle(uint8_t degree){
	OCR1A=9.73*degree+388;
}
void servo_angle_2(uint8_t degree){
	OCR1B=9.73*degree+388;
}

int main()
{
	char buffer[20], float_[10];
	float Xa,Ya,Za,t;
	float Xg=0,Yg=0,Zg=0, kalPitch, kalRoll, kalYaw;
	I2C_Init();											/* Initialize I2C */
	Gyro_Init();										/* Initialize Gyro */
	USART_Init(9600);									/* Initialize USART with 9600 baud rate */
	calman_init_roll();
	calman_init_pitch();
	timer_init();
	servo_angle(90);
	servo_angle_2(90);
	
	DDRD = 0xff;
	
	while(1)
	{
		Read_RawValue();

		Xa = Acc_x/16384.0;								/* Divide raw value by sensitivity scale factor to get real values */
		Ya = Acc_y/16384.0;
		Za = Acc_z/16384.0;
		
		Xg = Gyro_x/16.4;
		Yg = Gyro_y/16.4;
		Zg = Gyro_z/16.4;

		t = (Temperature/340.00)+36.53;					/* Convert temperature in °/c using formula */

		arx = (180/3.141592) * atan(Xa / sqrt(square(Ya) + square(Za)));  //pitch 
		ary = (180/3.141592) * atan(Ya / sqrt(square(Xa) + square(Za)));	//roll
		arz = (180/3.141592) * atan(sqrt(square(Ya) + square(Xa)) / Za);    //yaw
		 
		grx = grx + (0.047 * Xg);
		gry = gry + (0.047 * Yg);
		grz = grz + (0.047 * Zg);
		
		kalPitch = calman_filtr_pitch(arx, gry);
		kalRoll = calman_filtr_roll(ary, grx);
		
		
		if(kalRoll < 3 && kalRoll > -3) {
			servo_angle(90);
			servo_angle_2(-90);
		} else if(kalRoll > 3) {
			servo_angle(90-kalRoll);
			servo_angle_2(-90+kalRoll);
		} else if(kalRoll < -3) {
			servo_angle(90-(kalRoll));
			servo_angle_2(-90+(kalRoll));
		}
		
		
		dtostrf( arx, 3, 2, float_ );
		sprintf(buffer," AP = %s o\t",float_);
		USART_SendString(buffer);

		dtostrf( ary, 3, 2, float_ );
		sprintf(buffer," AR = %s o\t",float_);
		USART_SendString(buffer);
		
		dtostrf( grx, 3, 2, float_ );
		sprintf(buffer," Gx = %s o\t",float_);
		USART_SendString(buffer);

		dtostrf( gry, 3, 2, float_ );
		sprintf(buffer," Gy = %s o\t",float_,0xF8);
		USART_SendString(buffer);

		dtostrf( kalPitch, 3, 2, float_ );
		sprintf(buffer," kP = %s o\t",float_,0xF8);
		USART_SendString(buffer);

		dtostrf( kalRoll, 3, 2, float_ );
		sprintf(buffer," kR = %s 0\t\n",float_,0xF8);
		USART_SendString(buffer); 
		dtostrf( Xa, 3, 2, float_ );					
		sprintf(buffer," Ax = %s g\t",float_);
		USART_SendString(buffer);

		dtostrf( Ya, 3, 2, float_ );
		sprintf(buffer," Ay = %s g\t",float_);
		USART_SendString(buffer);
		
		dtostrf( Za, 3, 2, float_ );
		sprintf(buffer," Az = %s g\t",float_);
		USART_SendString(buffer);

		dtostrf( t, 3, 2, float_ );
		sprintf(buffer," T = %s%cC\t",float_,0xF8);           
		USART_SendString(buffer);

		dtostrf( Xg, 3, 2, float_ );
		sprintf(buffer," Gx = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);

		dtostrf( Yg, 3, 2, float_ );
		sprintf(buffer," Gy = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);
		
		dtostrf( Zg, 3, 2, float_ );
		sprintf(buffer," Gz = %s%c/s\r\n",float_,0xF8);
		USART_SendString(buffer); 
		
		dtostrf( grx, 3, 2, float_ );
		sprintf(buffer," Grx = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);

		dtostrf( gry, 3, 2, float_ );
		sprintf(buffer," Gry = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);
		
		dtostrf( grz, 3, 2, float_ );
		sprintf(buffer," Grz = %s%c/s\r",float_,0xF8);
		USART_SendString(buffer);
		
		dtostrf( arx, 3, 2, float_ );
		sprintf(buffer," Arx = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);

		dtostrf( ary, 3, 2, float_ );
		sprintf(buffer," Ary = %s%c/s\t",float_,0xF8);
		USART_SendString(buffer);
		
		dtostrf( arz, 3, 2, float_ );
		sprintf(buffer," Arz = %s%c/s\r\n",float_,0xF8);
		USART_SendString(buffer); 
	}
}