Diff: ahrs.h

Revision:

0:7ccd56e1c3b1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ahrs.h	Wed Aug 11 01:23:42 2010 +0000
@@ -0,0 +1,72 @@
+#define Ahrs_num 40
+#define start 0
+#define head1 0xFF
+#define head2 0xFF
+#define check1 1
+#define check2 2
+
+unsigned char buf[1000], head[2], q[41], ck[1], data_idx = 0;
+char AHRS[Ahrs_num], check_ahrs = start, checkdata = 0, checksum;
+
+
+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;
+} 
+        
+
+void Get_AHRS(char *data){
+    int i;
+    char *dTemp = (char *) &IMU;
+    
+    for(i=0; i<Ahrs_num; i++) *(dTemp+i) = *(data+i);
+    IMU_Update();
+}   
+
+void ahrs_data(unsigned char data){
+    
+    if(check_ahrs == start){
+        if(data == head1) check_ahrs = check1;
+        else check_ahrs = start;
+        }
+    
+    else if(check_ahrs == check1){
+        if(data == head2) check_ahrs = check2;
+        else check_ahrs = start;
+        }
+    
+    else if(check_ahrs == check2){
+        AHRS[data_idx] = data;
+        if(data_idx<(Ahrs_num-1)){                  
+             //checkdata ^= AHRS[data_idx];
+             data_idx++;
+             if(data_idx == 3) data_idx = 4;
+        }
+        else{
+            checksum = data;
+            //if(checksum == checkdata)
+            Get_AHRS(AHRS);                        
+            data_idx = 0;
+            check_ahrs = start;
+        }                  
+    }
+    
+    else{
+        data_idx = 0;
+        check_ahrs = start;
+    }
+}  
+
+void ahrs_rec(){      
+    unsigned char data;
+    data = ahrs.getc();
+    ahrs_data(data);
+    //pc.putc(data);
+}
\ No newline at end of file