Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1470 - 1
/*****************************************************************************
2
 *   Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de                  *
3
 *   Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net                  *
4
 *   Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
5
 *   Copyright (C) 2011 Harald Bongartz                                      *
6
 *                                                                           *
7
 *   This program is free software; you can redistribute it and/or modify    *
8
 *   it under the terms of the GNU General Public License as published by    *
9
 *   the Free Software Foundation; either version 2 of the License.          *
10
 *                                                                           *
11
 *   This program is distributed in the hope that it will be useful,         *
12
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of          *
13
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
14
 *   GNU General Public License for more details.                            *
15
 *                                                                           *
16
 *   You should have received a copy of the GNU General Public License       *
17
 *   along with this program; if not, write to the                           *
18
 *   Free Software Foundation, Inc.,                                         *
19
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.               *
20
 *                                                                           *
21
 *                                                                           *
22
 *   Credits to:                                                             *
23
 *   Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN  *
24
 *                          http://www.mikrokopter.de                        *
25
 *   Gregor "killagreg" Stobrawa for his version of the MK code              *
26
 *   Thomas Kaiser "thkais" for the original project. See                    *
27
 *                          http://www.ft-fanpage.de/mikrokopter/            *
28
 *                          http://forum.mikrokopter.de/topic-4061-1.html    *
29
 *   Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
30
 *                          http://www.mylifesucks.de/oss/c-osd/             *
31
 *   Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
32
 *****************************************************************************/
33
 
34
 
35
#include <avr/io.h>
36
#include <inttypes.h>
37
#include <stdlib.h>
38
#include <avr/pgmspace.h>
39
 
40
#include "main.h"
41
#include "lcd.h"
42
#include "timer.h"
43
#include "usart.h"
44
 
45
#define TIMEOUT 200 // 2 sec
46
 
47
uint8_t ck_a = 0;
48
uint8_t ck_b = 0;
49
uint8_t UBX_class = 0;
50
uint8_t UBX_id = 0;
51
uint8_t UBX_buffer[250];
52
uint8_t UBX_payload_counter = 0;
53
 
54
void checksum(uint8_t);
55
void UBX_process(void);
56
uint32_t join_4_bytes(uint8_t*);
57
 
58
uint8_t display_mode = 0;
59
 
60
 
61
//--------------------------------------------------------------
62
void gps(void)
63
{
64
        lcd_cls();
65
        display_mode = 2;
66
 
67
        if (hardware == FC)
68
        {
69
                lcd_printp_at(0, 3, PSTR("Nur mit NC !"), 0);
70
                timer = 100;
71
                while (timer > 0);
72
                return;
73
        }
74
 
75
 
76
        if(current_hardware != NC)
77
                SwitchToNC();
78
 
79
        SwitchToGPS();
80
 
81
        uint8_t mode = 0;
82
 
83
//      SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);
84
 
85
        timer = TIMEOUT;
86
        uint8_t data = 0;
87
        uint8_t length = 0;
88
        uint8_t UBX_ck_a = 0;
89
        do
90
        {
91
        //      if (rxFlag == 1)
92
                if (uart_getc_nb(&data))
93
                {
94
                        //rxFlag = 0;
95
                        //data = rx_byte;
96
                        timer = TIMEOUT;
97
 
98
                        switch(mode)
99
                        {
100
                                case 0: // init 1
101
                                        if(data == 0xB5)
102
                                        {
103
                                                UBX_payload_counter = 0;
104
                                                UBX_id = 0;
105
                                                UBX_class = 0;
106
                                                ck_a = 0;
107
                                                ck_b = 0;
108
                                                mode++;
109
                                        }
110
                                        break;
111
 
112
                                case 1: // init 2
113
                                        if(data == 0x62)
114
                                                mode++;
115
                                        else
116
                                                mode = 0;
117
                                        break;
118
 
119
                                case 2: //class
120
                                        if(data != 1)
121
                                                mode = 0;
122
                                        else
123
                                        {
124
                                                checksum(data);
125
                                                UBX_class = data;
126
                                                mode++;
127
                                        }
128
                                        break;
129
 
130
                                case 3: // id
131
                                        if((data != 48)&&(data != 6)&&(data != 18)&&(data != 2))
132
                                                mode = 0;
133
                                        else
134
                                        {
135
                                                UBX_id = data;
136
                                                checksum(data);
137
                                                mode++;
138
                                        }
139
                                        break;
140
 
141
                                case 4: // length lo
142
                                        if(data > 250)
143
                                                mode = 0;
144
                                        else
145
                                        {
146
                                                checksum(data);
147
                                                length = data;
148
                                                mode++;
149
                                        }
150
                                        break;
151
 
152
                                case 5: // length hi
153
                                        if(data != 0)
154
                                                mode = 0;
155
                                        else
156
                                        {
157
                                                checksum(data);
158
                                                mode++;
159
                                        }
160
                                        break;
161
 
162
                                case 6: // length hi
163
                                        length--;
164
                                        UBX_buffer[UBX_payload_counter] = data;
165
                                        checksum(data);
166
                                        UBX_payload_counter++;
167
                                        if(length==0)
168
                                        {
169
                                                mode++;
170
                                        };
171
                                        break;
172
 
173
                                case 7: // check lo
174
                                        mode++;
175
                                        UBX_ck_a = data;
176
                                        break;
177
 
178
                                case 8: // check hi
179
                                        mode=0;
180
                                        if((UBX_ck_a == ck_a) && (data == ck_b))
181
                                                UBX_process();
182
 
183
                        }
184
 
185
 
186
                //      write_ndigit_number_u (14, 0, data, 3, 0);
187
                }
188
 
189
        }
190
        while (!get_key_press (1 << KEY_ESC) && timer);
191
        get_key_press(KEY_ALL);
192
 
193
        SwitchToNC();
194
 
195
 
196
}
197
 
198
 
199
//--------------------------------------------------------------
200
void UBX_process()
201
{
202
 
203
        if ((get_key_press (1 << KEY_MINUS))||(display_mode == 2))
204
        {
205
                if (display_mode != 1)
206
                {
207
                        lcd_cls();
208
                        lcd_printp_at (0,0, PSTR("Fix Type : "), 0);
209
                        lcd_printp_at (0,1, PSTR("Sat      : "), 0);
210
                        lcd_printp_at (0,2, PSTR("Accuracy : "), 0);
211
                        lcd_printp_at (0,3, PSTR("PDOP     : "), 0);
212
                        lcd_printp_at (0,4, PSTR("Speed    : "), 0);
213
                        lcd_printp_at (0,5, PSTR("Long     : "), 0);
214
                        lcd_printp_at (0,6, PSTR("Lat      : "), 0);
215
                        lcd_printp_at (0,7, PSTR("Alt      : "), 0);
216
                }
217
                display_mode = 1;
218
        }
219
 
220
        if((UBX_class == 1) && (UBX_id == 48)&&(display_mode == 0))
221
        {
222
                uint8_t channels = UBX_buffer[4];
223
 
224
                uint8_t i = 0;
225
                for(i = 0; i < channels; i++)
226
                {
227
                        if (i > 15)
228
                                break;
229
 
230
                        uint8_t line;
231
                        uint8_t col;
232
 
233
                        if (i > 7)
234
                        {
235
                                line = i-7;
236
                                col = 11;
237
                        }
238
                        else
239
                                col = 0; line = i;
240
 
241
                        write_ndigit_number_u (col, line, UBX_buffer[9 + 12*i], 3, 0);
242
                        write_ndigit_number_u (col+4, line, UBX_buffer[12 + 12*i], 2, 0);
243
 
244
                        if((UBX_buffer[10 + 12*i] & 3) == 3)
245
                                lcd_printp_at (col+7,line, PSTR("O"), 0);
246
                        else if((UBX_buffer[10 + 12*i] & 1) == 1)
247
                                lcd_printp_at (col+7,line, PSTR("X"), 0);
248
                        else if(UBX_buffer[11 + 12*i] > 4)
249
                                lcd_printp_at (col+7,line, PSTR("x"), 0);
250
                        else if(UBX_buffer[11 + 12*i] > 1)
251
                                lcd_printp_at (col+7,line, PSTR("-"), 0);
252
                        else
253
                                lcd_printp_at (col+7,line, PSTR(" "), 0);
254
 
255
                }
256
        }
257
 
258
        if(display_mode == 1)
259
        {
260
                if((UBX_class == 1) && (UBX_id == 6))  //SVINFO
261
                {
262
                        switch (UBX_buffer[10])
263
                        {
264
                                case 4:
265
 
266
                                case 3:
267
                                        lcd_printp_at (11,0, PSTR("3D"), 0);
268
                                        break;
269
 
270
                                case 2:
271
                                        lcd_printp_at (11,0, PSTR("2D"), 0);
272
                                        break;
273
 
274
                                default:
275
                                        lcd_printp_at (11,0, PSTR("no"), 0);
276
                        }
277
 
278
                        if((UBX_buffer[11] & 3) == 3)
279
                                lcd_printp_at (17,0, PSTR("D"), 0);
280
                        else
281
                                lcd_printp_at (17,0, PSTR(" "), 0);
282
 
283
                        if((UBX_buffer[11] & 1) == 1)
284
                                lcd_printp_at (14,0, PSTR("ok"), 0);
285
                        else
286
                                lcd_printp_at (14,0, PSTR("  "), 0);
287
 
288
                        lcd_write_number_u_at (11, 1, UBX_buffer[47]);
289
 
290
                        uint16_t acc = (uint16_t)join_4_bytes(&UBX_buffer[24]);
291
                        write_ndigit_number_u (11, 2, acc, 5, 0);
292
                        lcd_printp_at (17,2, PSTR("cm"), 0);
293
 
294
                        uint16_t pdop = UBX_buffer[44]+UBX_buffer[45]*255;
295
                        write_ndigit_number_u (11, 3, pdop/100, 2, 0);
296
                        lcd_printp_at (13,3, PSTR("."), 0);
297
                        write_ndigit_number_u (14, 3, (pdop % 100),2, 1);
298
                }
299
 
300
                if((UBX_class == 1) && (UBX_id == 18))  //VELNED
301
                {
302
                        uint16_t speed = (uint16_t)((join_4_bytes(&UBX_buffer[20])*60*60)/100000);
303
                        write_ndigit_number_u (11, 4, speed, 3, 0);
304
                        lcd_printp_at (15,4, PSTR("km/h"), 0);
305
 
306
                }
307
 
308
                if((UBX_class == 1) && (UBX_id == 2))  //POSLLH
309
                {
310
                        uint32_t lon = join_4_bytes(&UBX_buffer[4]);
311
                        write_ndigit_number_u (10, 5, (uint16_t)(lon/10000000), 2, 0);
312
                        lcd_printp_at (12,5, PSTR("."), 0);
313
                        write_ndigit_number_u (13, 5, (uint16_t)((lon/1000) % 10000), 4, 1);
314
                        write_ndigit_number_u (17, 5, (uint16_t)((lon/10) % 100), 2, 1);
315
 
316
                        uint32_t lat = join_4_bytes(&UBX_buffer[8]);
317
                        write_ndigit_number_u (10, 6, (uint16_t)(lat/10000000), 2, 0);
318
                        lcd_printp_at (12,6, PSTR("."), 0);
319
                        write_ndigit_number_u (13, 6, (uint16_t)((lat/1000) % 10000), 4, 1);
320
                        write_ndigit_number_u (17, 6, (uint16_t)((lat/10) % 100), 2, 1);
321
 
322
                        uint16_t height = (uint16_t)(join_4_bytes(&UBX_buffer[16])/1000);
323
                        write_ndigit_number_u (11, 7, height, 4, 0);
324
                        lcd_printp_at (16,7, PSTR("m"), 0);
325
 
326
                }
327
        }
328
}
329
 
330
 
331
//--------------------------------------------------------------
332
union long_union
333
{
334
        uint32_t dword;
335
        uint8_t byte[4];
336
} longUnion;
337
 
338
 
339
//--------------------------------------------------------------
340
union int_union
341
{
342
        uint16_t dword;
343
        uint8_t byte[2];
344
} intUnion;
345
 
346
 
347
//--------------------------------------------------------------
348
uint32_t join_4_bytes(uint8_t Buffer[])
349
{
350
        longUnion.byte[0] = *Buffer;
351
        longUnion.byte[1] = *(Buffer+1);
352
        longUnion.byte[2] = *(Buffer+2);
353
        longUnion.byte[3] = *(Buffer+3);
354
        return (longUnion.dword);
355
}
356
 
357
 
358
//--------------------------------------------------------------
359
void checksum(uint8_t data)
360
{
361
        ck_a += data;
362
        ck_b += ck_a;
363
}
364