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;
+}