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); */
}
}