Subversion Repositories NaviCtrl

Rev

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

Rev Author Line No. Line
41 ingob 1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + www.MikroKopter.com
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 7
// + Software Nutzungsbedingungen (english version: see below)
8
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
9
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
10
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool 
11
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
12
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
41 ingob 13
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 14
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
15
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
16
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
17
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
18
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
19
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
20
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
21
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand 
22
// + des Mitverschuldens offen.
23
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
24
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
25
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
26
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
27
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
28
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
29
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
30
// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
41 ingob 31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 32
// + Software LICENSING TERMS
41 ingob 33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 34
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
35
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware 
36
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
37
// + The Software may only be used with the Licensor's products.
38
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
39
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
40
// + agreement shall be the property of the Licensor.
41
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
42
// + features that can be used to identify the program may not be altered or defaced by the customer.
43
// + The customer shall be responsible for taking reasonable precautions
44
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
45
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
46
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
47
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
48
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
49
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
50
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
51
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
52
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
53
// + #### END OF LICENSING TERMS ####
54
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
41 ingob 55
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
263 killagreg 56
#include <string.h>
41 ingob 57
#include "91x_lib.h"
58
#include "uart1.h"
59
#include "ubx.h"
60
#include "led.h"
119 killagreg 61
#include "timer1.h"
41 ingob 62
 
63
// ------------------------------------------------------------------------------------------------
64
// defines
65
 
66
#define DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980 722819 // the year 0 does not exist!
67
#define DAYS_PER_YEAR           365
68
#define DAYS_PER_LEAPYEAR       366
69
#define DAYS_PER_4YEARS         1461    //((3 * DAYS_PER_YEAR) + DAYS_PER_LEAPYEAR) // years dividable by 4 are leap years
70
#define DAYS_PER_100YEARS       36524   //((25 * DAYS_PER_4YEARS) - 1) // years dividable by 100 are no leap years
71
#define DAYS_PER_400YEARS       146097  //((4 * DAYS_PER_100YEARS) + 1L) // but years dividable by 400 are leap years
72
#define SECONDS_PER_MINUTE      60
73
#define MINUTES_PER_HOUR        60
74
#define HOURS_PER_DAY           24
75
#define DAYS_PER_WEEK           7
76
#define SECONDS_PER_HOUR        3600    //(SECONDS_PER_MINUTE * MINUTES_PER_HOUR)
77
#define SECONDS_PER_DAY         86400   //(SECONDS_PER_HOUR * HOURS_PER_DAY)
78
#define SECONDS_PER_WEEK        604800  //(SECONDS_PER_DAY * DAYS_PER_WEEK)
79
 
80
// days per month in normal and leap years
81
const u32       Leap[ 13 ]      = { 0,  31,  60,  91, 121, 152, 182, 213, 244, 274, 305, 335, 366 };
82
const u32 Normal[ 13 ]  = { 0,  31,  59,  90, 120, 151, 181, 212, 243, 273, 304, 334, 365 };
83
 
212 killagreg 84
#define LEAP_SECONDS_FROM_1980  15
41 ingob 85
 
86
// message sync bytes
87
#define UBX_SYNC1_CHAR  0xB5
88
#define UBX_SYNC2_CHAR  0x62
245 killagreg 89
// protocoll identifiers
90
// navigation class
41 ingob 91
#define UBX_CLASS_NAV   0x01
92
#define UBX_ID_POSLLH   0x02
93
#define UBX_ID_SOL              0x06
94
#define UBX_ID_VELNED   0x12
247 killagreg 95
 
41 ingob 96
// ------------------------------------------------------------------------------------------------
97
// typedefs
98
 
99
 
100
// ubx parser state
101
typedef enum
102
{
103
        UBXSTATE_IDLE,
104
        UBXSTATE_SYNC1,
105
        UBXSTATE_SYNC2,
106
        UBXSTATE_CLASS,
107
        UBXSTATE_LEN1,
108
        UBXSTATE_LEN2,
109
        UBXSTATE_DATA,
110
        UBXSTATE_CKA,
111
        UBXSTATE_CKB
112
} ubxState_t;
113
 
114
typedef struct
115
{
116
        u32                     itow;           // ms GPS Millisecond Time of Week
117
        s32                     frac;           // ns remainder of rounded ms above
118
        s16                     week;           // GPS week
119
        u8                      GPSfix;         // GPSfix Type, range 0..6
120
        u8                      Flags;          // Navigation Status Flags
121
        s32                     ECEF_X;         // cm ECEF X coordinate
122
        s32                     ECEF_Y;         // cm ECEF Y coordinate
123
        s32                     ECEF_Z;         // cm ECEF Z coordinate
124
        u32                     PAcc;           // cm 3D Position Accuracy Estimate
125
        s32                     ECEFVX;         // cm/s ECEF X velocity
126
        s32                     ECEFVY;         // cm/s ECEF Y velocity
127
        s32                     ECEFVZ;         // cm/s ECEF Z velocity
128
        u32                     SAcc;           // cm/s Speed Accuracy Estimate
129
        u16                     PDOP;           // 0.01 Position DOP
130
        u8                      res1;           // reserved
131
        u8                      numSV;          // Number of SVs used in navigation solution
132
        u32                     res2;           // reserved
52 killagreg 133
        u8                      Status;     // invalid/newdata/processed
41 ingob 134
} __attribute__((packed)) ubx_nav_sol_t;
135
 
136
 
137
typedef struct
138
{
139
        u32                     itow;           // ms  GPS Millisecond Time of Week
140
        s32                     VEL_N;          // cm/s  NED north velocity
141
        s32                     VEL_E;          // cm/s  NED east velocity
142
        s32                     VEL_D;          // cm/s  NED down velocity
143
        u32                     Speed;          // cm/s  Speed (3-D)
144
        u32                     GSpeed;         // cm/s  Ground Speed (2-D)
145
        s32                     Heading;        // 1e-05 deg  Heading 2-D
146
        u32                     SAcc;           // cm/s  Speed Accuracy Estimate
147
        u32                     CAcc;           // deg  Course / Heading Accuracy Estimate
52 killagreg 148
        u8                      Status;         // invalid/newdata/processed
41 ingob 149
} __attribute__((packed)) ubx_nav_velned_t;
150
 
151
typedef struct
152
{
153
        u32                     itow;           // ms GPS Millisecond Time of Week
154
        s32                     LON;            // 1e-07 deg Longitude
155
        s32                     LAT;            // 1e-07 deg Latitude
156
        s32                     HEIGHT;         // mm Height above Ellipsoid
157
        s32                     HMSL;           // mm Height above mean sea level
158
        u32                     Hacc;           // mm Horizontal Accuracy Estimate
159
        u32                     Vacc;           // mm Vertical Accuracy Estimate
52 killagreg 160
        u8                      Status;         // invalid/newdata/processed
41 ingob 161
} __attribute__((packed)) ubx_nav_posllh_t;
162
 
163
//------------------------------------------------------------------------------------
164
// global variables
165
 
166
// local buffers for the incomming ubx messages
263 killagreg 167
ubx_nav_sol_t           UbxSol    = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
168
ubx_nav_posllh_t        UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
169
ubx_nav_velned_t        UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
170
ubxmsg_t                        UbxMsg;
41 ingob 171
 
172
// shared buffer
244 killagreg 173
gps_data_t              GPSData = {200,{0,0,0,INVALID},0,0,0,0,0,0,0, INVALID};
41 ingob 174
DateTime_t                      GPSDateTime = {0,0,0,0,0,0,0, INVALID};
175
 
146 killagreg 176
#define UBX_TIMEOUT 500 // 500 ms
177
u32 UBX_Timeout = 0;
41 ingob 178
 
179
//------------------------------------------------------------------------------------
180
// functions
181
 
182
u8 IsLeapYear(u16 year)
183
{
184
        if((year%400 == 0) || ( (year%4 == 0) && (year%100 != 0) ) ) return 1;
185
        else return 0;
186
}
187
/********************************************************/
188
/*  Calculates the UTC Time from the GPS week and tow   */
189
/********************************************************/
190
void SetGPSTime(DateTime_t * pTimeStruct)
191
{
192
        u32 Days, Seconds, Week;
193
        u16 YearPart;
194
        u32 * MonthDayTab = 0;
195
        u8 i;
196
 
197
 
198
        // if GPS data show valid time data
199
        if((UbxSol.Status != INVALID) && (UbxSol.Flags & FLAG_WKNSET) && (UbxSol.Flags & FLAG_TOWSET) )
200
        {
201
                Seconds = UbxSol.itow / 1000L;
202
                Week = (u32)UbxSol.week;
203
                // correct leap seconds since 1980
204
                if(Seconds < LEAP_SECONDS_FROM_1980)
205
                {
206
                        Week--;
207
                        Seconds = SECONDS_PER_WEEK - LEAP_SECONDS_FROM_1980 + Seconds;
208
                }
209
                else Seconds -= LEAP_SECONDS_FROM_1980;
210
 
211
                Days = DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980;
212
                Days += (Week * DAYS_PER_WEEK);
213
                Days += Seconds / SECONDS_PER_DAY; // seperate days from GPS seconds of week
214
 
215
                pTimeStruct->Year = 1;
216
                YearPart = (u16)(Days / DAYS_PER_400YEARS);
217
                pTimeStruct->Year += YearPart * 400;
218
                Days = Days % DAYS_PER_400YEARS;
219
                YearPart = (u16)(Days / DAYS_PER_100YEARS);
220
                pTimeStruct->Year += YearPart * 100;
221
                Days = Days % DAYS_PER_100YEARS;
222
                YearPart = (u16)(Days / DAYS_PER_4YEARS);
223
                pTimeStruct->Year += YearPart * 4;
224
                Days = Days % DAYS_PER_4YEARS;
225
                if(Days < (3* DAYS_PER_YEAR)) YearPart = (u16)(Days / DAYS_PER_YEAR);
226
                else YearPart = 3;
227
                pTimeStruct->Year += YearPart;
228
                // calculate remaining days of year
229
                Days -= (u32)(YearPart *  DAYS_PER_YEAR);
230
                Days += 1;
231
                // check if current year is a leap year
232
                if(IsLeapYear(pTimeStruct->Year)) MonthDayTab = (u32*)Leap;
233
                else MonthDayTab = (u32*)Normal;
234
            // seperate month and day from days of year
235
                for ( i = 0; i < 12; i++ )
236
                {
237
                        if ( (MonthDayTab[i]< Days) && (Days <= MonthDayTab[i+1]) )
238
                        {
239
                                pTimeStruct->Month = i+1;
240
                                pTimeStruct->Day = Days - MonthDayTab[i];
241
                                i = 12;
242
                        }
243
                }
244
                Seconds = Seconds % SECONDS_PER_DAY; // remaining seconds of current day
245
                pTimeStruct->Hour = (u8)(Seconds / SECONDS_PER_HOUR);
246
                Seconds = Seconds % SECONDS_PER_HOUR; // remaining seconds of current hour
247
                pTimeStruct->Min = (u8)(Seconds / SECONDS_PER_MINUTE);
248
                Seconds = Seconds % SECONDS_PER_MINUTE; // remaining seconds of current minute
249
                pTimeStruct->Sec = (u8)(Seconds);
250
                pTimeStruct->mSec  = (u16)(UbxSol.itow % 1000L);
251
                pTimeStruct->Valid = 1;
252
        }
253
        else
254
        {
255
                pTimeStruct->Valid = 0;
256
        }
257
}
258
 
259
 
260
 
261
/********************************************************/
262
/*                  Initialize UBX Parser               */
263
/********************************************************/
264
void UBX_Init(void)
265
{
266
        // mark msg buffers invalid
267
        UbxSol.Status = INVALID;
268
        UbxPosLlh.Status = INVALID;
269
        UbxVelNed.Status = INVALID;
247 killagreg 270
        UbxMsg.Status = INVALID;
41 ingob 271
        GPSData.Status = INVALID;
146 killagreg 272
 
273
        UBX_Timeout = SetDelay(2 * UBX_Timeout);
41 ingob 274
}
275
 
276
/********************************************************/
277
/*            Upate GPS data stcructure                 */
278
/********************************************************/
263 killagreg 279
void Update_GPSData(void)
41 ingob 280
{
263 killagreg 281
        static u32 last_itow = 0;
41 ingob 282
 
283
        // if a new set of ubx messages was collected
263 killagreg 284
        if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))
285
        {       // and the itow is equal (same time base)
286
                if((UbxSol.itow == UbxPosLlh.itow) && (UbxPosLlh.itow == UbxVelNed.itow))
41 ingob 287
                {
146 killagreg 288
                        UBX_Timeout = SetDelay(UBX_TIMEOUT);
61 holgerb 289
                        DebugOut.Analog[9]++;
263 killagreg 290
                        // update GPS data only if the status is INVALID or PROCESSED
291
                        if(GPSData.Status != NEWDATA)  
41 ingob 292
                        { // wait for new data at all neccesary ubx messages
293
                                GPSData.Status = INVALID;
244 killagreg 294
                                // update message cycle time
264 killagreg 295
                                GPSData.MsgCycleTime = (u16)(UbxSol.itow-last_itow);
296
                                last_itow = UbxSol.itow; // update last itow
281 holgerb 297
//      DebugOut.Analog[16] = GPSData.MsgCycleTime;
41 ingob 298
                                // NAV SOL
322 holgerb 299
                                GPSData.Flags =         (GPSData.Flags & 0xf0) | (UbxSol.Flags & 0x0f); // we take only the lower bits
41 ingob 300
                                GPSData.NumOfSats =                     UbxSol.numSV;
301
                                GPSData.SatFix =                                UbxSol.GPSfix;
302
                                GPSData.Position_Accuracy =             UbxSol.PAcc;
303
                                GPSData.Speed_Accuracy =                UbxSol.SAcc;
304
                                SetGPSTime(&SystemTime); // update system time
305
                                // NAV POSLLH
306
                                GPSData.Position.Status =               INVALID;
307
                                GPSData.Position.Longitude =    UbxPosLlh.LON;
308
                                GPSData.Position.Latitude =     UbxPosLlh.LAT;
309
                                GPSData.Position.Altitude =     UbxPosLlh.HMSL;
310
                                GPSData.Position.Status =               NEWDATA;
311
                                // NAV VELNED
312
                                GPSData.Speed_East =                    UbxVelNed.VEL_E;
313
                                GPSData.Speed_North =                   UbxVelNed.VEL_N;
314
                                GPSData.Speed_Top       =                       -UbxVelNed.VEL_D;
315
                                GPSData.Speed_Ground =                  UbxVelNed.GSpeed;
316
                                GPSData.Heading =                               UbxVelNed.Heading;
263 killagreg 317
 
41 ingob 318
                                GPSData.Status = NEWDATA; // new data available
319
                        } // EOF if(GPSData.Status != NEWDATA)
263 killagreg 320
                        // set state to collect new data
321
                        UbxSol.Status =                                 PROCESSED;      // ready for new data
322
                        UbxPosLlh.Status =                              PROCESSED;      // ready for new data
323
                        UbxVelNed.Status =                              PROCESSED;      // ready for new data
324
                } // EOF all itow are equal
325
        } // EOF all ubx messages received
392 holgerb 326
 
327
 
328
//GPSData.Position.Longitude = 1517409123L;  // Hamilton, Australia
329
//GPSData.Position.Latitude = -329294773L;       // Hamilton, Australia
330
//GPSData.Position.Longitude =-1556010020L;  // Alaska
331
//GPSData.Position.Latitude = 629581270L;        // Alaska
332
//GPSData.Position.Longitude =-584343419L;   // Buenos aires
333
//GPSData.Position.Latitude = -345464421L;   // Buenos aires
334
//GPSData.Position.Longitude =1683362691L;   // Neuseeland
335
//GPSData.Position.Latitude = -465945926L;   // Neuseeland
41 ingob 336
}
337
 
338
 
339
/********************************************************/
340
/*                   UBX Parser                         */
341
/********************************************************/
245 killagreg 342
void UBX_RxParser(u8 c)
41 ingob 343
{
344
        static ubxState_t ubxState = UBXSTATE_IDLE;
263 killagreg 345
        static ubxmsghdr_t RxHdr;
346
        static u8 RxData[UBX_MSG_DATA_SIZE];
347
        static u16 RxBytes = 0;
41 ingob 348
        static u8 cka, ckb;
349
 
350
 
351
        //state machine
352
        switch (ubxState)       // ubx message parser
353
        {
354
                case UBXSTATE_IDLE: // check 1st sync byte
355
                        if (c == UBX_SYNC1_CHAR) ubxState = UBXSTATE_SYNC1;
356
                        else ubxState = UBXSTATE_IDLE; // out of synchronization
357
                        break;
358
 
359
                case UBXSTATE_SYNC1: // check 2nd sync byte
360
                        if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2;
361
                        else ubxState = UBXSTATE_IDLE; // out of synchronization
362
                        break;
363
 
364
                case UBXSTATE_SYNC2: // check msg class to be NAV
263 killagreg 365
                        RxHdr.Class = c;
245 killagreg 366
                        ubxState = UBXSTATE_CLASS;
41 ingob 367
                        break;
368
 
369
                case UBXSTATE_CLASS: // check message identifier
263 killagreg 370
                        RxHdr.Id = c;
247 killagreg 371
                        ubxState = UBXSTATE_LEN1;
263 killagreg 372
                        cka = RxHdr.Class + RxHdr.Id;
373
                        ckb = RxHdr.Class + cka;
247 killagreg 374
                        break;
375
 
376
                case UBXSTATE_LEN1: // 1st message length byte
263 killagreg 377
                        RxHdr.Length = (u16)c; // lowbyte first
247 killagreg 378
                        cka += c;
379
                        ckb += cka;
380
                        ubxState = UBXSTATE_LEN2;
381
                        break;
382
 
383
                case UBXSTATE_LEN2: // 2nd message length byte
263 killagreg 384
                        RxHdr.Length += ((u16)c)<<8; // high byte last
385
                        if (RxHdr.Length >= UBX_MSG_DATA_SIZE)
41 ingob 386
                        {
263 killagreg 387
                                ubxState = UBXSTATE_IDLE;
41 ingob 388
                        }
263 killagreg 389
                        else
41 ingob 390
                        {
263 killagreg 391
                                cka += c;
392
                                ckb += cka;
393
                                RxBytes = 0;    // reset data byte counter
394
                                ubxState = UBXSTATE_DATA;
41 ingob 395
                        }
396
                        break;
397
 
398
                case UBXSTATE_DATA: // collecting data
263 killagreg 399
                        if (RxBytes < UBX_MSG_DATA_SIZE)
41 ingob 400
                        {
263 killagreg 401
                                RxData[RxBytes++] = c; // copy curent data byte if any space is left
41 ingob 402
                                cka += c;
403
                                ckb += cka;
263 killagreg 404
                                if (RxBytes >= RxHdr.Length) ubxState = UBXSTATE_CKA; // switch to next state if all data have been received
41 ingob 405
                        }
406
                        else // rx buffer overrun
407
                        {
408
                                ubxState = UBXSTATE_IDLE;
409
                        }
410
                        break;
411
 
412
                case UBXSTATE_CKA:
413
                        if (c == cka) ubxState = UBXSTATE_CKB;
414
                        else
415
                        {
416
                                ubxState = UBXSTATE_IDLE;
417
                        }
418
                        break;
419
 
420
                case UBXSTATE_CKB:
421
                        if (c == ckb)
263 killagreg 422
                        {       // checksum is ok
423
 
424
                                switch(RxHdr.Class)
425
                                {
426
                                        case UBX_CLASS_NAV:
427
                                                switch(RxHdr.Id)
428
                                                {
429
                                                        case UBX_ID_POSLLH: // geodetic position
430
                                                                memcpy((u8*)&UbxPosLlh, RxData, RxHdr.Length);
431
                                                                UbxPosLlh.Status = NEWDATA;
432
                                                                break;
433
 
434
                                                        case UBX_ID_VELNED: // velocity vector in tangent plane 
435
                                                                memcpy((u8*)&UbxVelNed, RxData, RxHdr.Length);
436
                                                                UbxVelNed.Status = NEWDATA;
437
                                                                break;
438
 
439
                                                        case UBX_ID_SOL: // navigation solution
440
                                                                memcpy((u8*)&UbxSol, RxData, RxHdr.Length);
441
                                                                UbxSol.Status = NEWDATA;
442
                                                                break;
443
 
444
                                                        default:
445
                                                                break;
446
                                                } // EOF switch(Id)
447
                                                Update_GPSData();
448
                                                break;
449
 
450
                                        default:
451
                                                break;
452
                                } // EOF switch(class)
453
 
454
                                // check generic msg filter
455
                                if(UbxMsg.Status != NEWDATA)
456
                                {       // msg buffer is free
457
                                        if(((UbxMsg.Hdr.Class&UbxMsg.ClassMask) == (RxHdr.Class&UbxMsg.ClassMask)) && ((UbxMsg.Hdr.Id&UbxMsg.IdMask) == (RxHdr.Id&UbxMsg.IdMask)))
458
                                        {       // msg matches to the filter criteria
459
                                                UbxMsg.Status = INVALID;
460
                                                UbxMsg.Hdr.Class = RxHdr.Class;
461
                                                UbxMsg.Hdr.Id = RxHdr.Id;
462
                                                UbxMsg.Hdr.Length = RxHdr.Length;
463
                                                if(UbxMsg.Hdr.Length <= UBX_MSG_DATA_SIZE)
464
                                                {       // copy data block
465
                                                        memcpy(UbxMsg.Data, RxData, RxHdr.Length);
466
                                                        UbxMsg.Status = NEWDATA;
467
                                                }      
468
                                        } // EOF filter matches
469
                                } // EOF != INVALID
470
                        }// EOF crc ok
41 ingob 471
                        ubxState = UBXSTATE_IDLE; // ready to parse new data
472
                        break;
473
 
474
                default: // unknown ubx state
475
                        ubxState = UBXSTATE_IDLE;
476
                        break;
477
 
478
        }
479
}
245 killagreg 480
 
247 killagreg 481
u8 UBX_CreateMsg(Buffer_t* pBuff, u8* pData, u16 Len)
245 killagreg 482
{
483
        u16 i;
484
        u8 cka = 0, ckb = 0;
485
        // check if buffer is available
486
        if(pBuff->Locked == TRUE) return(0);
487
        // check if buffer size is sufficient
488
        if(pBuff->Size < 8 + Len) return(0);
489
        // lock the buffer
490
        pBuff->Locked = TRUE;
491
        // start at begin 
492
        pBuff->Position = 0;
493
        pBuff->pData[pBuff->Position++] = UBX_SYNC1_CHAR;
494
        pBuff->pData[pBuff->Position++] = UBX_SYNC2_CHAR;
247 killagreg 495
        for(i=0;i<Len;i++)
496
        {
497
                pBuff->pData[pBuff->Position++] = pData[i];
498
        }
245 killagreg 499
        // calculate checksum
500
        for(i=2;i<pBuff->Position;i++)
501
        {
502
                cka += pBuff->pData[i];
503
                ckb += cka;
504
        }
505
        pBuff->pData[pBuff->Position++] = cka;
506
        pBuff->pData[pBuff->Position++] = ckb;
507
        pBuff->DataBytes = pBuff->Position;
508
        pBuff->Position = 0;  // reset buffer position for transmision
509
        return(1);
510
}
263 killagreg 511
/*
512
switch(ubxclass)
513
                        {
514
                                case UBX_CLASS_NAV:
515
                                        switch(ubxid)
516
                                        {
517
                                                case UBX_ID_POSLLH: // geodetic position
518
                                                        ubxSp =  (u8 *)&UbxPosLlh; // data start pointer
519
                                                        ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer
520
                                                        ubxStP = (u8 *)&UbxPosLlh.Status; // status pointer
521
                                                        break;
522
 
523
                                                case UBX_ID_SOL: // navigation solution
524
                                                        ubxSp =  (u8 *)&UbxSol; // data start pointer
525
                                                        ubxEp = (u8 *)(&UbxSol + 1); // data end pointer
526
                                                        ubxStP = (u8 *)&UbxSol.Status; // status pointer
527
                                                        break;
528
 
529
                                                case UBX_ID_VELNED: // velocity vector in tangent plane
530
                                                        ubxSp =  (u8 *)&UbxVelNed; // data start pointer
531
                                                        ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer
532
                                                        ubxStP = (u8 *)&UbxVelNed.Status; // status pointer
533
                                                        break;
534
 
535
                                                default:                        // unsupported identifier
536
                                                        ubxState = UBXSTATE_IDLE;
537
                                                        return;
538
                                        }
539
                                        break;
540
 
541
                                default: // other  classes
542
                                        if(UbxMsg.Status == NEWDATA) ubxState = UBXSTATE_IDLE;
543
                                        else if(((UbxMsg.Hdr.Class&UbxMsg.ClassMask) == (ubxclass&UbxMsg.ClassMask)) && ((UbxMsg.Hdr.Id&UbxMsg.IdMask) == (ubxid&UbxMsg.IdMask)))
544
                                        {       // buffer is free and message matches to filter criteria
545
                                                UbxMsg.Status = INVALID;
546
                                                UbxMsg.Hdr.Class = ubxclass;
547
                                                UbxMsg.Hdr.Id = ubxid;
548
                                                UbxMsg.Hdr.Length = msglen;
549
                                                ubxSp =  (u8 *)&(UbxMsg.Data);   // data start pointer
550
                                                ubxEp = (u8 *)(&UbxMsg + 1);    // data end pointer
551
                                                ubxStP = (u8 *)&UbxMsg.Status;  // status pointer      
552
                                        }
553
                                        else ubxState = UBXSTATE_IDLE;
554
                                        break;
555
                        }
556
*/
557