Rev 839 | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 839 | Rev 966 | ||
---|---|---|---|
Line 12... | Line 12... | ||
12 | unless it is stated otherwise. |
12 | unless it is stated otherwise. |
13 | */ |
13 | */ |
Line 14... | Line 14... | ||
14 | 14 | ||
15 | #include "main.h" |
15 | #include "main.h" |
- | 16 | #include "kafi.h" |
|
- | 17 | #include "mymath.h" |
|
- | 18 | ||
- | 19 | #define sin45 -0.707106 |
|
Line 16... | Line 20... | ||
16 | #include "kafi.h" |
20 | #define cos45 0.707106 |
17 | 21 | ||
18 | extern void UART2Print(void); |
22 | extern void UART2Print(void); |
Line 29... | Line 33... | ||
29 | extern int nick_gain_p, nick_gain_d, roll_gain_p, roll_gain_d; |
33 | extern int nick_gain_p, nick_gain_d, roll_gain_p, roll_gain_d; |
30 | extern int Override, TargetGier, DeltaAltitude, Theta45, Phi45; |
34 | extern int Override, TargetGier, DeltaAltitude, Theta45, Phi45; |
31 | extern f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P; |
35 | extern f32_t sinPhi_P, cosPhi_P, sinTheta_P, cosTheta_P; |
32 | extern unsigned long maxDistance; |
36 | extern unsigned long maxDistance; |
Line -... | Line 37... | ||
- | 37 | ||
- | 38 | int Theta45; |
|
- | 39 | int Phi45; |
|
33 | 40 | ||
34 | unsigned char UART2PrintAbgeschlossen = 1; |
41 | unsigned char UART2PrintAbgeschlossen = 1; |
Line 35... | Line 42... | ||
35 | void UART2Print(void); |
42 | void UART2Print(void); |
36 | 43 | ||
Line 194... | Line 201... | ||
194 | void SendOSD() |
201 | void SendOSD() |
195 | { |
202 | { |
196 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
203 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
Line 197... | Line 204... | ||
197 | 204 | ||
198 | /*--- VARIABLES ---*/ |
205 | /*--- VARIABLES ---*/ |
199 | //static int dx, dy, x1,y1,x2,y2; |
206 | static int dx, dy, x1,y1,x2,y2; |
200 | //f32_t sinPhi_P_cosTheta_P; |
207 | f32_t sinPhi_P_cosTheta_P; |
201 | //static int x1_old, y1_old, x2_old, y2_old; |
- | |
202 | 208 | static int x1_old, y1_old, x2_old, y2_old; |
|
Line 203... | Line 209... | ||
203 | static int iState = 0; |
209 | static int iState = 0; |
204 | 210 | ||
205 | switch (iState) |
211 | switch (iState) |
Line 223... | Line 229... | ||
223 | OSD_printf ("\33[18;20HAGL:-.-\33[17;20HBar:%03d", CurrentAltitude); |
229 | OSD_printf ("\33[18;20HAGL:-.-\33[17;20HBar:%03d", CurrentAltitude); |
224 | } |
230 | } |
225 | iState++; |
231 | iState++; |
226 | break; |
232 | break; |
227 | case 3: /* Draw an artificial horizon. Part 1 */ |
233 | case 3: /* Draw an artificial horizon. Part 1 */ |
228 | #if 0 |
234 | #if 1 |
229 | sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P; |
235 | sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P; |
230 | Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F)); |
236 | 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); |
237 | 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; |
238 | dx = c_cos_8192(Phi45) / 128; |
233 | dy = c_sin_8192(Phi45) / 128; |
239 | dy = c_sin_8192(Phi45) / 128; |
Line 240... | Line 246... | ||
240 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old); |
246 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old); |
241 | #endif |
247 | #endif |
242 | iState++; |
248 | iState++; |
243 | break; |
249 | break; |
244 | case 4: /* Draw an artificial horizon. Part 2 */ |
250 | case 4: /* Draw an artificial horizon. Part 2 */ |
245 | #if 0 |
251 | #if 1 |
246 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2); |
252 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2); |
247 | UART2Print(); |
253 | UART2Print(); |
248 | x1_old = x1; |
254 | x1_old = x1; |
249 | y1_old = y1; |
255 | y1_old = y1; |
250 | x2_old = x2; |
256 | x2_old = x2; |
Line 270... | Line 276... | ||
270 | } |
276 | } |
271 | else |
277 | else |
272 | { |
278 | { |
273 | OSD_printf ("\33[1;20HDst:-.-"); |
279 | OSD_printf ("\33[1;20HDst:-.-"); |
274 | } |
280 | } |
- | 281 | iState = 8; |
|
- | 282 | break; |
|
- | 283 | case 7: |
|
- | 284 | OSD_printf ("\33[4;0HN:%03d\33[5;0HR:%03d", status.iTheta10 / 10, status.iPhi10 / 10); |
|
275 | iState++; |
285 | iState++; |
276 | break; |
286 | break; |
277 | case 7: /* Draw an artificial horizon. Part 1 */ |
287 | case 8: /* Draw an artificial horizon. Part 1 */ |
278 | #if 0 |
288 | #if 1 |
279 | sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P; |
289 | sinPhi_P_cosTheta_P = sinPhi_P * cosTheta_P; |
280 | Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F)); |
290 | 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); |
291 | 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; |
292 | dx = c_cos_8192(Phi45) / 128; |
283 | dy = c_sin_8192(Phi45) / 128; |
293 | dy = c_sin_8192(Phi45) / 128; |
Line 289... | Line 299... | ||
289 | OSDPtr = 0; |
299 | OSDPtr = 0; |
290 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old); |
300 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old); |
291 | #endif |
301 | #endif |
292 | iState++; |
302 | iState++; |
293 | break; |
303 | break; |
294 | case 8: /* Draw an artificial horizon. Part 2 */ |
304 | case 9: /* Draw an artificial horizon. Part 2 */ |
295 | #if 0 |
305 | #if 1 |
296 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2); |
306 | OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2); |
297 | UART2Print(); |
307 | UART2Print(); |
Line 298... | Line 308... | ||
298 | 308 | ||
299 | x1_old = x1; |
309 | x1_old = x1; |
300 | y1_old = y1; |
310 | y1_old = y1; |
301 | x2_old = x2; |
311 | x2_old = x2; |
302 | y2_old = y2; |
312 | y2_old = y2; |
303 | #endif |
313 | #endif |
304 | iState++; |
314 | iState++; |
305 | break; |
315 | break; |
306 | case 9: /* Display the GPS control outputs */ |
316 | case 10: /* 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); |
317 | 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++; |
318 | iState++; |
309 | break; |
319 | break; |
310 | case 10: |
320 | case 11: |
311 | OSD_printf ("\33[16;20HClb:%03d", DeltaAltitude); |
321 | OSD_printf ("\33[16;20HClb:%03d", DeltaAltitude); |
312 | iState++; |
322 | iState++; |
313 | break; |
323 | break; |
314 | case 11:/* Draw a ^ to indicate the target direction */ |
324 | case 12:/* Draw a ^ to indicate the target direction */ |
315 | { |
325 | { |
316 | static int LastGierOffset = 0; |
326 | static int LastGierOffset = 0; |
317 | int GierOffset = (TargetGier - status.iPsi10) / 10; |
327 | int GierOffset = (TargetGier - status.iPsi10) / 10; |
318 | if (GierOffset > 180) |
328 | if (GierOffset > 180) |
Line 325... | Line 335... | ||
325 | } |
335 | } |
326 | GierOffset /= 14; |
336 | GierOffset /= 14; |
Line 327... | Line 337... | ||
327 | 337 | ||
328 | OSD_printf ("\33[2;%dH \33[2;%dH^", 14 + LastGierOffset,14 + GierOffset); |
338 | OSD_printf ("\33[2;%dH \33[2;%dH^", 14 + LastGierOffset,14 + GierOffset); |
329 | LastGierOffset = GierOffset; |
339 | LastGierOffset = GierOffset; |
330 | iState++; |
340 | iState = 14; |
331 | break; |
341 | break; |
332 | } |
342 | } |
333 | case 12: /* Display the GPS_Nick and GPS_Roll / StickNick and StickRoll */ |
343 | case 13: /* Display the GPS_Nick and GPS_Roll / StickNick and StickRoll */ |
334 | if (Override == 0) |
344 | if (Override == 0) |
335 | { |
345 | { |
336 | OSD_printf ("\33[18;0HGN:%03d GR:%03d ", GPS_Nick, GPS_Roll); |
346 | OSD_printf ("\33[18;0HGN:%03d GR:%03d ", GPS_Nick, GPS_Roll); |
337 | } |
347 | } |
338 | else |
348 | else |
339 | { |
349 | { |
340 | OSD_printf ("\33[18;0HSN:%03d SR:%03d ", StickNick, StickRoll); |
350 | OSD_printf ("\33[18;0HSN:%03d SR:%03d ", StickNick, StickRoll); |
341 | } |
351 | } |
342 | iState++; |
352 | iState++; |
343 | break; |
353 | break; |
344 | case 13: |
354 | case 14: |
345 | #if 0 /* Display the Horizon */ |
355 | #if 1/* Display the Horizon */ |
346 | OSD_printf ("\33[156;120.r\33[204;120+r\33[/r "); |
356 | OSD_printf ("\33[156;120.r\33[204;120+r\33[/r "); |
347 | #endif |
357 | #endif |
348 | iState=0; |
358 | iState=0; |
349 | break; |
359 | break; |