User | Revision | Line number | New contents of line |
agiembed |
0:d463d5c04541
|
1
|
#define FCC_DATA_SIZE 52 //39 Direct from IMU
|
agiembed |
0:d463d5c04541
|
2
|
#define GPS_DATA_SIZE 20
|
agiembed |
0:d463d5c04541
|
3
|
|
agiembed |
0:d463d5c04541
|
4
|
char lati[6], altig[6];
|
agiembed |
0:d463d5c04541
|
5
|
|
agiembed |
0:d463d5c04541
|
6
|
float llt = 127.1234; //POSRoll;//dlat;// gps.lat; gps.lat =
|
agiembed |
0:d463d5c04541
|
7
|
float lng = 37.4321; //POSPitch; //dlon;// gps.lon; gps.lon =
|
agiembed |
0:d463d5c04541
|
8
|
|
agiembed |
0:d463d5c04541
|
9
|
void IMUtoGCS(){
|
agiembed |
0:d463d5c04541
|
10
|
|
agiembed |
0:d463d5c04541
|
11
|
char *FtoG = (char *)&fcc, i; //&IMU
|
agiembed |
0:d463d5c04541
|
12
|
|
agiembed |
0:d463d5c04541
|
13
|
radio.putc(0xFF);
|
agiembed |
0:d463d5c04541
|
14
|
radio.putc(0xFF);
|
agiembed |
0:d463d5c04541
|
15
|
|
agiembed |
0:d463d5c04541
|
16
|
fcc.acc_y = roll_con; // llt;
|
agiembed |
0:d463d5c04541
|
17
|
fcc.acc_z = pitch_con;// lng;
|
agiembed |
0:d463d5c04541
|
18
|
|
agiembed |
0:d463d5c04541
|
19
|
if(gps.lat>0) fcc.acc_x = 1; else fcc.acc_x = 0;
|
agiembed |
0:d463d5c04541
|
20
|
|
agiembed |
0:d463d5c04541
|
21
|
// fcc.IR2 = (unsigned short)F_range;
|
agiembed |
0:d463d5c04541
|
22
|
//fcc.IR1 = F_error;
|
agiembed |
0:d463d5c04541
|
23
|
|
agiembed |
0:d463d5c04541
|
24
|
llt += 0.0001;
|
agiembed |
0:d463d5c04541
|
25
|
if(llt > 128.00) llt = 127.1234;
|
agiembed |
0:d463d5c04541
|
26
|
lng += 0.0001;
|
agiembed |
0:d463d5c04541
|
27
|
if(lng > 38.00) lng = 37.4321;
|
agiembed |
0:d463d5c04541
|
28
|
|
agiembed |
0:d463d5c04541
|
29
|
|
agiembed |
0:d463d5c04541
|
30
|
|
agiembed |
0:d463d5c04541
|
31
|
for (i=0;i<FCC_DATA_SIZE;i++) radio.putc(*FtoG++);
|
agiembed |
0:d463d5c04541
|
32
|
radio.putc(0x0D);
|
agiembed |
0:d463d5c04541
|
33
|
return;
|
agiembed |
0:d463d5c04541
|
34
|
}
|
agiembed |
0:d463d5c04541
|
35
|
|
agiembed |
0:d463d5c04541
|
36
|
void GPStoGCS(){
|
agiembed |
0:d463d5c04541
|
37
|
|
agiembed |
0:d463d5c04541
|
38
|
char *FtoG = (char *)&gps, i; //&IMU
|
agiembed |
0:d463d5c04541
|
39
|
|
agiembed |
0:d463d5c04541
|
40
|
radio.putc(0xFE);
|
agiembed |
0:d463d5c04541
|
41
|
radio.putc(0xFE);
|
agiembed |
0:d463d5c04541
|
42
|
|
agiembed |
0:d463d5c04541
|
43
|
for (i=0;i<GPS_DATA_SIZE;i++) radio.putc(*FtoG++);
|
agiembed |
0:d463d5c04541
|
44
|
radio.putc(0x0D);
|
agiembed |
0:d463d5c04541
|
45
|
return;
|
agiembed |
0:d463d5c04541
|
46
|
}
|
agiembed |
0:d463d5c04541
|
47
|
|
agiembed |
0:d463d5c04541
|
48
|
void GPSASCII(){
|
agiembed |
0:d463d5c04541
|
49
|
|
agiembed |
0:d463d5c04541
|
50
|
radio.printf("%f ", dlat);
|
agiembed |
0:d463d5c04541
|
51
|
radio.printf("%f ", dlon);
|
agiembed |
0:d463d5c04541
|
52
|
//radio.printf("%d ", FMD.lock);
|
agiembed |
0:d463d5c04541
|
53
|
radio.printf("%f ", gps.lat);
|
agiembed |
0:d463d5c04541
|
54
|
// radio.printf("%d\n", FMD.mode);
|
agiembed |
0:d463d5c04541
|
55
|
radio.printf("%f ", gps.lon);
|
agiembed |
0:d463d5c04541
|
56
|
/// radio.printf("%d\n", FMD.mission);
|
agiembed |
0:d463d5c04541
|
57
|
radio.printf("%f\n ", gps.alti);
|
agiembed |
0:d463d5c04541
|
58
|
|
agiembed |
0:d463d5c04541
|
59
|
|
agiembed |
0:d463d5c04541
|
60
|
|
agiembed |
0:d463d5c04541
|
61
|
//radio.printf("%f ", IMU.roll);
|
agiembed |
0:d463d5c04541
|
62
|
//radio.printf("%f ", IMU.pitch);
|
agiembed |
0:d463d5c04541
|
63
|
//radio.printf("%f\n", IMU.yaw);
|
agiembed |
0:d463d5c04541
|
64
|
|
agiembed |
0:d463d5c04541
|
65
|
// radio.printf("%f", gps.dist);
|
agiembed |
0:d463d5c04541
|
66
|
//radio.printf(" ");
|
agiembed |
0:d463d5c04541
|
67
|
//radio.printf("%f", gps.bear);
|
agiembed |
0:d463d5c04541
|
68
|
//radio.printf("\n");
|
agiembed |
0:d463d5c04541
|
69
|
}
|
agiembed |
0:d463d5c04541
|
70
|
|
agiembed |
0:d463d5c04541
|
71
|
void toBHG(){
|
agiembed |
0:d463d5c04541
|
72
|
|
agiembed |
0:d463d5c04541
|
73
|
char *FtoG = (char *)&fcc, i; //&IMU
|
agiembed |
0:d463d5c04541
|
74
|
|
agiembed |
0:d463d5c04541
|
75
|
pc.putc(0xFF);
|
agiembed |
0:d463d5c04541
|
76
|
pc.putc(0xFF);
|
agiembed |
0:d463d5c04541
|
77
|
for (i=0;i<FCC_DATA_SIZE;i++) pc.putc(*FtoG++);
|
agiembed |
0:d463d5c04541
|
78
|
pc.putc(0x0D);
|
agiembed |
0:d463d5c04541
|
79
|
}
|
agiembed |
0:d463d5c04541
|
80
|
|
agiembed |
0:d463d5c04541
|
81
|
void rcprint(){
|
agiembed |
0:d463d5c04541
|
82
|
|
agiembed |
0:d463d5c04541
|
83
|
printf("%d\t", RC.rolls);
|
agiembed |
0:d463d5c04541
|
84
|
printf("%d\t", RC.throttles);
|
agiembed |
0:d463d5c04541
|
85
|
printf("%d\t", RC.pitchs);
|
agiembed |
0:d463d5c04541
|
86
|
printf("%d\t", RC.yaws);
|
agiembed |
0:d463d5c04541
|
87
|
printf("%d\n", RC.sws);
|
agiembed |
0:d463d5c04541
|
88
|
return;
|
agiembed |
0:d463d5c04541
|
89
|
}
|
agiembed |
0:d463d5c04541
|
90
|
|
agiembed |
0:d463d5c04541
|
91
|
void imuprint(){
|
agiembed |
0:d463d5c04541
|
92
|
printf("%d\t", (int)fcc.roll);
|
agiembed |
0:d463d5c04541
|
93
|
printf("%d\t", (int)fcc.pitch);
|
agiembed |
0:d463d5c04541
|
94
|
printf("%d\n", (int)fcc.yaw);
|
agiembed |
0:d463d5c04541
|
95
|
return;
|
agiembed |
0:d463d5c04541
|
96
|
}
|
agiembed |
0:d463d5c04541
|
97
|
|
agiembed |
0:d463d5c04541
|
98
|
/*void motprint(){
|
agiembed |
0:d463d5c04541
|
99
|
printf("%d\t", bl_F[1]);
|
agiembed |
0:d463d5c04541
|
100
|
printf("%d\t", bl_B[1]);
|
agiembed |
0:d463d5c04541
|
101
|
printf("%d\t", bl_R[1]);
|
agiembed |
0:d463d5c04541
|
102
|
printf("%d\n", bl_L[1]);
|
agiembed |
0:d463d5c04541
|
103
|
return;
|
agiembed |
0:d463d5c04541
|
104
|
}
|
agiembed |
0:d463d5c04541
|
105
|
|
agiembed |
0:d463d5c04541
|
106
|
void xmotprint(){
|
agiembed |
0:d463d5c04541
|
107
|
printf("%d\t", front_bl);
|
agiembed |
0:d463d5c04541
|
108
|
printf("%d\t", back_bl);
|
agiembed |
0:d463d5c04541
|
109
|
printf("%d\t", right_bl);
|
agiembed |
0:d463d5c04541
|
110
|
printf("%d\n", left_bl);
|
agiembed |
0:d463d5c04541
|
111
|
return;
|
agiembed |
0:d463d5c04541
|
112
|
}*/
|
agiembed |
0:d463d5c04541
|
113
|
|
agiembed |
0:d463d5c04541
|
114
|
void pidprint(){
|
agiembed |
0:d463d5c04541
|
115
|
printf("%d\t", alti_con);
|
agiembed |
0:d463d5c04541
|
116
|
printf("%d\t", roll_con);
|
agiembed |
0:d463d5c04541
|
117
|
printf("%d\t", pitch_con);
|
agiembed |
0:d463d5c04541
|
118
|
printf("%d\n", yaw_con);
|
agiembed |
0:d463d5c04541
|
119
|
return;
|
agiembed |
0:d463d5c04541
|
120
|
}
|
agiembed |
0:d463d5c04541
|
121
|
|
agiembed |
0:d463d5c04541
|
122
|
void gainprint(){
|
agiembed |
0:d463d5c04541
|
123
|
printf("%d\t", (int)gainRoll.p);
|
agiembed |
0:d463d5c04541
|
124
|
printf("%d\t", (int)gainRoll.i);
|
agiembed |
0:d463d5c04541
|
125
|
printf("%d\t", (int)gainRoll.d);
|
agiembed |
0:d463d5c04541
|
126
|
printf("%d\n", (int)gainRoll.r);
|
agiembed |
0:d463d5c04541
|
127
|
return;
|
agiembed |
0:d463d5c04541
|
128
|
}
|
agiembed |
0:d463d5c04541
|
129
|
|
agiembed |
0:d463d5c04541
|
130
|
|
agiembed |
0:d463d5c04541
|
131
|
void yawprint(){
|
agiembed |
0:d463d5c04541
|
132
|
|
agiembed |
0:d463d5c04541
|
133
|
printf("%d\t", RC.yaws);
|
agiembed |
0:d463d5c04541
|
134
|
printf("%d\t", (short)fcc.yaw);
|
agiembed |
0:d463d5c04541
|
135
|
printf("%d\n", (short)Yaw_ref);
|
agiembed |
0:d463d5c04541
|
136
|
return;
|
agiembed |
0:d463d5c04541
|
137
|
}
|