Subversion Repositories Projects

Rev

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