Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

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