2gcs.h

00001 #define FCC_DATA_SIZE 52  //39 Direct from IMU
00002 #define GPS_DATA_SIZE 20
00003 
00004 char lati[6], altig[6];
00005 
00006 float llt =  127.1234; //POSRoll;//dlat;//  gps.lat; gps.lat =
00007 float lng = 37.4321; //POSPitch; //dlon;//  gps.lon; gps.lon =
00008 
00009 void IMUtoGCS(){ 
00010     
00011     char *FtoG = (char *)&fcc, i;   //&IMU
00012      
00013     radio.putc(0xFF);
00014     radio.putc(0xFF);    
00015     
00016     fcc.acc_y = roll_con; // llt;
00017     fcc.acc_z =  pitch_con;// lng;
00018     
00019     if(gps.lat>0) fcc.acc_x = 1; else fcc.acc_x = 0;
00020     
00021    // fcc.IR2 = (unsigned short)F_range;
00022     //fcc.IR1 =  F_error;
00023     
00024     llt += 0.0001;
00025     if(llt > 128.00) llt = 127.1234;
00026     lng += 0.0001;
00027     if(lng > 38.00) lng = 37.4321;
00028     
00029 
00030     
00031     for (i=0;i<FCC_DATA_SIZE;i++)  radio.putc(*FtoG++);
00032     radio.putc(0x0D);
00033     return;
00034 }    
00035 
00036 void GPStoGCS(){ 
00037     
00038     char *FtoG = (char *)&gps, i;   //&IMU
00039      
00040     radio.putc(0xFE);
00041     radio.putc(0xFE);    
00042     
00043     for (i=0;i<GPS_DATA_SIZE;i++)  radio.putc(*FtoG++);
00044     radio.putc(0x0D);
00045     return;
00046 }
00047 
00048 void GPSASCII(){    
00049   
00050     radio.printf("%f ", dlat);
00051     radio.printf("%f ", dlon);
00052     //radio.printf("%d ", FMD.lock);
00053     radio.printf("%f ", gps.lat);
00054   //  radio.printf("%d\n", FMD.mode);
00055    radio.printf("%f ", gps.lon);
00056    /// radio.printf("%d\n", FMD.mission);
00057    radio.printf("%f\n ", gps.alti);
00058 
00059 
00060 
00061 //radio.printf("%f ", IMU.roll);
00062 //radio.printf("%f ", IMU.pitch);
00063 //radio.printf("%f\n", IMU.yaw);
00064 
00065     // radio.printf("%f", gps.dist);
00066     //radio.printf(" ");
00067     //radio.printf("%f", gps.bear);
00068     //radio.printf("\n");  
00069 }
00070 
00071 void toBHG(){ 
00072     
00073     char *FtoG = (char *)&fcc, i;   //&IMU
00074      
00075     pc.putc(0xFF);
00076     pc.putc(0xFF);
00077     for (i=0;i<FCC_DATA_SIZE;i++)  pc.putc(*FtoG++);
00078     pc.putc(0x0D);
00079 }
00080 
00081 void rcprint(){
00082         
00083     printf("%d\t", RC.rolls);
00084     printf("%d\t", RC.throttles);
00085     printf("%d\t", RC.pitchs);
00086     printf("%d\t", RC.yaws);
00087     printf("%d\n", RC.sws);
00088     return;
00089 }
00090 
00091 void imuprint(){
00092     printf("%d\t", (int)fcc.roll);
00093     printf("%d\t", (int)fcc.pitch);
00094     printf("%d\n", (int)fcc.yaw);
00095     return;
00096 }    
00097 
00098 /*void motprint(){
00099     printf("%d\t", bl_F[1]);
00100     printf("%d\t", bl_B[1]);
00101     printf("%d\t", bl_R[1]);
00102     printf("%d\n", bl_L[1]);
00103     return;
00104 }    
00105     
00106 void xmotprint(){
00107     printf("%d\t", front_bl);
00108     printf("%d\t", back_bl);
00109     printf("%d\t", right_bl);
00110     printf("%d\n", left_bl);
00111     return;
00112 }*/      
00113 
00114 void pidprint(){
00115     printf("%d\t", alti_con);
00116     printf("%d\t", roll_con);
00117     printf("%d\t", pitch_con);
00118     printf("%d\n", yaw_con);
00119     return;
00120 }    
00121 
00122 void gainprint(){
00123     printf("%d\t", (int)gainRoll.p);
00124     printf("%d\t", (int)gainRoll.i);
00125     printf("%d\t", (int)gainRoll.d);
00126     printf("%d\n", (int)gainRoll.r);                    
00127     return;
00128 }  
00129 
00130 
00131 void yawprint(){
00132         
00133     printf("%d\t", RC.yaws);
00134     printf("%d\t", (short)fcc.yaw);
00135     printf("%d\n", (short)Yaw_ref);
00136     return;
00137 }