Diff: ahrs.h

Revision:

0:d463d5c04541

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ahrs.h	Tue Aug 16 05:32:33 2011 +0000
@@ -0,0 +1,118 @@
+#define Ahrs_num 42
+char checkhead[2], imu_cnt = 0, data_flag = 0, gcs_cnt = 0;
+unsigned char data_que[Ahrs_num];
+
+
+void IMU_Update(){
+    fcc.roll = IMU.roll;
+    fcc.pitch = IMU.pitch;
+    fcc.yaw = IMU.yaw;
+    fcc.g_roll = IMU.g_roll;
+    fcc.g_pitch = IMU.g_pitch;
+    fcc.g_yaw = IMU.g_yaw;
+    fcc.acc_x = IMU.acc_x;
+    fcc.acc_y = IMU.acc_y;
+    fcc.acc_z = IMU.acc_z;// + (cos((fcc.roll*3.14)/180) * cos((fcc.pitch*3.14)/180));
+    fcc.bat = (unsigned short)(bat * 12000);
+    fcc.US = (unsigned short)(alti.read_u16() * 0.06);
+    fcc.IR1 = (unsigned short)(IRb.read_u16() * 0.01) - 220;
+    fcc.IR2 = (unsigned short)(IRl.read_u16() * 0.01) - 220;
+    fcc.IR3 = (unsigned short)(IRf.read_u16() * 0.01) - 220;
+    fcc.IR4 = (unsigned short)(IRr.read_u16() * 0.01) - 220;
+    
+    if(fcc.roll>180 || fcc.roll<-180) fcc.roll = last.roll;
+    if(fcc.pitch>90 || fcc.pitch<-90) fcc.pitch = last.pitch;
+    if(fcc.yaw>360 || fcc.yaw<0) fcc.yaw = last.yaw;
+    if(fcc.g_roll>300 || fcc.g_roll<-300) fcc.g_roll = last.g_roll;
+    if(fcc.g_pitch>300 || fcc.g_pitch<-300) fcc.g_pitch = last.g_pitch;
+    if(fcc.g_yaw>300 || fcc.g_yaw<-300) fcc.g_yaw = last.g_yaw;
+    if(fcc.acc_x>4 || fcc.acc_x<-4) fcc.acc_x = last.acc_x;
+    if(fcc.acc_y>4 || fcc.acc_y<-4) fcc.acc_y = last.acc_y;
+    if(fcc.acc_z>4 || fcc.acc_z<-4) fcc.acc_z = last.acc_z;
+    
+    last.roll = IMU.roll;
+    last.pitch = IMU.pitch;
+    last.yaw = IMU.yaw;
+    last.g_roll = IMU.g_roll;
+    last.g_pitch = IMU.g_pitch;
+    last.g_yaw = IMU.g_yaw;
+    last.acc_x = IMU.acc_x;
+    last.acc_y = IMU.acc_y;
+    last.acc_z = IMU.acc_z;
+    
+    if(gcs_cnt == 3){
+        //IMUtoGCS();
+        gcs_cnt = 0;
+    }
+    else toBHG();
+    gcs_cnt++;    
+
+}                 
+
+void ahrs_parsing(){
+    int i = 0;
+    
+    data.roll = (char *) &IMU.roll;
+    for(i=0; i<4; i++) data.roll[i] = data_que[i+28];
+    
+    data.pitch = (char *) &IMU.pitch;
+    for(i=0; i<4; i++) data.pitch[i] = data_que[i+32];
+    
+    data.yaw = (char *) &IMU.yaw;
+    for(i=0; i<4; i++) data.yaw[i] = data_que[i+36];    
+    
+    data.g_roll = (char *) &IMU.g_roll;
+    for(i=0; i<4; i++) data.g_roll[i] = data_que[i+4];
+    
+    data.g_pitch = (char *) &IMU.g_pitch;
+    for(i=0; i<4; i++) data.g_pitch[i] = data_que[i+8];
+    
+    data.g_yaw = (char *) &IMU.g_yaw;
+    for(i=0; i<4; i++) data.g_yaw[i] = data_que[i+12];        
+            
+    data.acc_x = (char *) &IMU.acc_x;
+    for(i=0; i<4; i++) data.acc_x[i] = data_que[i+16];
+    
+    data.acc_y = (char *) &IMU.acc_y;
+    for(i=0; i<4; i++) data.acc_y[i] = data_que[i+20];
+    
+    data.acc_z = (char *) &IMU.acc_z;
+    for(i=0; i<4; i++) data.acc_z[i] = data_que[i+24]; 
+    
+    IMU_Update();
+}
+
+void Checksum(){
+    char checksum = 0;
+    for(char i=1; i<40; i++) checksum = checksum ^ data_que[i];
+    if(checksum == data_que[40]) ahrs_parsing();
+}    
+
+void ahrs_rec(){      
+    unsigned char data;
+    data = ahrs.getc();
+    checkhead[0] = checkhead[1];
+    checkhead[1] = data;
+    if(checkhead[0] == 0xFF && checkhead[1] == 0xFF) {
+        imu_cnt = 0;
+        data_flag = 1;
+    }
+    if(data_flag == 1){
+        data_que[imu_cnt] = data;
+        imu_cnt++;
+    }
+    if(imu_cnt == 41){
+        Checksum();
+        data_flag = 2;
+    }    
+}
+
+
+
+
+    
+    
+    
+    
+        
+       
\ No newline at end of file