Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
836 | MikeW | 1 | /* |
2 | Copyright 2008, by Michael Walter |
||
3 | |||
4 | All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser |
||
5 | General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but |
||
6 | WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||
7 | See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public |
||
8 | License along with this program. If not, see <http://www.gnu.org/licenses/>. |
||
9 | |||
10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
||
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
||
12 | unless it is stated otherwise. |
||
13 | */ |
||
14 | |||
15 | #include "main.h" |
||
16 | #include "kafi.h" |
||
17 | |||
18 | extern void UART2Print(void); |
||
19 | void InitOSD(void); |
||
20 | void SendOSD(void); |
||
21 | |||
22 | extern gpsInfo_t actualPos; // measured position (last gps record) |
||
23 | extern gpsInfo_t targetPos; // measured position (last gps record) |
||
24 | |||
25 | extern signed int GPS_Nick; |
||
26 | extern signed int GPS_Roll; |
||
27 | |||
28 | extern int CurrentAltitude, LastAltitude, targetPosValid, RCQuality; |
||
29 | extern int nick_gain_p, nick_gain_d, roll_gain_p, roll_gain_d; |
||
30 | extern int Override, TargetGier, DeltaAltitude, Theta45, Phi45; |
||
31 | extern f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P; |
||
32 | extern unsigned long maxDistance; |
||
33 | |||
34 | unsigned char UART2PrintAbgeschlossen = 1; |
||
35 | void UART2Print(void); |
||
36 | |||
37 | char DisplayBuff[80] = ""; |
||
38 | unsigned char DispPtr = 0; |
||
39 | |||
40 | char OSDBuff[80] = ""; |
||
41 | unsigned char OSDPtr = 0; |
||
42 | |||
43 | |||
44 | #define OSD_printf(format, args...) { OSDPtr = 0; _printf_P(OUT_OSD,PSTR(format) , ## args);UART2Print();} |
||
45 | #define OSD_printf_(format, args...) { _printf_P(OUT_OSD,PSTR(format) , ## args);} |
||
46 | |||
47 | |||
48 | /* For OSD_printf to work, Putchar() has to be extendet as follows. |
||
49 | char Putchar(char zeichen) |
||
50 | { |
||
51 | if(PrintZiel == OUT_LCD) |
||
52 | { |
||
53 | DisplayBuff[DispPtr++] = zeichen; return(1); |
||
54 | } |
||
55 | else if (PrintZiel == OUT_OSD) |
||
56 | { |
||
57 | OSDBuff[OSDPtr++] = zeichen; return(1); |
||
58 | } |
||
59 | else |
||
60 | { |
||
61 | return(uart_putchar(zeichen)); |
||
62 | } |
||
63 | } |
||
64 | */ |
||
65 | |||
66 | |||
67 | /* **************************************************************************** |
||
68 | Functionname: OsdClear */ /*! |
||
69 | Description: |
||
70 | |||
71 | @param[in] |
||
72 | |||
73 | @return void |
||
74 | @pre - |
||
75 | @post - |
||
76 | @author Michael Walter |
||
77 | **************************************************************************** */ |
||
78 | void OsdClear(void) |
||
79 | { |
||
80 | unsigned char i; |
||
81 | for(i=0;i<80;i++) OSDBuff[i] = ' '; |
||
82 | } |
||
83 | |||
84 | /* **************************************************************************** |
||
85 | Functionname: LcdClear */ /*! |
||
86 | Description: |
||
87 | |||
88 | @param[in] |
||
89 | |||
90 | @return void |
||
91 | @pre - |
||
92 | @post - |
||
93 | @author |
||
94 | **************************************************************************** */ |
||
95 | void LcdClear(void) |
||
96 | { |
||
97 | unsigned char i; |
||
98 | for(i=0;i<80;i++) DisplayBuff[i] = ' '; |
||
99 | } |
||
100 | |||
101 | |||
102 | /* **************************************************************************** |
||
103 | Functionname: InitOSD */ /*! |
||
104 | Description: Init the Bob-4H OSD |
||
105 | |||
106 | @param[in] |
||
107 | |||
108 | @return void |
||
109 | @pre - |
||
110 | @post - |
||
111 | @author Michael Walter |
||
112 | **************************************************************************** */ |
||
113 | void InitOSD() |
||
114 | { |
||
115 | /* Clear OSD */ |
||
116 | OSD_printf ("\33[2J"); |
||
117 | Delay_ms_Mess(300); |
||
118 | OSD_printf ("\33[2J"); |
||
119 | |||
120 | /* Set OSD Pixel Clock */ |
||
121 | Delay_ms_Mess(300); |
||
122 | OSD_printf ("\33[23;6v"); |
||
123 | |||
124 | /* Set OSD Pixel Width */ |
||
125 | Delay_ms_Mess(300); |
||
126 | OSD_printf ("\33[25;448v"); |
||
127 | |||
128 | /* Set OSD Font */ |
||
129 | Delay_ms_Mess(300); |
||
130 | OSD_printf ("\33[0z"); |
||
131 | } |
||
132 | |||
133 | |||
134 | |||
135 | /* **************************************************************************** |
||
136 | Functionname: UART2Print */ /*! |
||
137 | Description: Send OSD Data to the Bob-4H OSD |
||
138 | |||
139 | @return void |
||
140 | @pre - |
||
141 | @post - |
||
142 | @author Michael Walter |
||
143 | **************************************************************************** */ |
||
144 | void UART2Print() |
||
145 | { |
||
146 | OSDBuff[OSDPtr] = '\r'; |
||
147 | if (UART2PrintAbgeschlossen == 1) |
||
148 | { |
||
149 | UART2PrintAbgeschlossen = 0; |
||
150 | UDR1 = OSDBuff[0]; |
||
151 | } |
||
152 | } |
||
153 | |||
154 | /* **************************************************************************** |
||
155 | Functionname: SIGNAL */ /*! |
||
156 | Description: Send OSD Data to the Bob-4H OSD |
||
157 | |||
158 | @return void |
||
159 | @pre - |
||
160 | @post - |
||
161 | @author Michael Walter |
||
162 | **************************************************************************** */ |
||
163 | SIGNAL(SIG_USART1_TRANS) |
||
164 | { |
||
165 | static unsigned int ptr = 0; |
||
166 | |||
167 | unsigned char tmp_tx; |
||
168 | if(!UART2PrintAbgeschlossen) |
||
169 | { |
||
170 | ptr++; // die [0] wurde schon gesendet |
||
171 | tmp_tx = OSDBuff[ptr]; |
||
172 | if((tmp_tx == '\r') /* tmp_tx == 0 */) |
||
173 | { |
||
174 | ptr = 0; |
||
175 | UART2PrintAbgeschlossen = 1; |
||
176 | } |
||
177 | UDR1 = tmp_tx; |
||
178 | } |
||
179 | else ptr = 0; |
||
180 | } |
||
181 | |||
182 | |||
183 | /* **************************************************************************** |
||
184 | Functionname: SendOSD */ /*! |
||
185 | Description: Display OnScreen Display Data on a Bob-4H |
||
186 | |||
187 | @param[in] |
||
188 | |||
189 | @return void |
||
190 | @pre - |
||
191 | @post - |
||
192 | @author Michael Walter |
||
193 | **************************************************************************** */ |
||
194 | void SendOSD() |
||
195 | { |
||
196 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
||
197 | |||
198 | /*--- VARIABLES ---*/ |
||
199 | //static int dx, dy, x1,y1,x2,y2; |
||
200 | //f32_t sinPhi_P_cosTheta_P; |
||
201 | //static int x1_old, y1_old, x2_old, y2_old; |
||
202 | |||
203 | static int iState = 0; |
||
204 | |||
205 | switch (iState) |
||
206 | { |
||
207 | case 0: |
||
208 | OSD_printf ("\33[0;11HHDG:%03d", status.iPsi10 / 10); |
||
209 | iState++; |
||
210 | break; |
||
211 | case 1: /* Display the battery voltage an the RC signal level */ |
||
212 | OSD_printf ("\33[0;0HU:%03d\33[1;0HR:%03d", UBat, RCQuality); |
||
213 | iState++; |
||
214 | break; |
||
215 | case 2: /* Display the current altitude */ |
||
216 | if (targetPosValid == 1) |
||
217 | { |
||
218 | int DeltaGPSAltitude = (actualPos.altitude - targetPos.altitude); |
||
219 | OSD_printf ("\33[18;20HAGL:%03d\33[17;20HBar:%03d", DeltaGPSAltitude, CurrentAltitude); |
||
220 | } |
||
221 | else |
||
222 | { |
||
223 | OSD_printf ("\33[18;20HAGL:-.-\33[17;20HBar:%03d", CurrentAltitude); |
||
224 | } |
||
225 | iState++; |
||
226 | break; |
||
227 | case 3: /* Draw an artificial horizon. Part 1 */ |
||
228 | #if 0 |
||
229 | sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P; |
||
230 | Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F)); |
||
231 | Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100); |
||
232 | dx = c_cos_8192(Phi45) / 128; |
||
233 | dy = c_sin_8192(Phi45) / 128; |
||
234 | x1 = 180 - dx; |
||
235 | y1 = 120 + 2 * Theta45 + dy; |
||
236 | x2 = 180 + dx; |
||
237 | y2 = y1 - 2 * dy; |
||
238 | |||
239 | OSDPtr = 0; |
||
240 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old); |
||
241 | #endif |
||
242 | iState++; |
||
243 | break; |
||
244 | case 4: /* Draw an artificial horizon. Part 2 */ |
||
245 | #if 0 |
||
246 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2); |
||
247 | UART2Print(); |
||
248 | x1_old = x1; |
||
249 | y1_old = y1; |
||
250 | x2_old = x2; |
||
251 | y2_old = y2; |
||
252 | #endif |
||
253 | iState++; |
||
254 | break; |
||
255 | case 5: /* Display velocity over ground */ |
||
256 | if (actualPos.state > 1) |
||
257 | { |
||
258 | OSD_printf ("\33[0;20HVel:%03d", ((actualPos.groundSpeed / 10) * 36) /100); |
||
259 | } |
||
260 | else |
||
261 | { |
||
262 | OSD_printf ("\33[0;20HVel:-.-"); |
||
263 | } |
||
264 | iState++; |
||
265 | break; |
||
266 | case 6: /* Display distance from target */ |
||
267 | if (targetPosValid == 1) |
||
268 | { |
||
269 | OSD_printf ("\33[1;20HDst:%03d", maxDistance / 10); |
||
270 | } |
||
271 | else |
||
272 | { |
||
273 | OSD_printf ("\33[1;20HDst:-.-"); |
||
274 | } |
||
275 | iState++; |
||
276 | break; |
||
277 | case 7: /* Draw an artificial horizon. Part 1 */ |
||
278 | #if 0 |
||
279 | sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P; |
||
280 | Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F)); |
||
281 | Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100); |
||
282 | dx = c_cos_8192(Phi45) / 128; |
||
283 | dy = c_sin_8192(Phi45) / 128; |
||
284 | x1 = 180 - dx; |
||
285 | y1 = 120 + 2 * Theta45 + dy; |
||
286 | x2 = 180 + dx; |
||
287 | y2 = y1 - 2 * dy; |
||
288 | |||
289 | OSDPtr = 0; |
||
290 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old); |
||
291 | #endif |
||
292 | iState++; |
||
293 | break; |
||
294 | case 8: /* Draw an artificial horizon. Part 2 */ |
||
295 | #if 0 |
||
296 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2); |
||
297 | UART2Print(); |
||
298 | |||
299 | x1_old = x1; |
||
300 | y1_old = y1; |
||
301 | x2_old = x2; |
||
302 | y2_old = y2; |
||
303 | #endif |
||
304 | iState++; |
||
305 | break; |
||
306 | case 9: /* Display the GPS control outputs */ |
||
307 | OSD_printf ("\33[16;0HNG:%03d EG:%03d \33[17;0HNV:%03d EV:%03d ", nick_gain_p, roll_gain_p, nick_gain_d, roll_gain_d); |
||
308 | iState++; |
||
309 | break; |
||
310 | case 10: |
||
311 | OSD_printf ("\33[16;20HClb:%03d", DeltaAltitude); |
||
312 | iState++; |
||
313 | break; |
||
314 | case 11:/* Draw a ^ to indicate the target direction */ |
||
315 | { |
||
316 | static int LastGierOffset = 0; |
||
317 | int GierOffset = (TargetGier - status.iPsi10) / 10; |
||
318 | if (GierOffset > 180) |
||
319 | { |
||
320 | GierOffset -= 360; |
||
321 | } |
||
322 | if (GierOffset < -180) |
||
323 | { |
||
324 | GierOffset += 360; |
||
325 | } |
||
326 | GierOffset /= 14; |
||
327 | |||
328 | OSD_printf ("\33[2;%dH \33[2;%dH^", 14 + LastGierOffset,14 + GierOffset); |
||
329 | LastGierOffset = GierOffset; |
||
330 | iState++; |
||
331 | break; |
||
332 | } |
||
333 | case 12: /* Display the GPS_Nick and GPS_Roll / StickNick and StickRoll */ |
||
334 | if (Override == 0) |
||
335 | { |
||
336 | OSD_printf ("\33[18;0HGN:%03d GR:%03d ", GPS_Nick, GPS_Roll); |
||
337 | } |
||
338 | else |
||
339 | { |
||
340 | OSD_printf ("\33[18;0HSN:%03d SR:%03d ", StickNick, StickRoll); |
||
341 | } |
||
342 | iState++; |
||
343 | break; |
||
344 | case 13: |
||
345 | #if 0 /* Display the Horizon */ |
||
346 | OSD_printf ("\33[156;120.r\33[204;120+r\33[/r "); |
||
347 | #endif |
||
348 | iState=0; |
||
349 | break; |
||
350 | default: |
||
351 | iState = 0; |
||
352 | } |
||
353 | } |
||
354 |