Subversion Repositories Projects

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

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