Subversion Repositories Projects

Rev

Rev 434 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
426 killagreg 1
#include <inttypes.h>
2
#include "ubx.h"
3
#include "timer0.h"
4
#include "uart0.h"
436 killagreg 5
#include "printf_P.h"
426 killagreg 6
 
7
 
8
// ------------------------------------------------------------------------------------------------
9
// defines
10
 
11
#define DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980 722819 // the year 0 does not exist!
12
#define DAYS_PER_YEAR           365
13
#define DAYS_PER_LEAPYEAR       366
14
#define DAYS_PER_4YEARS         1461    //((3 * DAYS_PER_YEAR) + DAYS_PER_LEAPYEAR) // years dividable by 4 are leap years
15
#define DAYS_PER_100YEARS       36524   //((25 * DAYS_PER_4YEARS) - 1) // years dividable by 100 are no leap years
16
#define DAYS_PER_400YEARS       146097  //((4 * DAYS_PER_100YEARS) + 1L) // but years dividable by 400 are leap years
17
#define SECONDS_PER_MINUTE      60
18
#define MINUTES_PER_HOUR        60
19
#define HOURS_PER_DAY           24
20
#define DAYS_PER_WEEK           7
21
#define SECONDS_PER_HOUR        3600    //(SECONDS_PER_MINUTE * MINUTES_PER_HOUR)
22
#define SECONDS_PER_DAY         86400   //(SECONDS_PER_HOUR * HOURS_PER_DAY)
23
#define SECONDS_PER_WEEK        604800  //(SECONDS_PER_DAY * DAYS_PER_WEEK)
24
 
25
// days per month in normal and leap years
26
const uint32_t   Leap[ 13 ] = { 0,  31,  60,  91, 121, 152, 182, 213, 244, 274, 305, 335, 366 };
27
const uint32_t Normal[ 13 ]     = { 0,  31,  59,  90, 120, 151, 181, 212, 243, 273, 304, 334, 365 };
28
 
29
#define LEAP_SECONDS_FROM_1980  15 // the last one was on the Dec 31th 2008
30
 
31
// message sync bytes
32
#define UBX_SYNC1_CHAR  0xB5
33
#define UBX_SYNC2_CHAR  0x62
34
// protocoll identifier
35
#define UBX_CLASS_NAV   0x01
36
// message id
37
#define UBX_ID_POSLLH   0x02
38
#define UBX_ID_SOL              0x06
39
#define UBX_ID_VELNED   0x12
40
 
41
// ------------------------------------------------------------------------------------------------
42
// typedefs
43
 
44
 
45
// ubx parser state
46
typedef enum
47
{
48
        UBXSTATE_IDLE,
49
        UBXSTATE_SYNC1,
50
        UBXSTATE_SYNC2,
51
        UBXSTATE_CLASS,
52
        UBXSTATE_LEN1,
53
        UBXSTATE_LEN2,
54
        UBXSTATE_DATA,
55
        UBXSTATE_CKA,
56
        UBXSTATE_CKB
57
} ubxState_t;
58
 
59
typedef struct
60
{
61
        uint32_t        itow;           // ms GPS Millisecond Time of Week
62
        int32_t         frac;           // ns remainder of rounded ms above
63
        int16_t         week;           // GPS week
64
        uint8_t         GPSfix;         // GPSfix Type, range 0..6
65
        uint8_t         Flags;          // Navigation Status Flags
66
        int32_t         ECEF_X;         // cm ECEF X coordinate
67
        int32_t         ECEF_Y;         // cm ECEF Y coordinate
68
        int32_t         ECEF_Z;         // cm ECEF Z coordinate
69
        int32_t         PAcc;           // cm 3D Position Accuracy Estimate
70
        int32_t         ECEFVX;         // cm/s ECEF X velocity
71
        int32_t         ECEFVY;         // cm/s ECEF Y velocity
72
        int32_t         ECEFVZ;         // cm/s ECEF Z velocity
73
        uint32_t        SAcc;           // cm/s Speed Accuracy Estimate
74
        uint16_t        PDOP;           // 0.01 Position DOP
75
        uint8_t         res1;           // reserved
76
        uint8_t         numSV;          // Number of SVs used in navigation solution
77
        uint32_t        res2;           // reserved
78
        uint8_t         Status;     // invalid/newdata/processed
79
} __attribute__((packed)) ubx_nav_sol_t;
80
 
81
 
82
typedef struct
83
{
84
        uint32_t        itow;           // ms  GPS Millisecond Time of Week
85
        int32_t         VEL_N;          // cm/s  NED north velocity
86
        int32_t         VEL_E;          // cm/s  NED east velocity
87
        int32_t         VEL_D;          // cm/s  NED down velocity
88
        int32_t         Speed;          // cm/s  Speed (3-D)
89
        int32_t         GSpeed;         // cm/s  Ground Speed (2-D)
90
        int32_t         Heading;        // 1e-05 deg  Heading 2-D
91
        uint32_t        SAcc;           // cm/s  Speed Accuracy Estimate
92
        uint32_t        CAcc;           // deg  Course / Heading Accuracy Estimate
93
        uint8_t         Status;         // invalid/newdata/processed
94
} __attribute__((packed)) ubx_nav_velned_t;
95
 
96
typedef struct
97
{
98
        uint32_t        itow;           // ms GPS Millisecond Time of Week
99
        int32_t         LON;            // 1e-07 deg Longitude
100
        int32_t         LAT;            // 1e-07 deg Latitude
101
        int32_t         HEIGHT;         // mm Height above Ellipsoid
102
        int32_t         HMSL;           // mm Height above mean sea level
103
        uint32_t        Hacc;           // mm Horizontal Accuracy Estimate
104
        uint32_t        Vacc;           // mm Vertical Accuracy Estimate
105
        uint8_t         Status;         // invalid/newdata/processed
106
} __attribute__((packed)) ubx_nav_posllh_t;
107
 
108
 
109
 
110
//------------------------------------------------------------------------------------
111
// global variables
112
 
113
// local buffers for the incomming ubx messages
114
volatile ubx_nav_sol_t          UbxSol    = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
115
volatile ubx_nav_posllh_t       UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
116
volatile ubx_nav_velned_t       UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
117
 
118
uint16_t CheckGPSOkay = 0;
119
 
120
// shared buffer
121
gps_data_t              GPSData = {{0,0,0,INVALID},0,0,0,0,0,0,0, INVALID};
122
 
123
//------------------------------------------------------------------------------------
124
// functions
125
 
126
uint8_t IsLeapYear(uint16_t year)
127
{
128
        if((year%400 == 0) || ( (year%4 == 0) && (year%100 != 0) ) ) return 1;
129
        else return 0;
130
}
131
 
132
/********************************************************/
133
/*  Calculates the UTC Time from the GPS week and tow   */
134
/********************************************************/
135
void SetGPSTime(DateTime_t * pTimeStruct)
136
{
137
        uint32_t Days, Seconds, Week;
138
        uint16_t YearPart;
139
        uint32_t * MonthDayTab = 0;
140
        uint8_t  i;
141
 
142
 
143
 
144
        // if GPS data show valid time data
145
        if((UbxSol.Status != INVALID) && (UbxSol.Flags & FLAG_WKNSET) && (UbxSol.Flags & FLAG_TOWSET) )
146
        {
147
                Seconds = UbxSol.itow / 1000L;
148
                Week = (uint32_t)UbxSol.week;
149
                // correct leap seconds since 1980
150
                if(Seconds < LEAP_SECONDS_FROM_1980)
151
                {
152
                        Week--;
153
                        Seconds = SECONDS_PER_WEEK - LEAP_SECONDS_FROM_1980 + Seconds;
154
                }
155
                else Seconds -= LEAP_SECONDS_FROM_1980;
156
 
157
                Days = DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980;
158
                Days += (Week * DAYS_PER_WEEK);
159
                Days += Seconds / SECONDS_PER_DAY; // seperate days from GPS seconds of week
160
 
161
                pTimeStruct->Year = 1;
162
                YearPart = (uint16_t)(Days / DAYS_PER_400YEARS);
163
                pTimeStruct->Year += YearPart * 400;
164
                Days = Days % DAYS_PER_400YEARS;
165
                YearPart = (uint16_t)(Days / DAYS_PER_100YEARS);
166
                pTimeStruct->Year += YearPart * 100;
167
                Days = Days % DAYS_PER_100YEARS;
168
                YearPart = (uint16_t)(Days / DAYS_PER_4YEARS);
169
                pTimeStruct->Year += YearPart * 4;
170
                Days = Days % DAYS_PER_4YEARS;
171
                if(Days < (3* DAYS_PER_YEAR)) YearPart = (uint16_t)(Days / DAYS_PER_YEAR);
172
                else YearPart = 3;
173
                pTimeStruct->Year += YearPart;
174
                // calculate remaining days of year
175
                Days -= (uint32_t)(YearPart *  DAYS_PER_YEAR);
176
                Days += 1;
177
                // check if current year is a leap year
178
                if(IsLeapYear(pTimeStruct->Year)) MonthDayTab = (uint32_t*)Leap;
179
                else MonthDayTab = (uint32_t*)Normal;
180
            // seperate month and day from days of year
181
                for ( i = 0; i < 12; i++ )
182
                {
183
                        if ( (MonthDayTab[i]< Days) && (Days <= MonthDayTab[i+1]) )
184
                        {
185
                                pTimeStruct->Month = i+1;
186
                                pTimeStruct->Day = Days - MonthDayTab[i];
187
                                i = 12;
188
                        }
189
                }
190
                Seconds = Seconds % SECONDS_PER_DAY; // remaining seconds of current day
191
                pTimeStruct->Hour = (uint8_t)(Seconds / SECONDS_PER_HOUR);
192
                Seconds = Seconds % SECONDS_PER_HOUR; // remaining seconds of current hour
193
                pTimeStruct->Min = (uint8_t)(Seconds / SECONDS_PER_MINUTE);
194
                Seconds = Seconds % SECONDS_PER_MINUTE; // remaining seconds of current minute
195
                pTimeStruct->Sec = (uint8_t)(Seconds);
196
                pTimeStruct->mSec  = (uint16_t)(UbxSol.itow % 1000L);
197
                pTimeStruct->Valid = 1;
198
        }
199
        else
200
        {
201
                pTimeStruct->Valid = 0;
202
        }
203
}
204
 
205
 
206
 
207
/********************************************************/
208
/*                  Initialize UBX Parser               */
209
/********************************************************/
210
void UBX_Init(void)
211
{
434 killagreg 212
        printf("\r\n UBX init...");
426 killagreg 213
        // mark msg buffers invalid
214
        UbxSol.Status = INVALID;
215
        UbxPosLlh.Status = INVALID;
216
        UbxVelNed.Status = INVALID;
217
        GPSData.Status = INVALID;
434 killagreg 218
        printf("ok");
426 killagreg 219
}
220
 
221
/********************************************************/
222
/*            Upate GPS data stcructure                 */
223
/********************************************************/
224
void Update_GPSData (void)
225
{
226
        static uint16_t Ubx_Timeout = 0;
227
        static uint8_t  Msg_Count = 0;
228
 
229
        // the timeout is used to detect the delay between two message sets
230
        // and is used for synchronisation so that always a set is collected
231
        // that belongs together
232
        // _______NAVSOL|POSLLH|VELNED|___________________NAVSOL|POSLLH|VELNED|_____________
233
        //              |  8ms | 8ms  |         184 ms          |      |      |
234
        // msg_count:   0      1      2                         0      1      2
235
 
236
        if(CheckDelay(Ubx_Timeout))     Msg_Count = 0;
237
        else Msg_Count++;
238
        Ubx_Timeout = SetDelay(100); // reset ubx msg timeout
239
 
240
        // if a new set of ubx messages was collected
241
        if((Msg_Count >= 2))
242
        {       // if set is complete
243
                if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))
244
                {
245
                        CheckGPSOkay++;
246
                        // update GPS data only if the status is INVALID or PROCESSED  and the last ubx message was received within less than 100 ms
247
                        if(GPSData.Status != NEWDATA) // if last data were processed
248
                        { // wait for new data at all neccesary ubx messages
249
                                GPSData.Status = INVALID;
250
                                // NAV SOL
251
                                GPSData.Flags =                                 UbxSol.Flags;
252
                                GPSData.NumOfSats =                     UbxSol.numSV;
253
                                GPSData.SatFix =                                UbxSol.GPSfix;
254
                                GPSData.Position_Accuracy =             UbxSol.PAcc;
255
                                GPSData.Speed_Accuracy =                UbxSol.SAcc;
256
                                SetGPSTime(&SystemTime); // update system time
257
                                // NAV POSLLH
258
                                GPSData.Position.Status =               INVALID;
259
                                GPSData.Position.Longitude =    UbxPosLlh.LON;
260
                                GPSData.Position.Latitude =     UbxPosLlh.LAT;
261
                                GPSData.Position.Altitude =     UbxPosLlh.HMSL;
262
                                GPSData.Position.Status =               NEWDATA;
263
                                // NAV VELNED
264
                                GPSData.Speed_East =                    UbxVelNed.VEL_E;
265
                                GPSData.Speed_North =                   UbxVelNed.VEL_N;
266
                                GPSData.Speed_Top       =                       -UbxVelNed.VEL_D;
267
                                GPSData.Speed_Ground =                  UbxVelNed.GSpeed;
268
                                GPSData.Heading =                               UbxVelNed.Heading;
269
 
270
                                GPSData.Status = NEWDATA; // new data available
271
                        } // EOF if(GPSData.Status != NEWDATA)
272
                } // EOF all ubx messages received
273
                // set state to collect new data
274
                UbxSol.Status =                                 PROCESSED;      // ready for new data
275
                UbxPosLlh.Status =                              PROCESSED;      // ready for new data
276
                UbxVelNed.Status =                              PROCESSED;      // ready for new data
277
        }
278
}
279
 
280
 
281
/********************************************************/
282
/*                   UBX Parser                         */
283
/********************************************************/
284
void UBX_Parser(uint8_t c)
285
{
286
        static ubxState_t ubxState = UBXSTATE_IDLE;
287
        static uint16_t msglen;
288
        static uint8_t cka, ckb;
289
        static uint8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered
290
 
291
 
292
        //state machine
293
        switch (ubxState)       // ubx message parser
294
        {
295
                case UBXSTATE_IDLE: // check 1st sync byte
296
                        if (c == UBX_SYNC1_CHAR) ubxState = UBXSTATE_SYNC1;
297
                        else ubxState = UBXSTATE_IDLE; // out of synchronization
298
                        break;
299
 
300
                case UBXSTATE_SYNC1: // check 2nd sync byte
301
                        if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2;
302
                        else ubxState = UBXSTATE_IDLE; // out of synchronization
303
                        break;
304
 
305
                case UBXSTATE_SYNC2: // check msg class to be NAV
306
                        if (c == UBX_CLASS_NAV) ubxState = UBXSTATE_CLASS;
307
                        else ubxState = UBXSTATE_IDLE; // unsupported message class
308
                        break;
309
 
310
                case UBXSTATE_CLASS: // check message identifier
311
                        switch(c)
312
                        {
313
                                case UBX_ID_POSLLH: // geodetic position
314
                                        ubxP =  (uint8_t *)&UbxPosLlh; // data start pointer
315
                                        ubxEp = (uint8_t *)(&UbxPosLlh + 1); // data end pointer
316
                                        ubxSp = (uint8_t *)&UbxPosLlh.Status; // status pointer
317
                                        break;
318
 
319
                                case UBX_ID_SOL: // navigation solution
320
                                        ubxP =  (uint8_t *)&UbxSol; // data start pointer
321
                                        ubxEp = (uint8_t *)(&UbxSol + 1); // data end pointer
322
                                        ubxSp = (uint8_t *)&UbxSol.Status; // status pointer
323
                                        break;
324
 
325
                                case UBX_ID_VELNED: // velocity vector in tangent plane
326
                                        ubxP =  (uint8_t *)&UbxVelNed; // data start pointer
327
                                        ubxEp = (uint8_t *)(&UbxVelNed + 1); // data end pointer
328
                                        ubxSp = (uint8_t *)&UbxVelNed.Status; // status pointer
329
                                        break;
330
 
331
                                default:                        // unsupported identifier
332
                                        ubxState = UBXSTATE_IDLE;
333
                                        break;
334
                        }
335
                        if (ubxState != UBXSTATE_IDLE)
336
                        {
337
                                ubxState = UBXSTATE_LEN1;
338
                                cka = UBX_CLASS_NAV + c;
339
                                ckb = UBX_CLASS_NAV + cka;
340
                        }
341
                        break;
342
 
343
                case UBXSTATE_LEN1: // 1st message length byte
344
                        msglen = (uint16_t)c; // lowbyte first
345
                        cka += c;
346
                        ckb += cka;
347
                        ubxState = UBXSTATE_LEN2;
348
                        break;
349
 
350
                case UBXSTATE_LEN2: // 2nd message length byte
351
                        msglen += ((uint16_t)c)<<8; // high byte last
352
                        cka += c;
353
                        ckb += cka;
354
                        // if the old data are not processed so far then break parsing now
355
                        // to avoid writing new data in ISR during reading by another function
356
                        if ( *ubxSp == NEWDATA )
357
                        {
358
                                ubxState = UBXSTATE_IDLE;
359
                                Update_GPSData(); //update GPS info respectively
360
                        }
361
                        else // data invalid or allready processd
362
                        {
363
                                *ubxSp = INVALID; // mark invalid during buffer filling
364
                                ubxState = UBXSTATE_DATA;
365
                        }
366
                        break;
367
 
368
                case UBXSTATE_DATA: // collecting data
369
                        if (ubxP < ubxEp)
370
                        {
371
                                *ubxP++ = c; // copy curent data byte if any space is left
372
                                cka += c;
373
                                ckb += cka;
374
                                if (--msglen == 0)      ubxState = UBXSTATE_CKA; // switch to next state if all data was read
375
                        }
376
                        else // rx buffer overrun
377
                        {
378
                                ubxState = UBXSTATE_IDLE;
379
                        }
380
                        break;
381
 
382
                case UBXSTATE_CKA:
383
                        if (c == cka) ubxState = UBXSTATE_CKB;
384
                        else
385
                        {
386
                                *ubxSp = INVALID;
387
                                ubxState = UBXSTATE_IDLE;
388
                        }
389
                        break;
390
 
391
                case UBXSTATE_CKB:
392
                        if (c == ckb)
393
                        {
394
                                *ubxSp = NEWDATA; // new data are valid
395
                                Update_GPSData(); //update GPS info respectively
396
                        }
397
                        else
398
                        {       // if checksum not match then set data invalid
399
                                *ubxSp = INVALID;
400
                        }
401
                        ubxState = UBXSTATE_IDLE; // ready to parse new data
402
                        break;
403
 
404
                default: // unknown ubx state
405
                        ubxState = UBXSTATE_IDLE;
406
                        break;
407
 
408
        }
409
}