#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);
}
}
