File content as of revision 0:d463d5c04541:
#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;
}