Rev 724 | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
724 | woggle | 1 | /***************************************************************************** |
2 | * Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net * |
||
3 | * based on the C-OSD code from CaScAdE * |
||
4 | * http://www.mylifesucks.de/oss/c-osd/ * |
||
5 | * * |
||
6 | * This program is free software; you can redistribute it and/or modify * |
||
7 | * it under the terms of the GNU General Public License as published by * |
||
8 | * the Free Software Foundation; either version 2 of the License. * |
||
9 | * * |
||
10 | * This program is distributed in the hope that it will be useful, * |
||
11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
||
12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
||
13 | * GNU General Public License for more details. * |
||
14 | * * |
||
15 | * You should have received a copy of the GNU General Public License * |
||
16 | * along with this program; if not, write to the * |
||
17 | * Free Software Foundation, Inc., * |
||
18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
||
19 | * * |
||
20 | *****************************************************************************/ |
||
21 | |||
22 | #include <avr/io.h> |
||
23 | #include <inttypes.h> |
||
24 | #include <stdlib.h> |
||
25 | #include <avr/pgmspace.h> |
||
26 | |||
27 | #include "main.h" |
||
28 | #include "osd.h" |
||
29 | #include "lcd.h" |
||
30 | #include "timer.h" |
||
31 | #include "usart.h" |
||
32 | |||
33 | #include "mk-data-structs.h" |
||
34 | |||
35 | #define COSD_WASFLYING 4 |
||
36 | |||
37 | /* ########################################################################## |
||
38 | * global definitions and global vars |
||
39 | * ##########################################################################*/ |
||
40 | NaviData_t *naviData; |
||
41 | |||
42 | // stats for after flight |
||
43 | int16_t max_Altimeter = 0; |
||
44 | uint16_t max_GroundSpeed = 0; |
||
45 | int16_t max_Distance = 0; |
||
46 | uint8_t min_UBat = 255; |
||
47 | uint16_t max_FlyingTime = 0; |
||
48 | |||
49 | // cache old vars for blinking attribute, checkup is faster than full |
||
50 | // attribute write each time |
||
51 | volatile uint8_t last_UBat = 255; |
||
52 | volatile uint8_t last_RC_Quality = 255; |
||
53 | |||
54 | volatile uint16_t ftimer = 0; |
||
55 | |||
56 | // store stats description in progmem to save space |
||
57 | const char stats_item_0[] PROGMEM = "max Altitude:"; |
||
58 | const char stats_item_1[] PROGMEM = "max Speed :"; |
||
59 | const char stats_item_2[] PROGMEM = "max Distance:"; |
||
60 | const char stats_item_3[] PROGMEM = "min Voltage :"; |
||
61 | const char stats_item_4[] PROGMEM = "max Time :"; |
||
62 | const char stats_item_5[] PROGMEM = "Long. :"; |
||
63 | const char stats_item_6[] PROGMEM = "Lat. :"; |
||
64 | const char *stats_item_pointers[] PROGMEM = { |
||
65 | stats_item_0, |
||
66 | stats_item_1, |
||
67 | stats_item_2, |
||
68 | stats_item_3, |
||
69 | stats_item_4, |
||
70 | stats_item_5, |
||
71 | stats_item_6 |
||
72 | }; |
||
73 | |||
74 | //char* rose = "-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W"; |
||
75 | const char rose[48] PROGMEM = { |
||
76 | 0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S', |
||
77 | 0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O', |
||
78 | 0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N', |
||
79 | 0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W'}; |
||
80 | // the center is char 19 (north), we add the current heading in 8th |
||
81 | // which would be 22.5 degrees, but float would bloat up the code |
||
82 | // and *10 / 225 would take ages... so we take the uncorrect way |
||
83 | |||
84 | |||
85 | const char str_NE[] PROGMEM = "NE"; |
||
86 | const char str_E[] PROGMEM = "E "; |
||
87 | const char str_SE[] PROGMEM = "SE"; |
||
88 | const char str_S[] PROGMEM = "S "; |
||
89 | const char str_SW[] PROGMEM = "SW"; |
||
90 | const char str_W[] PROGMEM = "W "; |
||
91 | const char str_NW[] PROGMEM = "NW"; |
||
92 | const char str_N[] PROGMEM = "N "; |
||
93 | const char *directions_p[8] PROGMEM = { |
||
94 | str_NE, |
||
95 | str_E, |
||
96 | str_SE, |
||
97 | str_S, |
||
98 | str_SW, |
||
99 | str_W, |
||
100 | str_NW, |
||
101 | str_N |
||
102 | }; |
||
103 | |||
104 | // Flags |
||
105 | uint8_t COSD_FLAGS2 = 0; |
||
106 | |||
107 | |||
108 | /** |
||
109 | * convert the <heading> gotton from NC into an index |
||
110 | */ |
||
111 | uint8_t heading_conv (uint16_t heading) |
||
112 | { |
||
113 | if (heading > 23 && heading < 68) |
||
114 | { |
||
115 | return 0; //direction = "NE"; |
||
116 | } |
||
117 | else if (heading > 67 && heading < 113) |
||
118 | { |
||
119 | return 1; //direction = "E "; |
||
120 | } |
||
121 | else if (heading > 112 && heading < 158) |
||
122 | { |
||
123 | return 2; //direction = "SE"; |
||
124 | } |
||
125 | else if (heading > 157 && heading < 203) |
||
126 | { |
||
127 | return 3; //direction = "S "; |
||
128 | } |
||
129 | else if (heading > 202 && heading < 248) |
||
130 | { |
||
131 | return 4; //direction = "SW"; |
||
132 | } |
||
133 | else if (heading > 247 && heading < 293) |
||
134 | { |
||
135 | return 5; //direction = "W "; |
||
136 | } |
||
137 | else if (heading > 292 && heading < 338) |
||
138 | { |
||
139 | return 6; //direction = "NW"; |
||
140 | } |
||
141 | return 7; //direction = "N "; |
||
142 | } |
||
143 | |||
144 | /** |
||
145 | * draw a compass rose at <x>/<y> for <heading> |
||
146 | */ |
||
147 | void draw_compass (uint8_t x, uint8_t y, uint16_t heading) |
||
148 | { |
||
149 | uint8_t front = 19 + (heading / 22); |
||
150 | for (uint8_t i = 0; i < 9; i++) |
||
151 | { |
||
152 | lcd_putc (x++, y, pgm_read_byte(&rose[front - 4 + i]), 0); |
||
153 | } |
||
154 | } |
||
155 | |||
156 | /* ########################################################################## |
||
157 | * variometer |
||
158 | * ##########################################################################*/ |
||
159 | /** |
||
160 | * draw variometer arrows at <x>/<y> according to <variometer> |
||
161 | */ |
||
162 | void draw_variometer (uint8_t x, uint8_t y, uint8_t width_x, uint8_t width_y, int16_t variometer) |
||
163 | { |
||
164 | lcd_rect (x, y - ((width_y - 1) / 2), width_x, width_y, 1); |
||
165 | lcd_frect (x + 1, y - ((width_y - 1) / 2) + 1, width_x - 2, width_y - 2, 0); |
||
166 | lcd_line (x, y, x + width_x, y, 1); |
||
167 | |||
168 | if (variometer > 0) |
||
169 | { // gain height |
||
170 | switch (variometer / 5) |
||
171 | { |
||
172 | case 0: |
||
173 | lcd_frect (x + 3, y - 1, 3, 1, 1); |
||
174 | break; |
||
175 | case 1: |
||
176 | lcd_frect (x + 2, y - 3, 5, 3, 1); |
||
177 | break; |
||
178 | case 2: |
||
179 | lcd_frect (x + 2, y - 4, 5, 4, 1); |
||
180 | break; |
||
181 | default: |
||
182 | lcd_frect (x + 1, y - 5, 7, 5, 1); |
||
183 | break; |
||
184 | } |
||
185 | } |
||
186 | else |
||
187 | { // sink |
||
188 | switch (variometer / -5) |
||
189 | { |
||
190 | case 0: |
||
191 | lcd_frect (x + 3, y, 3, 1, 1); |
||
192 | break; |
||
193 | case 1: |
||
194 | lcd_frect (x + 2, y, 5, 3, 1); |
||
195 | break; |
||
196 | case 2: |
||
197 | lcd_frect (x + 2, y, 5, 4, 1); |
||
198 | break; |
||
199 | default: |
||
200 | lcd_frect (x + 1, y, 7, 5, 1); |
||
201 | break; |
||
202 | } |
||
203 | } |
||
204 | } |
||
205 | |||
206 | |||
207 | #define TIMEOUT 200 // 2 sec |
||
208 | |||
209 | void print_statistics (void) |
||
210 | { |
||
211 | uint8_t line = 0; |
||
212 | lcd_cls (); |
||
213 | |||
214 | // max Altitude |
||
215 | lcd_printpns_at (0, line, stats_item_pointers[0], 0); |
||
216 | write_ndigit_number_s (13, line, max_Altimeter / 30, 4, 0); |
||
217 | lcd_putc (17, line, 'm', 0); |
||
218 | |||
219 | // max Speed |
||
220 | lcd_printpns_at (0, ++line, stats_item_pointers[1], 0); |
||
221 | write_ndigit_number_u (14, line, (uint16_t) (((uint32_t) max_GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0); |
||
222 | lcd_printpns_at(17, line, PSTR("km/h"), 0); |
||
223 | |||
224 | // max Distance |
||
225 | lcd_printpns_at (0, ++line, stats_item_pointers[2], 0); |
||
226 | write_ndigit_number_u (14, line, max_Distance / 10, 3, 0); |
||
227 | lcd_putc (17, line, 'm', 0); |
||
228 | |||
229 | // min voltage |
||
230 | lcd_printpns_at (0, ++line, stats_item_pointers[3], 0); |
||
231 | write_ndigit_number_u_10th (13, line, min_UBat, 3, 0); |
||
232 | lcd_putc (17, line, 'V', 0); |
||
233 | |||
234 | // max time |
||
235 | lcd_printpns_at (0, ++line, stats_item_pointers[4], 0); |
||
236 | write_time (13, line, max_FlyingTime); |
||
237 | |||
238 | // longitude |
||
239 | lcd_printpns_at (0, ++line, stats_item_pointers[5], 0); |
||
240 | write_gps_pos (8, line, naviData->CurrentPosition.Longitude); |
||
241 | |||
242 | // latitude |
||
243 | lcd_printpns_at (0, ++line, stats_item_pointers[6], 0); |
||
244 | write_gps_pos (8, line, naviData->CurrentPosition.Latitude); |
||
245 | |||
246 | while (!get_key_press (1 << KEY_ESC)) |
||
247 | timer = TIMEOUT; |
||
248 | COSD_FLAGS2 &= ~COSD_WASFLYING; |
||
249 | get_key_press(KEY_ALL); |
||
250 | lcd_cls(); |
||
251 | } |
||
252 | |||
253 | void osd (void) |
||
254 | { |
||
255 | uint8_t flag; |
||
256 | uint8_t tmp_dat; |
||
257 | |||
258 | // Clear statistics |
||
259 | max_Altimeter = 0; |
||
260 | max_GroundSpeed = 0; |
||
261 | max_Distance = 0; |
||
262 | min_UBat = 255; |
||
263 | max_FlyingTime = 0; |
||
264 | |||
265 | // flags from last round to check for changes |
||
266 | uint8_t old_MKFlags = 0; |
||
267 | |||
268 | uint16_t old_hh = 0; |
||
269 | |||
270 | lcd_cls(); |
||
271 | |||
272 | if (hardware == FC) |
||
273 | { |
||
274 | lcd_printp_at(0, 3, PSTR("Only with NC !"), 0); |
||
275 | timer = 100; |
||
276 | while (timer > 0); |
||
277 | } |
||
278 | |||
279 | SwitchToNC(); |
||
280 | |||
281 | mode = 'O'; |
||
282 | |||
283 | // request OSD Data from NC every 100ms |
||
284 | // RS232_request_mk_data (1, 'o', 100); |
||
285 | tmp_dat = 10; |
||
286 | SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
287 | |||
288 | // and disable debug... |
||
289 | // RS232_request_mk_data (0, 'd', 0); |
||
290 | tmp_dat = 0; |
||
291 | SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
||
292 | |||
293 | flag = 0; |
||
294 | timer = TIMEOUT; |
||
295 | |||
296 | do |
||
297 | { |
||
298 | if (rxd_buffer_locked) |
||
299 | { |
||
300 | timer = TIMEOUT; |
||
301 | Decode64 (); |
||
302 | naviData = (NaviData_t *) pRxData; |
||
303 | |||
304 | flag = 1; |
||
305 | |||
306 | if (naviData->MKFlags & FLAG_MOTOR_RUN) |
||
307 | { // should be engines running |
||
308 | // motors are on, assume we were/are flying |
||
309 | COSD_FLAGS2 |= COSD_WASFLYING; |
||
310 | } |
||
311 | else |
||
312 | { // stats |
||
313 | if ((COSD_FLAGS2 & COSD_WASFLYING) || (get_key_press (1 << KEY_ENTER))) |
||
314 | { |
||
315 | print_statistics (); |
||
316 | } |
||
317 | } |
||
318 | |||
319 | // lcd_printpns_at (0, 3, PSTR("012345678901234567890"), 0); |
||
320 | lcd_ecircle(22, 35, 16, 1); |
||
321 | |||
322 | // Ground Speed |
||
323 | write_ndigit_number_u (1, 0, (uint16_t) (((uint32_t) naviData->GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0); |
||
324 | lcd_printpns_at(4, 0, PSTR("km/h"), 0); |
||
325 | |||
326 | // Compass |
||
327 | write_ndigit_number_u (14, 0, naviData->CompassHeading, 3, 0); |
||
328 | lcd_putc (17, 0, 0x1E, 0); // degree symbol |
||
329 | lcd_printpns_at (18, 0, (const char *) (pgm_read_word ( &(directions_p[heading_conv(naviData->CompassHeading)]))), 0); |
||
330 | |||
331 | draw_compass (12, 1, naviData->CompassHeading); |
||
332 | |||
333 | // Altitude |
||
334 | //note:lephisto:according to several sources it's /30 |
||
335 | if (naviData->Altimeter > 300 || naviData->Altimeter < -300) |
||
336 | { |
||
337 | // above 10m only write full meters |
||
338 | write_ndigit_number_s (0, 1, naviData->Altimeter / 30, 4, 0); |
||
339 | } |
||
340 | else |
||
341 | { |
||
342 | // up to 10m write meters.dm |
||
343 | write_ndigit_number_s_10th (0, 1, naviData->Altimeter / 3, 3, 0); |
||
344 | } |
||
345 | lcd_putc (4, 1, 'm', 0); |
||
346 | |||
347 | draw_variometer (55, 7, 9, 13, naviData->Variometer); |
||
348 | |||
349 | // TODO: verify correctness |
||
350 | uint16_t heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360; |
||
351 | lcd_ecirc_line (22, 35, 15, old_hh, 0); |
||
352 | old_hh = heading_home; |
||
353 | lcd_ecirc_line (22, 35, 15, heading_home, 1); |
||
354 | |||
355 | write_ndigit_number_u (7, 3, heading_home, 3, 0); |
||
356 | lcd_putc (10, 3, 0x1e, 0); // degree symbol |
||
357 | |||
358 | write_ndigit_number_u (7, 2, naviData->HomePositionDeviation.Distance / 10, 3, 0); |
||
359 | lcd_putc (10, 2, 'm', 0); |
||
360 | |||
361 | // Sats in use |
||
362 | lcd_printp_at(10, 4, PSTR("Sats"), 0); |
||
363 | write_ndigit_number_u (8, 4, naviData->SatsInUse, 2, 0); |
||
364 | |||
365 | if (naviData->NCFlags & NC_FLAG_MANUAL_CONTROL) |
||
366 | { |
||
367 | lcd_putc (19, 4, 'M', 0); // rc transmitter |
||
368 | } |
||
369 | else |
||
370 | { |
||
371 | lcd_putc (19, 4, ' ', 0); // clear |
||
372 | } |
||
373 | #if 0 |
||
374 | lcd_printp_at(11, 5, PSTR("Mode:"), 0); |
||
375 | if (naviData->NCFlags & NC_FLAG_CH) |
||
376 | { |
||
377 | lcd_printpns_at (17, 5, PSTR("CH "), 0); |
||
378 | } |
||
379 | else if (naviData->NCFlags & NC_FLAG_PH) |
||
380 | { |
||
381 | lcd_printpns_at (17, 5, PSTR("PH "), 0); |
||
382 | } |
||
383 | else |
||
384 | { // (naviData->NCFlags & NC_FLAG_FREE) |
||
385 | lcd_printpns_at (17, 5, PSTR("Free"), 0); // sat2 (free) |
||
386 | } |
||
387 | #endif |
||
388 | if (naviData->NCFlags & NC_FLAG_CH) |
||
389 | { |
||
390 | lcd_printpns_at (10, 5, PSTR("Coming Home"), 0); |
||
391 | } |
||
392 | else if (naviData->NCFlags & NC_FLAG_PH) |
||
393 | { |
||
394 | lcd_printpns_at (10, 5, PSTR("Pos. Hold "), 0); |
||
395 | } |
||
396 | else |
||
397 | { // (naviData->NCFlags & NC_FLAG_FREE) |
||
398 | lcd_printpns_at (10, 5, PSTR("Free "), 0); |
||
399 | } |
||
400 | |||
401 | // Battery level |
||
402 | write_ndigit_number_u_10th (0, 7, naviData->UBat, 3, 0); |
||
403 | lcd_putc (4, 7, 'V', 0); |
||
404 | |||
405 | // Flying time |
||
406 | write_time (7, 7, naviData->FlyingTime); |
||
407 | // lcd_printp_at (7, 6, PSTR("Fly"), 0); |
||
408 | |||
409 | // RC |
||
410 | write_ndigit_number_u (15, 7, naviData->RC_Quality, 3, 0); |
||
411 | lcd_putc (18, 7, 0x1F, 0); // RC-transmitter |
||
412 | if (naviData->NCFlags & NC_FLAG_NOSERIALLINK) |
||
413 | { |
||
414 | lcd_printpns_at(19, 7, PSTR(" "), 0); // clear |
||
415 | } |
||
416 | else |
||
417 | { |
||
418 | lcd_printpns_at(19, 7, PSTR("PC"), 0); |
||
419 | } |
||
420 | |||
421 | // remember statistics (only when engines running) |
||
422 | if (naviData->MKFlags & FLAG_MOTOR_RUN) |
||
423 | { |
||
424 | if (naviData->Altimeter > max_Altimeter) max_Altimeter = naviData->Altimeter; |
||
425 | if (naviData->GroundSpeed > max_GroundSpeed) max_GroundSpeed = naviData->GroundSpeed; |
||
426 | if (naviData->HomePositionDeviation.Distance > max_Distance) max_Distance = naviData->HomePositionDeviation.Distance; |
||
427 | if (naviData->UBat < min_UBat) min_UBat = naviData->UBat; |
||
428 | if (naviData->FlyingTime > max_FlyingTime) max_FlyingTime = naviData->FlyingTime; |
||
429 | } |
||
430 | |||
431 | // remember last values |
||
432 | last_RC_Quality = naviData->RC_Quality; |
||
433 | last_UBat = naviData->UBat; |
||
434 | old_MKFlags = naviData->MKFlags; |
||
435 | |||
436 | rxd_buffer_locked = FALSE; |
||
437 | } |
||
438 | } |
||
439 | while (!get_key_press (1 << KEY_ESC) && timer); |
||
440 | get_key_press(KEY_ALL); |
||
441 | |||
442 | // disable OSD Data from NC |
||
443 | // RS232_request_mk_data (1, 'o', 0); |
||
444 | tmp_dat = 0; |
||
445 | SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
446 | |||
447 | mode = 0; |
||
448 | rxd_buffer_locked = FALSE; |
||
449 | |||
450 | if (!timer) |
||
451 | { // timeout occured |
||
452 | if (flag) |
||
453 | { |
||
454 | lcd_cls (); |
||
455 | } |
||
456 | lcd_printp_at (0, 2, PSTR("ERROR: no data"), 0); |
||
457 | |||
458 | timer = 100; |
||
459 | while (timer > 0); |
||
460 | print_statistics (); |
||
461 | } |
||
462 | } |