ahrs.h

00001 #define Ahrs_num 42
00002 char checkhead[2], imu_cnt = 0, data_flag = 0, gcs_cnt = 0;
00003 unsigned char data_que[Ahrs_num];
00004 
00005 
00006 void IMU_Update(){
00007     fcc.roll = IMU.roll;
00008     fcc.pitch = IMU.pitch;
00009     fcc.yaw = IMU.yaw;
00010     fcc.g_roll = IMU.g_roll;
00011     fcc.g_pitch = IMU.g_pitch;
00012     fcc.g_yaw = IMU.g_yaw;
00013     fcc.acc_x = IMU.acc_x;
00014     fcc.acc_y = IMU.acc_y;
00015     fcc.acc_z = IMU.acc_z;// + (cos((fcc.roll*3.14)/180) * cos((fcc.pitch*3.14)/180));
00016     fcc.bat = (unsigned short)(bat * 12000);
00017     fcc.US = (unsigned short)(alti.read_u16() * 0.06);
00018     fcc.IR1 = (unsigned short)(IRb.read_u16() * 0.01) - 220;
00019     fcc.IR2 = (unsigned short)(IRl.read_u16() * 0.01) - 220;
00020     fcc.IR3 = (unsigned short)(IRf.read_u16() * 0.01) - 220;
00021     fcc.IR4 = (unsigned short)(IRr.read_u16() * 0.01) - 220;
00022     
00023     if(fcc.roll>180 || fcc.roll<-180) fcc.roll = last.roll;
00024     if(fcc.pitch>90 || fcc.pitch<-90) fcc.pitch = last.pitch;
00025     if(fcc.yaw>360 || fcc.yaw<0) fcc.yaw = last.yaw;
00026     if(fcc.g_roll>300 || fcc.g_roll<-300) fcc.g_roll = last.g_roll;
00027     if(fcc.g_pitch>300 || fcc.g_pitch<-300) fcc.g_pitch = last.g_pitch;
00028     if(fcc.g_yaw>300 || fcc.g_yaw<-300) fcc.g_yaw = last.g_yaw;
00029     if(fcc.acc_x>4 || fcc.acc_x<-4) fcc.acc_x = last.acc_x;
00030     if(fcc.acc_y>4 || fcc.acc_y<-4) fcc.acc_y = last.acc_y;
00031     if(fcc.acc_z>4 || fcc.acc_z<-4) fcc.acc_z = last.acc_z;
00032     
00033     last.roll = IMU.roll;
00034     last.pitch = IMU.pitch;
00035     last.yaw = IMU.yaw;
00036     last.g_roll = IMU.g_roll;
00037     last.g_pitch = IMU.g_pitch;
00038     last.g_yaw = IMU.g_yaw;
00039     last.acc_x = IMU.acc_x;
00040     last.acc_y = IMU.acc_y;
00041     last.acc_z = IMU.acc_z;
00042     
00043     if(gcs_cnt == 3){
00044         //IMUtoGCS();
00045         gcs_cnt = 0;
00046     }
00047     else toBHG();
00048     gcs_cnt++;    
00049 
00050 }                 
00051 
00052 void ahrs_parsing(){
00053     int i = 0;
00054     
00055     data.roll = (char *) &IMU.roll;
00056     for(i=0; i<4; i++) data.roll[i] = data_que[i+28];
00057     
00058     data.pitch = (char *) &IMU.pitch;
00059     for(i=0; i<4; i++) data.pitch[i] = data_que[i+32];
00060     
00061     data.yaw = (char *) &IMU.yaw;
00062     for(i=0; i<4; i++) data.yaw[i] = data_que[i+36];    
00063     
00064     data.g_roll = (char *) &IMU.g_roll;
00065     for(i=0; i<4; i++) data.g_roll[i] = data_que[i+4];
00066     
00067     data.g_pitch = (char *) &IMU.g_pitch;
00068     for(i=0; i<4; i++) data.g_pitch[i] = data_que[i+8];
00069     
00070     data.g_yaw = (char *) &IMU.g_yaw;
00071     for(i=0; i<4; i++) data.g_yaw[i] = data_que[i+12];        
00072             
00073     data.acc_x = (char *) &IMU.acc_x;
00074     for(i=0; i<4; i++) data.acc_x[i] = data_que[i+16];
00075     
00076     data.acc_y = (char *) &IMU.acc_y;
00077     for(i=0; i<4; i++) data.acc_y[i] = data_que[i+20];
00078     
00079     data.acc_z = (char *) &IMU.acc_z;
00080     for(i=0; i<4; i++) data.acc_z[i] = data_que[i+24]; 
00081     
00082     IMU_Update();
00083 }
00084 
00085 void Checksum(){
00086     char checksum = 0;
00087     for(char i=1; i<40; i++) checksum = checksum ^ data_que[i];
00088     if(checksum == data_que[40]) ahrs_parsing();
00089 }    
00090 
00091 void ahrs_rec(){      
00092     unsigned char data;
00093     data = ahrs.getc();
00094     checkhead[0] = checkhead[1];
00095     checkhead[1] = data;
00096     if(checkhead[0] == 0xFF && checkhead[1] == 0xFF) {
00097         imu_cnt = 0;
00098         data_flag = 1;
00099     }
00100     if(data_flag == 1){
00101         data_que[imu_cnt] = data;
00102         imu_cnt++;
00103     }
00104     if(imu_cnt == 41){
00105         Checksum();
00106         data_flag = 2;
00107     }    
00108 }
00109 
00110 
00111 
00112 
00113     
00114     
00115     
00116     
00117         
00118