ahrs.h

00001 #define Ahrs_num 40
00002 #define start 0
00003 #define head1 0xFF
00004 #define head2 0xFF
00005 #define check1 1
00006 #define check2 2
00007 
00008 unsigned char buf[1000], head[2], q[41], ck[1], data_idx = 0;
00009 char AHRS[Ahrs_num], check_ahrs = start, checkdata = 0, checksum;
00010 
00011 
00012 void IMU_Update(){
00013     fcc.roll = IMU.roll;
00014     fcc.pitch = IMU.pitch;
00015     fcc.yaw = IMU.yaw;
00016     fcc.g_roll = IMU.g_roll;
00017     fcc.g_pitch = IMU.g_pitch;
00018     fcc.g_yaw = IMU.g_yaw;
00019     fcc.acc_x = IMU.acc_x;
00020     fcc.acc_y = IMU.acc_y;
00021     fcc.acc_z = IMU.acc_z;
00022 } 
00023         
00024 
00025 void Get_AHRS(char *data){
00026     int i;
00027     char *dTemp = (char *) &IMU;
00028     
00029     for(i=0; i<Ahrs_num; i++) *(dTemp+i) = *(data+i);
00030     IMU_Update();
00031 }   
00032 
00033 void ahrs_data(unsigned char data){
00034     
00035     if(check_ahrs == start){
00036         if(data == head1) check_ahrs = check1;
00037         else check_ahrs = start;
00038         }
00039     
00040     else if(check_ahrs == check1){
00041         if(data == head2) check_ahrs = check2;
00042         else check_ahrs = start;
00043         }
00044     
00045     else if(check_ahrs == check2){
00046         AHRS[data_idx] = data;
00047         if(data_idx<(Ahrs_num-1)){                  
00048              //checkdata ^= AHRS[data_idx];
00049              data_idx++;
00050              if(data_idx == 3) data_idx = 4;
00051         }
00052         else{
00053             checksum = data;
00054             //if(checksum == checkdata)
00055             Get_AHRS(AHRS);                        
00056             data_idx = 0;
00057             check_ahrs = start;
00058         }                  
00059     }
00060     
00061     else{
00062         data_idx = 0;
00063         check_ahrs = start;
00064     }
00065 }  
00066 
00067 void ahrs_rec(){      
00068     unsigned char data;
00069     data = ahrs.getc();
00070     ahrs_data(data);
00071     //pc.putc(data);
00072 }