Subversion Repositories Projects

Rev

Rev 373 | Rev 378 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
321 cascade 1
/****************************************************************************
2
 *   Copyright (C) 2009 by Claas Anders "CaScAdE" Rathje                    *
3
 *   admiralcascade@gmail.com                                               *
4
 *   Project-URL: 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
 *   Credits to:                                                            *
22
 *   Holger Buss & Ingo Busker from mikrokopter.de for the MK project       *
23
 *   Gregor "killagreg" Stobrawa for making the MK code readable            *
24
 *   Klaus "akku" Buettner for the hardware                                 *
25
 *   Manuel "KeyOz" Schrape for explaining the MK protocol to me            *
26
 ****************************************************************************/
27
 
28
#include <avr/io.h>
29
#include <avr/interrupt.h>
30
#include <util/delay.h>
346 cascade 31
#include <avr/pgmspace.h> 
32
#include "main.h"
33
#include "max7456_software_spi.h"
331 cascade 34
#include "usart1.h"
321 cascade 35
 
36
/* TODO:
37
 * - verifiy correctness of values
324 cascade 38
 * - clean up code :)
321 cascade 39
 */
40
 
329 cascade 41
#if !(ALLCHARSDEBUG|(WRITECHARS != -1))
346 cascade 42
// data structs not needed for character flashin
331 cascade 43
#include "mk-data-structs.h"
321 cascade 44
 
45
/* ##########################################################################
46
 * global definitions and global vars
47
 * ##########################################################################*/
346 cascade 48
 
321 cascade 49
volatile uint16_t setsReceived = 0;
50
 
51
volatile NaviData_t naviData;
52
volatile DebugOut_t debugData;
53
 
54
// cache old vars for blinking attribute, checkup is faster than full
55
// attribute write each time
56
volatile uint8_t last_UBat = 255;
57
volatile uint8_t last_RC_Quality = 255;
58
 
59
// 16bit should be enough, normal LiPos don't last that long
60
volatile uint16_t uptime = 0;
61
volatile uint16_t timer = 0;
62
 
331 cascade 63
// remember last time data was received
64
volatile uint8_t seconds_since_last_data = 0;
65
 
346 cascade 66
// store stats description in progmem to save space
67
char stats_item_0[] PROGMEM = "max Altitude:";
68
char stats_item_1[] PROGMEM = "max Speed   :";
69
char stats_item_2[] PROGMEM = "max Distance:";
70
char stats_item_3[] PROGMEM = "min voltage :";
71
char stats_item_4[] PROGMEM = "max time    :";
72
char* stats_item_pointers[] PROGMEM = {stats_item_0, stats_item_1, stats_item_2, stats_item_3, stats_item_4};
73
 
74
// store more fixed strings in progmen
75
char ON[]  PROGMEM = "ON ";
76
char OFF[] PROGMEM = "OFF";
77
 
329 cascade 78
#endif // ends !(ALLCHARSDEBUG|(WRITECHARS != -1))
326 cascade 79
 
80
// general PAL|NTSC distingiusch stuff
81
uint8_t top_line = 1;
82
uint8_t bottom_line = 14;
83
 
84
// Flags
85
uint8_t COSD_FLAGS = 0;
86
 
321 cascade 87
/* ##########################################################################
326 cascade 88
 * debounce buttons
89
 * ##########################################################################*/
90
int s1_pressed() {
91
        if (S1_PRESSED) {
92
                _delay_ms(25);
93
                if (S1_PRESSED) return 1;
94
        }
95
        return 0;
96
}
97
 
98
int s2_pressed() {
99
        if (S2_PRESSED) {
100
                _delay_ms(25);
101
                if (S2_PRESSED) return 1;
102
        }
103
        return 0;
104
}
105
 
321 cascade 106
 
329 cascade 107
#if !(ALLCHARSDEBUG|(WRITECHARS != -1))
321 cascade 108
 
109
/**
331 cascade 110
 * serial support
321 cascade 111
 */
346 cascade 112
//#include "usart1.c"
321 cascade 113
 
114
 
115
/* ##########################################################################
116
 * timer stuff
117
 * ##########################################################################*/
118
 
331 cascade 119
/**
120
 * timer kicks in every 1000uS ^= 1ms
321 cascade 121
 */
122
ISR(TIMER0_OVF_vect) {
123
    OCR0 = 6; // preload
124
    if (!timer--) {
125
        uptime++;
126
        timer = 999;
331 cascade 127
                seconds_since_last_data++;
321 cascade 128
    }
129
}
130
 
131
/* ##########################################################################
132
 * compass stuff
133
 * ##########################################################################*/
134
 
135
/**
136
 * convert the <heading> gotton from NC into an index
137
 */
138
uint8_t heading_conv(uint16_t heading) {
139
    if (heading > 23 && heading < 68) {
140
        //direction = "NE";
141
        return 0;
142
    } else if (heading > 67 && heading < 113) {
143
        //direction = "E ";
144
        return 1;
145
    } else if (heading > 112 && heading < 158) {
146
        //direction = "SE";
147
        return 2;
148
    } else if (heading > 157 && heading < 203) {
149
        //direction = "S ";
150
        return 3;
151
    } else if (heading > 202 && heading < 248) {
152
        //direction = "SW";
153
        return 4;
154
    } else if (heading > 247 && heading < 293) {
155
        //direction = "W ";
156
        return 5;
157
    } else if (heading > 292 && heading < 338) {
158
        //direction = "NW";
159
        return 6;
160
    }
161
    //direction = "N ";
162
    return 7;
163
}
164
 
165
/**
377 cascade 166
 * convert the <heading> gotton from NC into a more
167
 * precise index
168
 */
169
uint8_t heading_fine_conv(uint16_t heading) {
170
        if (heading > 337) return 0;
171
        return (heading * 10) / 225;
172
}
173
 
174
/**
321 cascade 175
 * draw a compass rose at <x>/<y> for <heading>
176
 */
177
void draw_compass(uint8_t x, uint8_t y, uint16_t heading) {
178
    //char* rose = "---N---O---S---W---N---O---S---W---N---O---S---W";
179
    char rose[48] = {216, 215, 216, 211, 216, 215, 216, 213, 216, 215, 216, 212,
180
                    216, 215, 216, 214, 216, 215, 216, 211, 216, 215, 216, 213,
181
                    216, 215, 216, 212, 216, 215, 216, 214, 216, 215, 216, 211,
182
                    216, 215, 216, 213, 216, 215, 216, 212, 216, 215, 216, 214};
183
        // the center is char 19 (north), we add the current heading in 8th
184
        // which would be 22.5 degrees, but float would bloat up the code
185
        // and *10 / 225 would take ages... so we take the uncorrect way
186
    uint8_t front = 19 + (heading / 22);
187
    for (uint8_t i = 0; i < 9; i++) {
188
                write_char_xy(x++, y, rose[front - 4 + i]);
189
    }
190
}
191
 
192
/* ##########################################################################
377 cascade 193
 * battery index
194
 * ##########################################################################*/
195
/**
196
 * draw a battery symbol at <x>/<y> according to <voltage>
197
 */
198
void draw_battery(uint8_t x, uint8_t y, uint16_t voltage) {
199
        uint8_t percent = (100* (voltage - UBAT_WRN) / (UBAT_MAX - UBAT_WRN));
200
        if (percent > 100) percent = 100;
201
        write_char_xy(x, y, 0x9d - (percent * 13 / 100));
202
        //write_ndigit_number_u(x, y-1, percent * 13 / 100, 100, 0);
203
}
204
 
205
/**
206
 * draw variometer arrows at <x>/<y> according to <variometer>
207
 */
208
void draw_variometer(uint8_t x, uint8_t y, int16_t variometer) {
209
        if (variometer == 0) {
210
                write_char_xy(x, y, 0xbb); // plain line
211
        } else if (variometer > 0) { // gain height
212
                switch (variometer / 5){
213
                        case 0:
214
                                write_char_xy(x, y, 0xba); // smallest arrow up
215
                                break;
216
                        case 1:
217
                                write_char_xy(x, y, 0xb9); // small arrow up
218
                                break;
219
                        case 2:
220
                                write_char_xy(x, y, 0xb8); // large arrow up
221
                                break;
222
                        default:
223
                                write_char_xy(x, y, 0xb7); // largest arrow up
224
                }
225
        } else { // sink
226
                switch (variometer / -5){
227
                        case 0:
228
                                write_char_xy(x, y, 0xbc); // smallest arrow down
229
                                break;
230
                        case 1:
231
                                write_char_xy(x, y, 0xbd); // small arrow down
232
                                break;
233
                        case 2:
234
                                write_char_xy(x, y, 0xbe); // large arrow down
235
                                break;
236
                        default:
237
                                write_char_xy(x, y, 0xbf); // largest arrow down
238
                }
239
        }
240
}
241
 
242
/* ##########################################################################
321 cascade 243
 * artificial horizon
244
 * ##########################################################################*/
245
// remember last time displayed values
246
int8_t old_af_x = -1, old_af_y = -1;
247
 
248
/**
249
 * draw roll und nick indicators (could be enhanced to full artificial horizon)
250
 * from line <firstline> to <listlines> for given <nick> and <roll> values
251
 */
252
void draw_artificial_horizon(uint8_t firstline, uint8_t lastline, int16_t nick, int16_t roll) {
253
        char noodle[5] = {225, 225, 226, 227, 227};
254
        uint8_t center_x = 15;
255
        uint8_t center_y = lastline - firstline;
256
        center_y = 7;
257
        write_char_xy(center_x,center_y,228);
258
        uint8_t cpos, nicky, rollx;
259
 
260
        // which line
261
        int8_t ypos =  nick / 20;
262
        // which character from the array?
263
        if (nick < 0) {
264
                cpos = -1*((nick - (ypos * 20))/4);
265
                ypos--;
266
        } else cpos = 4-((nick - (ypos * 20))/4);
267
        if (cpos > 4) cpos = 4;
268
 
269
        nicky = center_y - ypos;
270
        if (nicky > lastline) nicky = lastline;
271
        else if (nicky < firstline) nicky = firstline;
272
 
273
        // ensure roll-borders
274
        rollx = (roll / 8)+15;
275
        if (rollx < 2) rollx = 2;
276
        else if (rollx > 28) rollx = 28;
277
 
278
 
279
        // clear roll
280
        if (old_af_x != rollx && old_af_x >= 0) {
281
                write_char_xy(old_af_x,13,0);
282
        }
283
 
284
        // clear nick
285
        if (old_af_y != nicky && old_af_y >= 0) {
286
                write_char_xy(center_x-1,old_af_y,0);
287
                write_char_xy(center_x+1,old_af_y,0);
288
        }
289
 
290
 
291
        // draw nick
292
        write_char_xy(center_x-1,nicky,noodle[cpos]);
293
        write_char_xy(center_x+1,nicky,noodle[cpos]);
294
 
295
        // draw roll
296
        write_char_xy(rollx,lastline,229);
297
 
298
        // update old vars
299
        old_af_x = rollx;
300
        old_af_y = nicky;
301
 
302
        // debug numbers
303
        //write_3digit_number_u(20,6,cpos);
304
        //write_number_s(20,7,ypos);    
305
        //write_number_s(0,7,nick);             
306
        //write_number_s(18,11,roll);   
307
}
308
 
326 cascade 309
/* ##########################################################################
310
 * A simple config menu for the flags
311
 * ##########################################################################*/
339 cascade 312
 
313
/**
314
 * helper function for menu updating
315
 */
316
void config_menu_drawings(uint8_t chosen) {
317
    // clear prevoius _cursor_
346 cascade 318
    write_ascii_string(3, (chosen + 6) % 7, " ");
339 cascade 319
    // draw current _cursor_
320
    write_ascii_string(3, chosen + 6, ">");
321
    if (COSD_FLAGS & COSD_FLAG_HUD) {
346 cascade 322
        write_ascii_string_pgm(23, 6, ON);
339 cascade 323
    } else {
346 cascade 324
                write_ascii_string_pgm(23, 6, OFF);
339 cascade 325
    }
326
    if (COSD_FLAGS & COSD_FLAG_ARTHORIZON) {
346 cascade 327
        write_ascii_string_pgm(23, 7, ON);
339 cascade 328
    } else {
346 cascade 329
        write_ascii_string_pgm(23, 7, OFF);
339 cascade 330
    }
331
        if (COSD_FLAGS & COSD_FLAG_STATS) {
346 cascade 332
        write_ascii_string_pgm(23, 8, ON);
339 cascade 333
    } else {
346 cascade 334
        write_ascii_string_pgm(23, 8, OFF);
339 cascade 335
    }
336
    if (COSD_FLAGS & COSD_FLAG_WARNINGS) {
346 cascade 337
        write_ascii_string_pgm(23, 9, ON);
339 cascade 338
    } else {
346 cascade 339
        write_ascii_string_pgm(23, 9, OFF);
339 cascade 340
    }
341
}
342
 
343
/**
346 cascade 344
 * some sort of clicking response in the menu
345
 */
346
void config_menu_doclick(uint8_t chosen, char** menu) {
347
        write_ascii_string(4, chosen + 6, "DONE              ");
348
        _delay_ms(1000);
349
        write_ascii_string(4, chosen + 6, menu[chosen]);
350
}
351
 
352
/**
339 cascade 353
 * a simple config menu tryout
354
 */
326 cascade 355
void config_menu(void) {
356
        // disable interrupts (makes the menu more smoothely)
357
        cli();
321 cascade 358
 
326 cascade 359
        // clear screen
360
        clear();
361
 
346 cascade 362
        char* menu[8] = {"Full HUD",
339 cascade 363
                                         "Art.Horizon in HUD",
364
                                         "Statistics",
365
                                         "Warnings",    // TODO: do it!
346 cascade 366
                                         "Reset uptime",
367
                                         "Request OSD-data",
368
                                         "Disable Debug-data",
339 cascade 369
                                         "EXIT"};
326 cascade 370
 
371
        uint8_t inmenu = 1;
372
        uint8_t chosen = 0;
339 cascade 373
        write_ascii_string(6,  2, "C-OSD Config Menu");
326 cascade 374
 
375
        // wait a bit before doing stuff so user has chance to release button
376
        _delay_ms(250);
377
 
339 cascade 378
        write_ascii_string(4,  6, menu[0]);
379
        write_ascii_string(4,  7, menu[1]);
380
        write_ascii_string(4,  8, menu[2]);
381
        write_ascii_string(4,  9, menu[3]);
382
        write_ascii_string(4, 10, menu[4]);
346 cascade 383
        write_ascii_string(4, 11, menu[5]);
384
        write_ascii_string(4, 12, menu[6]);
385
        write_ascii_string(4, 13, menu[7]);
339 cascade 386
 
387
        config_menu_drawings(chosen);
388
 
326 cascade 389
        while (inmenu) {
390
                        if (s2_pressed()) {
339 cascade 391
                                write_ascii_string(3,  chosen+6, " ");
346 cascade 392
                                chosen = (chosen + 1) % 8;
339 cascade 393
                                write_ascii_string(3,  chosen+6, ">");
326 cascade 394
                                _delay_ms(500);
395
                        } else if (s1_pressed()) {
396
                                switch (chosen) {
339 cascade 397
                                        case 0:         // full HUD
398
                                                COSD_FLAGS ^= COSD_FLAG_HUD;
399
                                                config_menu_drawings(chosen);
326 cascade 400
                                                break;
339 cascade 401
                                        case 1:         // art horizon
402
                                                COSD_FLAGS ^= COSD_FLAG_ARTHORIZON;
403
                                                config_menu_drawings(chosen);
326 cascade 404
                                                break;
339 cascade 405
                                        case 2:         // statistics
406
                                                COSD_FLAGS ^= COSD_FLAG_STATS;
407
                                                config_menu_drawings(chosen);
326 cascade 408
                                                break;
339 cascade 409
                                        case 3:         // warnings
410
                                                COSD_FLAGS ^= COSD_FLAG_WARNINGS;
411
                                                config_menu_drawings(chosen);
412
                                                break;
346 cascade 413
                                        case 4:         // reset uptime
414
                                                uptime = 0;
415
                                                config_menu_doclick(chosen, menu);
416
                                                break;
417
                                        case 5:         // re-request OSD data
418
                                                // request OSD Data from NC every 100ms
419
                                                usart1_request_mk_data(1, 'o', 100);
420
                                                config_menu_doclick(chosen, menu);
421
                                                break;
422
                                        case 6:         // disable debug data
423
                                                // disable sending of debug data
424
                                                // may result in smoother ddata display
425
                                                usart1_request_mk_data(0, 'd', 0);
426
                                                config_menu_doclick(chosen, menu);
427
                                                break;
428
                                        case 7:         // exit
339 cascade 429
                                                inmenu = 0;
430
                                                break;
326 cascade 431
                                }
339 cascade 432
                                _delay_ms(250);
326 cascade 433
                        }
434
        }
435
 
436
        // clear screen up again
437
        clear();
438
 
439
        // update flags to paint display again if needed
440
        COSD_FLAGS &= ~COSD_ICONS_WRITTEN;
441
 
442
        // enable interrupts again
443
        sei();
444
}
445
 
329 cascade 446
#endif // ends !(ALLCHARSDEBUG|(WRITECHARS != -1))
321 cascade 447
/* ##########################################################################
448
 * MAIN
449
 * ##########################################################################*/
450
int main(void) {
339 cascade 451
    // set up FLAGS, compiler should flatten this one
452
    COSD_FLAGS = (NTSC << 0);
453
    COSD_FLAGS |= (HUD << 1);
454
    COSD_FLAGS |= (ARTHORIZON << 2);
455
        COSD_FLAGS |= (STATS << 3);
456
    COSD_FLAGS |= (WARNINGS << 4);
324 cascade 457
 
339 cascade 458
    // set up Atmega162 Ports
321 cascade 459
    DDRA |= (1 << PA1); // PA1 output (/CS)
460
    MAX_CS_HIGH
461
    DDRA |= (1 << PA2); // PA2 output (SDIN)
462
    MAX_SDIN_LOW
463
    DDRA |= (1 << PA3); // PA3 output (SCLK)
464
    MAX_SCLK_LOW
465
    DDRA |= (1 << PA5); // PA5 output (RESET)
466
    MAX_RESET_HIGH
467
 
468
    DDRC |= (1 << PC0); // PC0 output (LED1 gn)
469
    LED1_OFF
470
    DDRC |= (1 << PC1); // PC1 output (LED2 rt)
471
    LED2_OFF
472
    DDRC |= (1 << PC2); // PC2 output (LED3 gn)
473
    LED3_OFF
474
    DDRC |= (1 << PC3); // PC3 output (LED4 rt)
475
    LED4_OFF
476
 
477
    DDRC &= ~(1 << PC4); // PC4 input  (MODE)
478
    PORTC |= (1 << PC4); // pullup
479
    DDRC &= ~(1 << PC5); // PC5 input  (SET)
480
    PORTC |= (1 << PC5); // pullup
481
 
339 cascade 482
    // set up top and bottom lines
483
    if (COSD_FLAGS & COSD_FLAG_NTSC) {
484
        bottom_line = 12;
485
    } else {
486
        bottom_line = 14;
487
    }
326 cascade 488
 
339 cascade 489
    // reset the MAX7456 to be sure any undefined states do no harm
321 cascade 490
    MAX_RESET_LOW
491
    MAX_RESET_HIGH
492
 
493
    // give the FC/NC and the maxim time to come up
494
    LED4_ON
495
    _delay_ms(2000);
496
 
497
    LED4_OFF
498
 
331 cascade 499
 
339 cascade 500
    //Pushing NEW chars to the MAX7456
329 cascade 501
#if (WRITECHARS != -1)
339 cascade 502
    // DISABLE display (VM0)
321 cascade 503
    spi_send_byte(0x00, 0b00000000);
339 cascade 504
#include "characters.c"
321 cascade 505
 
331 cascade 506
#endif 
321 cascade 507
 
339 cascade 508
    // Setup Video Mode
509
    if (COSD_FLAGS & COSD_FLAG_NTSC) {
510
        // NTSC + enable display immediately (VM0)
511
        spi_send_byte(0x00, 0b00001000);
512
    } else {
513
        // PAL + enable display immediately (VM0)
514
        spi_send_byte(0x00, 0b01001000);
515
    }
321 cascade 516
 
517
    // clear all display-mem (DMM)
518
    spi_send_byte(0x04, 0b00000100);
519
 
520
    // clearing takes 12uS according to maxim so lets wait longer
521
    _delay_us(120);
522
 
523
    // 8bit mode
524
    spi_send_byte(0x04, 0b01000000);
525
 
526
    // write blank chars to whole screen
527
    clear();
528
 
329 cascade 529
#if !(ALLCHARSDEBUG|(WRITECHARS != -1))
321 cascade 530
    // init usart
531
    usart1_init();
532
 
533
    // set up timer
534
    TCCR0 |= (1 << CS00) | (1 << CS01); // timer0 prescaler 64
535
    OCR0 = 6; // preload
536
    TIMSK |= (1 << TOIE0); // enable overflow timer0
537
 
326 cascade 538
    // enable interrupts
321 cascade 539
    sei();
540
#endif
541
 
542
    //write_ascii_string(2,  7, "         CaScAdE          ");
543
    //write_ascii_string(2,  8, "is TESTING his open source");
544
    //write_ascii_string(2,  9, "    EPi OSD Firmware");
545
 
546
    // custom char preview
547
    /*write_char_xy( 2, 7, 200);
548
    write_char_xy( 3, 7, 201);
549
    write_char_xy( 4, 7, 202);
550
    write_char_xy( 5, 7, 203);
551
    write_char_xy( 6, 7, 204);
552
    write_char_xy( 7, 7, 205);
553
    write_char_xy( 8, 7, 206);
554
    write_char_xy( 9, 7, 207);
555
    write_char_xy(10, 7, 208);
556
    write_char_xy(11, 7, 209);
557
    write_char_xy(12, 7, 210);
558
    write_char_xy(13, 7, 211);
559
    write_char_xy(14, 7, 212);
560
    write_char_xy(15, 7, 213);
561
    write_char_xy(16, 7, 214);
562
    write_char_xy(17, 7, 215);*/
563
 
564
    // we are ready
565
    LED3_ON
566
 
567
 
568
 
329 cascade 569
#if ALLCHARSDEBUG | (WRITECHARS != -1)
321 cascade 570
        clear();
571
    write_all_chars();
572
#else
339 cascade 573
        // clear serial screen
574
        //usart1_puts("\x1B[2J\x1B[H");
575
        //usart1_puts("hello world!\r\n");
321 cascade 576
 
339 cascade 577
 
346 cascade 578
        // request data ever 100ms from FC;
579
        //usart1_request_mk_data(0, 'd', 100);
339 cascade 580
 
581
        // request OSD Data from NC every 100ms
346 cascade 582
        usart1_request_mk_data(1, 'o', 100);
583
 
339 cascade 584
    // and disable debug...
346 cascade 585
        //usart1_request_mk_data(0, 'd', 0);
321 cascade 586
 
339 cascade 587
    // disable TXD-pin
588
    usart1_DisableTXD();
331 cascade 589
 
339 cascade 590
    // stats for after flight
591
    int16_t max_Altimeter = 0;
592
    uint16_t max_GroundSpeed = 0;
593
    int16_t max_Distance = 0;
594
    uint8_t min_UBat = 255;
595
    uint16_t max_FlyingTime = 0;
321 cascade 596
 
339 cascade 597
    // flags from last round to check for changes
598
    uint8_t old_MKFlags = 0;
599
 
321 cascade 600
    char* directions[8] = {"NE", "E ", "SE", "S ", "SW", "W ", "NW", "N "};
377 cascade 601
    //char arrowdir[8] = {218, 217, 224, 223, 222, 221, 220, 219};
321 cascade 602
 
349 cascade 603
        /*write_ndigit_number_s_10th(5, 10, 1, 100, 0);
604
        write_ndigit_number_s_10th(5, 11, -1, 100, 0);
605
        write_ndigit_number_s_10th(5, 12, -11, 100, 0);*/
606
 
377 cascade 607
        //write_ndigit_number_u(2,2, heading_fine_conv(45), 100, 0);
608
 
321 cascade 609
    while (1) {
339 cascade 610
        // write icons at init or after menu/mode-switch
611
        if (!(COSD_FLAGS & COSD_ICONS_WRITTEN) && (COSD_FLAGS & COSD_FLAG_HUD)) {
612
            write_char_xy(5, top_line, 203); // km/h
613
            write_char_xy(10, top_line, 202); // RC-transmitter
614
            write_char_xy(16, top_line, 208); // degree symbol
615
            write_char_xy(27, top_line, 204); // small meters m
377 cascade 616
            //write_ascii_string(6, bottom_line, "V"); // voltage
339 cascade 617
            write_char_xy(14, bottom_line, 209); // on clock
618
            write_char_xy(22, bottom_line, 210); // fly clock
619
            write_char_xy(26, bottom_line, 200); // sat1
620
            write_char_xy(27, bottom_line, 201); // sat2
621
            COSD_FLAGS |= COSD_ICONS_WRITTEN;
622
        }
321 cascade 623
        if (rxd_buffer_locked) {
339 cascade 624
            if (COSD_FLAGS & COSD_FLAG_HUD) {
625
                if (rxd_buffer[2] == 'D') { // FC Data
626
                    /*Decode64();
627
                    debugData = *((DebugOut_t*) pRxData);
628
                    write_number_s(12, 2, RxDataLen);
629
                    write_number_s(20, 2, setsReceived++);
630
                    write_number_s(12, 3, debugData.Analog[0]); // AngleNick
631
                    write_number_s(12, 4, debugData.Analog[1]); // AngleRoll
336 cascade 632
                                        write_number_s(12, 5, debugData.Analog[5]); // Height
339 cascade 633
                    write_number_s(12, 6, debugData.Analog[9]); // Voltage
634
                    write_number_s(12, 7, debugData.Analog[10]);// RC Signal
336 cascade 635
                                        write_number_s(12, 8, debugData.Analog[11]);// Gyro compass*/
339 cascade 636
                } else if (rxd_buffer[2] == 'O') { // NC OSD Data
637
                    Decode64();
638
                    naviData = *((NaviData_t*) pRxData);
321 cascade 639
 
339 cascade 640
                    // first line
373 cascade 641
                    write_ndigit_number_u(2, top_line, (uint16_t) (((uint32_t) naviData.GroundSpeed * (uint32_t)9) / (uint32_t)250), 100, 0);
321 cascade 642
 
349 cascade 643
                    write_ndigit_number_u(7, top_line, naviData.RC_Quality, 100, 0);
339 cascade 644
                    if (naviData.RC_Quality <= RCLVL_WRN && last_RC_Quality > RCLVL_WRN) {
645
                        for (uint8_t x = 0; x < 4; x++)
646
                            write_char_att_xy(7 + x, top_line, BLINK);
647
                    } else if (naviData.RC_Quality > RCLVL_WRN && last_RC_Quality <= RCLVL_WRN) {
648
                        for (uint8_t x = 0; x < 4; x++)
649
                            write_char_att_xy(7 + x, top_line, 0);
650
                    }
651
                    last_RC_Quality = naviData.RC_Quality;
321 cascade 652
 
349 cascade 653
                    write_ndigit_number_u(13, top_line, naviData.CompassHeading, 100, 0);
321 cascade 654
 
339 cascade 655
                    write_ascii_string(17, top_line, directions[heading_conv(naviData.CompassHeading)]);
377 cascade 656
 
657
                                        draw_variometer(20, top_line, naviData.Variometer);
321 cascade 658
 
339 cascade 659
                    //note:lephisto:according to several sources it's /30
349 cascade 660
                    if (naviData.Altimeter > 300 || naviData.Altimeter < -300) {
339 cascade 661
                        // above 10m only write full meters
349 cascade 662
                        write_ndigit_number_s(23, top_line, naviData.Altimeter / 30, 1000, 0);
339 cascade 663
                    } else {
664
                        // up to 10m write meters.dm
349 cascade 665
                        //write_number_u_10th(21, top_line, naviData.Altimeter / 3);
666
                                                write_ndigit_number_s_10th(23, top_line, naviData.Altimeter / 3, 100, 0);
339 cascade 667
                    }
321 cascade 668
 
339 cascade 669
                    // seccond line
670
                    draw_compass(11, top_line + 1, naviData.CompassHeading);
321 cascade 671
 
339 cascade 672
                    // TODO: verify correctness
673
                    uint16_t heading_home = (naviData.HomePositionDeviation.Bearing + 360 - naviData.CompassHeading) % 360;
377 cascade 674
                    //write_char_xy(27, top_line + 1, arrowdir[heading_conv(heading_home)]);
675
                                        // finer resolution, 0xa0 is first character and we add the index 0 <= index < 16
676
                                        write_char_xy(27, top_line + 1, 0xa0 + heading_fine_conv(heading_home));
321 cascade 677
 
355 shaddi 678
                    write_ndigit_number_u(24, top_line + 1, naviData.HomePositionDeviation.Distance / 10, 100, 0);
321 cascade 679
 
339 cascade 680
                    // center
681
                    if (naviData.MKFlags & FLAG_MOTOR_RUN) { // should be engines running
682
                        if (!(old_MKFlags & FLAG_MOTOR_RUN)) { // motors just started, clear middle
683
                            clear();
684
                            // update flags to paint display again if needed
685
                            COSD_FLAGS &= ~COSD_ICONS_WRITTEN;
686
                        }
687
                        if (COSD_FLAGS & COSD_FLAG_ARTHORIZON) {
688
                            draw_artificial_horizon(top_line + 2, bottom_line - 1, naviData.AngleNick, naviData.AngleRoll);
689
                        }
690
                    } else {
336 cascade 691
                                                // stats
339 cascade 692
                                                if (COSD_FLAGS & COSD_FLAG_STATS) {
346 cascade 693
                                                        write_ascii_string_pgm(2, 5, stats_item_pointers[0]); // max Altitude
349 cascade 694
                                write_ndigit_number_s(18, 5, max_Altimeter / 30, 1000, 0);
339 cascade 695
                                write_char_xy(22, 5, 204); // small meters m
346 cascade 696
                                                        write_ascii_string_pgm(2, 6, stats_item_pointers[1]); // max Speed
349 cascade 697
                                write_ndigit_number_u(19, 6, (uint16_t) (((uint32_t) max_GroundSpeed * 36) / 1000), 100, 0);
339 cascade 698
                                write_char_xy(22, 6, 203); // km/h
346 cascade 699
                                                        write_ascii_string_pgm(2, 7, stats_item_pointers[2]); // max Distance
349 cascade 700
                                write_ndigit_number_u(19, 7, max_Distance / 100, 100, 0);
339 cascade 701
                                write_char_xy(22, 7, 204); // small meters m
346 cascade 702
                                                        write_ascii_string_pgm(2, 8, stats_item_pointers[3]); // min voltage
349 cascade 703
                                                        write_ndigit_number_u_10th(18, 8, min_UBat, 100, 0);
339 cascade 704
                                write_ascii_string(22, 8, "V"); // voltage
346 cascade 705
                                                        write_ascii_string_pgm(2, 9, stats_item_pointers[4]); // max time
339 cascade 706
                                write_time(16, 9, max_FlyingTime);
707
                                write_char_xy(22, 9, 210); // fly clock
708
                                                } else if (COSD_FLAGS & COSD_FLAG_ARTHORIZON) { // if no stats there is space horizon
709
                            draw_artificial_horizon(top_line + 2, bottom_line - 1, naviData.AngleNick, naviData.AngleRoll);
710
                        }
711
                    }
321 cascade 712
 
339 cascade 713
                    // bottom line
349 cascade 714
                                        write_ndigit_number_u_10th(2, bottom_line, naviData.UBat, 100, 0);
339 cascade 715
                    if (naviData.UBat <= UBAT_WRN && last_UBat > UBAT_WRN) {
716
                        for (uint8_t x = 0; x < 7; x++)
717
                            write_char_att_xy(x, bottom_line, BLINK);
718
                    } else {
719
                        for (uint8_t x = 0; x < 7; x++)
720
                            write_char_att_xy(x, bottom_line, 0);
721
                    }
377 cascade 722
                                        draw_battery(6, bottom_line, naviData.UBat);
321 cascade 723
 
339 cascade 724
                    write_time(8, bottom_line, uptime);
725
                    write_time(16, bottom_line, naviData.FlyingTime);
321 cascade 726
 
349 cascade 727
                    write_ndigit_number_u(24, bottom_line, naviData.SatsInUse, 10, 0);
321 cascade 728
 
339 cascade 729
                    if (naviData.NCFlags & NC_FLAG_CH) {
730
                        write_char_xy(27, bottom_line, 231); // gps ch
731
                    } else if (naviData.NCFlags & NC_FLAG_PH) {
732
                        write_char_xy(27, bottom_line, 230); // gps ph
733
                    } else { // (naviData.NCFlags & NC_FLAG_FREE)
734
                        write_char_xy(27, bottom_line, 201); // sat2 (free)
735
                    }
321 cascade 736
 
339 cascade 737
                    //write_number_s(8, 5, RxDataLen);
738
                    //write_number_s(16, 5, setsReceived++);
321 cascade 739
 
339 cascade 740
                    // remember statistics
741
                    if (naviData.Altimeter > max_Altimeter) max_Altimeter = naviData.Altimeter;
742
                    if (naviData.GroundSpeed > max_GroundSpeed) max_GroundSpeed = naviData.GroundSpeed;
743
                    if (naviData.HomePositionDeviation.Distance > max_Distance) {
744
                        max_Distance = naviData.HomePositionDeviation.Distance;
745
                    }
746
                    if (naviData.UBat < min_UBat) min_UBat = naviData.UBat;
747
                    if (naviData.FlyingTime > max_FlyingTime) max_FlyingTime = naviData.FlyingTime;
748
 
749
                    old_MKFlags = naviData.MKFlags;
750
                }
751
            }
752
            seconds_since_last_data = 0;
321 cascade 753
            rxd_buffer_locked = 0;
754
        }
755
        // handle keypress
326 cascade 756
        if (s1_pressed()) {
339 cascade 757
            config_menu();
321 cascade 758
        }
339 cascade 759
        if (seconds_since_last_data > 2) {
346 cascade 760
                        // request OSD Data from NC every 100ms
761
                        usart1_request_mk_data(1, 'o', 100);
762
                        seconds_since_last_data = 0;
339 cascade 763
        }
321 cascade 764
    }
765
#endif
766
    return 0;
767
}