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