Diff: 2gcs.h

Revision:

0:d463d5c04541

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/2gcs.h	Tue Aug 16 05:32:33 2011 +0000
@@ -0,0 +1,137 @@
+#define FCC_DATA_SIZE 52  //39 Direct from IMU
+#define GPS_DATA_SIZE 20
+
+char lati[6], altig[6];
+
+float llt =  127.1234; //POSRoll;//dlat;//  gps.lat; gps.lat =
+float lng = 37.4321; //POSPitch; //dlon;//  gps.lon; gps.lon =
+
+void IMUtoGCS(){ 
+    
+    char *FtoG = (char *)&fcc, i;   //&IMU
+     
+    radio.putc(0xFF);
+    radio.putc(0xFF);    
+    
+    fcc.acc_y = roll_con; // llt;
+    fcc.acc_z =  pitch_con;// lng;
+    
+    if(gps.lat>0) fcc.acc_x = 1; else fcc.acc_x = 0;
+    
+   // fcc.IR2 = (unsigned short)F_range;
+    //fcc.IR1 =  F_error;
+    
+    llt += 0.0001;
+    if(llt > 128.00) llt = 127.1234;
+    lng += 0.0001;
+    if(lng > 38.00) lng = 37.4321;
+    
+
+    
+    for (i=0;i<FCC_DATA_SIZE;i++)  radio.putc(*FtoG++);
+    radio.putc(0x0D);
+    return;
+}    
+
+void GPStoGCS(){ 
+    
+    char *FtoG = (char *)&gps, i;   //&IMU
+     
+    radio.putc(0xFE);
+    radio.putc(0xFE);    
+    
+    for (i=0;i<GPS_DATA_SIZE;i++)  radio.putc(*FtoG++);
+    radio.putc(0x0D);
+    return;
+}
+
+void GPSASCII(){    
+  
+    radio.printf("%f ", dlat);
+    radio.printf("%f ", dlon);
+    //radio.printf("%d ", FMD.lock);
+    radio.printf("%f ", gps.lat);
+  //  radio.printf("%d\n", FMD.mode);
+   radio.printf("%f ", gps.lon);
+   /// radio.printf("%d\n", FMD.mission);
+   radio.printf("%f\n ", gps.alti);
+
+
+
+//radio.printf("%f ", IMU.roll);
+//radio.printf("%f ", IMU.pitch);
+//radio.printf("%f\n", IMU.yaw);
+
+    // radio.printf("%f", gps.dist);
+    //radio.printf(" ");
+    //radio.printf("%f", gps.bear);
+    //radio.printf("\n");  
+}
+
+void toBHG(){ 
+    
+    char *FtoG = (char *)&fcc, i;   //&IMU
+     
+    pc.putc(0xFF);
+    pc.putc(0xFF);
+    for (i=0;i<FCC_DATA_SIZE;i++)  pc.putc(*FtoG++);
+    pc.putc(0x0D);
+}
+
+void rcprint(){
+        
+    printf("%d\t", RC.rolls);
+    printf("%d\t", RC.throttles);
+    printf("%d\t", RC.pitchs);
+    printf("%d\t", RC.yaws);
+    printf("%d\n", RC.sws);
+    return;
+}
+
+void imuprint(){
+    printf("%d\t", (int)fcc.roll);
+    printf("%d\t", (int)fcc.pitch);
+    printf("%d\n", (int)fcc.yaw);
+    return;
+}    
+
+/*void motprint(){
+    printf("%d\t", bl_F[1]);
+    printf("%d\t", bl_B[1]);
+    printf("%d\t", bl_R[1]);
+    printf("%d\n", bl_L[1]);
+    return;
+}    
+    
+void xmotprint(){
+    printf("%d\t", front_bl);
+    printf("%d\t", back_bl);
+    printf("%d\t", right_bl);
+    printf("%d\n", left_bl);
+    return;
+}*/      
+
+void pidprint(){
+    printf("%d\t", alti_con);
+    printf("%d\t", roll_con);
+    printf("%d\t", pitch_con);
+    printf("%d\n", yaw_con);
+    return;
+}    
+
+void gainprint(){
+    printf("%d\t", (int)gainRoll.p);
+    printf("%d\t", (int)gainRoll.i);
+    printf("%d\t", (int)gainRoll.d);
+    printf("%d\n", (int)gainRoll.r);                    
+    return;
+}  
+
+
+void yawprint(){
+        
+    printf("%d\t", RC.yaws);
+    printf("%d\t", (short)fcc.yaw);
+    printf("%d\n", (short)Yaw_ref);
+    return;
+}