ADIS16255陀螺仪程序

#include<avr/io.h> #include<avr/interrupt.h> #include<util/delay.h> #include"ADIS16255.h" #include"UART.h" #include"I2C.h" #include"UI.h" int main(void) { UART_init(); I2C_init(); I2C_localaddress(10); gyro_init(); UI_init(); int i=0; sei(); //double X=0,Z,P=1,Q=1e-1,R=1e-1; _delay_ms(300); printf("正在进行零漂修正，请稍后...\n"); gyro_calibration(4); printf("零漂修正结束。\n"); while (1) { if (key0()) { i=0; LED_on(); } if (key1()) { LED_off(); } if (UART_received()) { printf("寄存器数据：%X %X\n",UART_data,gyro_read(UART_data)); } //Kalman filter /*{ Z=0.07326*b; X=(R*X+(P+Q)*Z)/(P+Q+R); P=R*(P+Q)/(P+Q+R); }*/ if (key1()) { gyro_calibration(2); } double angle=gyro_angle(); if (angle>180) angle-=360; printf("时间：%4d ",i); printf("角度：%10.5f ",angle); printf("温度： %10.4f\n",gyro_temp()); ++i; _delay_ms(1000); } }