#include <avr/io.h>
#include <avr/iom16.h>
#include <avr/interrupt.h>
#include <math.h>
#include <stdlib.h>
#include <avr/pgmspace.h>
//------------------------------------------
//-----------------------------------
#define RXB8 1
#define TXB8 0
#define UPE 2
#define OVR 3
#define FE 4
#define UDRE 5
#define RXC 7
#define FRAMING_ERROR (1<<FE)
#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<OVR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)
//-----------------------------------------
volatile unsigned int datadc;
volatile unsigned char flagt0,flagt1,channel,data,adc_ready,ch0,ch1,ch2,ch3,ch4,ch5;
volatile float gyro_x,gyro_y,accel_x,accel_y,accel_z,roll,pitch,roll_rate,pitch_rate,kalman_output,kalman_output1;
//float gyro_input;
//float accel_input;
void int_all()
{
// Declare your local variables here
// Input/Output Ports initialization
// Port A initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In
// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T
PORTA=0x00;
DDRA=0x30;
// Port B initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In
// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T
PORTB=0x00;
DDRB=0x00;
// Port C initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In
// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T
PORTC=0x00;
DDRC=0x00;
// Port D initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=Out Func6=In Func7=In
// State0=T State1=T State2=T State3=T State4=T State5=0 State6=T State7=T
PORTD=0x00;
DDRD=0x20;
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=FFh
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 1000.000 kHz
// Mode: Normal top=FFFFh
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
TCCR1A=0x00;
TCCR1B=0x02;
TCNT1H=0x00;
TCNT1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer 2 Stopped
// Mode: Normal top=FFh
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;
// External Interrupt(s) initialization
// INT0: On
// INT0 Mode: Falling Edge
// INT1: On
// INT1 Mode: Falling Edge
// INT2: Off
GICR|=0xC0;
MCUCR=0x0A;
MCUCSR=0x00;
GIFR=0xC0;
// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x04;
// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud rate: 4800
UCSRA=0x00;
UCSRB=0x98;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x67;
// ADC initialization
// ADC Clock frequency: 125.000 kHz
// ADC Voltage Reference: AREF pin
// ADC High Speed Mode: Off
// ADC Auto Trigger Source: None
ADMUX=0;
ADCSRA=0x86;
SFIOR&=0xEF;
}
//------------------------------------------------------------------------
//uart 发送一字节
int usart_putchar(char c)
{
if(c=='\n')
usart_putchar('\r');
loop_until_bit_is_set(UCSRA,UDRE);
UDR=c;
return 0;
}
//uart 接收一字节
int usart_getchar(void)
{
loop_until_bit_is_set(UCSRA,RXC);
return UDR;
}
void matrix_multiply(float* A, float* B, int m, int p, int n, float* C)
{
int i, j, k;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
{
C[n*i+j]=0;
for (k=0;k<p;k++)
C[n*i+j]= C[n*i+j]+A[p*i+k]*B[n*k+j];
}
}
static void matrix_addition(float* A, float* B, int m, int n, float* C)
{
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C[n*i+j]=A[n*i+j]+B[n*i+j];
}
void matrix_subtraction(float* A, float* B, int m, int n, float* C)
{
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C[n*i+j]=A[n*i+j]-B[n*i+j];
}
void matrix_transpose(float* A, int m, int n, float* C)
{
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C[m*j+i]=A[n*i+j];
}
int matrix_inversion(float* A, int n, float* AInverse)
{
int i, j, iPass, imx, icol, irow;
float det, temp, pivot, factor;
float* ac = (float*)calloc(n*n, sizeof(float));
det = 1;
for (i = 0; i < n; i++)
{
for (j = 0; j < n; j++)
{
AInverse[n*i+j] = 0;
ac[n*i+j] = A[n*i+j];
}
AInverse[n*i+i] = 1;
}
for (iPass = 0; iPass < n; iPass++)
{
imx = iPass;
for (irow = iPass; irow < n; irow++)
{
if (fabs(A[n*irow+iPass]) > fabs(A[n*imx+iPass])) imx = irow;
}
if (imx != iPass)
{
for (icol = 0; icol < n; icol++)
{
temp = AInverse[n*iPass+icol];
AInverse[n*iPass+icol] = AInverse[n*imx+icol];
AInverse[n*imx+icol] = temp;
if (icol >= iPass)
{
temp = A[n*iPass+icol];
A[n*iPass+icol] = A[n*imx+icol];
A[n*imx+icol] = temp;
}
}
}
pivot = A[n*iPass+iPass];
det = det * pivot;
if (det == 0)
{
free(ac);
return 0;
}
for (icol = 0; icol < n; icol++)
{
AInverse[n*iPass+icol] = AInverse[n*iPass+icol] / pivot;
if (icol >= iPass) A[n*iPass+icol] = A[n*iPass+icol] / pivot;
}
for (irow = 0; irow < n; irow++)
{
if (irow != iPass) factor = A[n*irow+iPass];
for (icol = 0; icol < n; icol++)
{
if (irow != iPass)
{
AInverse[n*irow+icol] -= factor * AInverse[n*iPass+icol];
A[n*irow+icol] -= factor * A[n*iPass+icol];
}
}
}
}
free(ac);
return 1;
}
float kalman_update(float gyroscope_rate, float accelerometer_angle)
{
static float A[2][2] = {{1.0, -0.019968}, {0.0, 1.0}};
static float B[2][1] = {{0.019968}, {0.0}};
static float C[1][2] = {{1.0, 0.0}};
static float Sz[1][1] = {{17.2}};
static float Sw[2][2] = {{0.005, 0.005}, {0.005, 0.005}};
static float xhat[2][1] = {{0.0}, {0.0}};
static float P[2][2] = {{0.005, 0.005}, {0.005, 0.005}};
float u[1][1];
float y[1][1];
float AP[2][2];
float CT[2][1];
float APCT[2][1];
float CP[1][2];
float CPCT[1][1];
float CPCTSz[1][1];
float CPCTSzInv[1][1];
float K[2][1];
float Cxhat[1][1];
float yCxhat[1][1];
float KyCxhat[2][1];
float Axhat[2][1];
float Bu[2][1];
float AxhatBu[2][1];
float AT[2][2];
float APAT[2][2];
float APATSw[2][2];
float KC[2][2];
float KCP[2][2];
float KCPAT[2][2];
u[0][0] = gyroscope_rate;
y[0][0] = accelerometer_angle;
matrix_multiply((float*) A, (float*) xhat, 2, 2, 1, (float*) Axhat);
matrix_multiply((float*) B, (float*) u, 2, 1, 1, (float*) Bu);
matrix_addition((float*) Axhat, (float*) Bu, 2, 1, (float*) AxhatBu);
matrix_multiply((float*) C, (float*) xhat, 1, 2, 1, (float*) Cxhat);
matrix_subtraction((float*) y, (float*) Cxhat, 1, 1, (float*) yCxhat);
matrix_transpose((float*) C, 1,