Rev 244 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 244 | Rev 245 | ||
---|---|---|---|
Line 52... | Line 52... | ||
52 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
52 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
53 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
53 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
54 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
54 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
55 | // + POSSIBILITY OF SUCH DAMAGE. |
55 | // + POSSIBILITY OF SUCH DAMAGE. |
56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 57 | #include <string.h> |
|
57 | #include "91x_lib.h" |
58 | #include "91x_lib.h" |
58 | #include "uart1.h" |
59 | #include "uart1.h" |
59 | #include "ubx.h" |
60 | #include "ubx.h" |
60 | #include "led.h" |
61 | #include "led.h" |
61 | #include "timer1.h" |
62 | #include "timer1.h" |
Line 84... | Line 85... | ||
84 | #define LEAP_SECONDS_FROM_1980 15 |
85 | #define LEAP_SECONDS_FROM_1980 15 |
Line 85... | Line 86... | ||
85 | 86 | ||
86 | // message sync bytes |
87 | // message sync bytes |
87 | #define UBX_SYNC1_CHAR 0xB5 |
88 | #define UBX_SYNC1_CHAR 0xB5 |
88 | #define UBX_SYNC2_CHAR 0x62 |
89 | #define UBX_SYNC2_CHAR 0x62 |
- | 90 | // protocoll identifiers |
|
- | 91 | #define UBX_CLASS_UNDEF 0x00 |
|
89 | // protocoll identifier |
92 | // navigation class |
90 | #define UBX_CLASS_NAV 0x01 |
- | |
91 | // message id |
93 | #define UBX_CLASS_NAV 0x01 |
92 | #define UBX_ID_POSLLH 0x02 |
94 | #define UBX_ID_POSLLH 0x02 |
93 | #define UBX_ID_SOL 0x06 |
95 | #define UBX_ID_SOL 0x06 |
94 | #define UBX_ID_VELNED 0x12 |
- | |
- | 96 | #define UBX_ID_VELNED 0x12 |
|
- | 97 | // acknowledge class |
|
- | 98 | #define UBX_CLASS_ACK 0x05 |
|
- | 99 | #define UBX_ID_ACK_NAK 0x00 |
|
95 | 100 | #define UBX_ID_ACK_ACK 0x01 |
|
96 | // ------------------------------------------------------------------------------------------------ |
101 | // ------------------------------------------------------------------------------------------------ |
Line 97... | Line 102... | ||
97 | // typedefs |
102 | // typedefs |
Line 158... | Line 163... | ||
158 | u32 Hacc; // mm Horizontal Accuracy Estimate |
163 | u32 Hacc; // mm Horizontal Accuracy Estimate |
159 | u32 Vacc; // mm Vertical Accuracy Estimate |
164 | u32 Vacc; // mm Vertical Accuracy Estimate |
160 | u8 Status; // invalid/newdata/processed |
165 | u8 Status; // invalid/newdata/processed |
161 | } __attribute__((packed)) ubx_nav_posllh_t; |
166 | } __attribute__((packed)) ubx_nav_posllh_t; |
Line 162... | Line -... | ||
162 | - | ||
163 | - | ||
164 | 167 | ||
165 | //------------------------------------------------------------------------------------ |
168 | //------------------------------------------------------------------------------------ |
Line 166... | Line 169... | ||
166 | // global variables |
169 | // global variables |
167 | 170 | ||
168 | // local buffers for the incomming ubx messages |
171 | // local buffers for the incomming ubx messages |
169 | volatile ubx_nav_sol_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID}; |
172 | volatile ubx_nav_sol_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID}; |
- | 173 | volatile ubx_nav_posllh_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID}; |
|
Line 170... | Line 174... | ||
170 | volatile ubx_nav_posllh_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID}; |
174 | volatile ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID}; |
171 | volatile ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID}; |
175 | volatile ubx_ack_t UbxAck = {0,0,0,INVALID}; |
172 | 176 | ||
Line 266... | Line 270... | ||
266 | { |
270 | { |
267 | // mark msg buffers invalid |
271 | // mark msg buffers invalid |
268 | UbxSol.Status = INVALID; |
272 | UbxSol.Status = INVALID; |
269 | UbxPosLlh.Status = INVALID; |
273 | UbxPosLlh.Status = INVALID; |
270 | UbxVelNed.Status = INVALID; |
274 | UbxVelNed.Status = INVALID; |
- | 275 | UbxAck.Status = INVALID; |
|
271 | GPSData.Status = INVALID; |
276 | GPSData.Status = INVALID; |
Line 272... | Line 277... | ||
272 | 277 | ||
273 | UBX_Timeout = SetDelay(2 * UBX_Timeout); |
278 | UBX_Timeout = SetDelay(2 * UBX_Timeout); |
Line 343... | Line 348... | ||
343 | 348 | ||
344 | 349 | ||
345 | /********************************************************/ |
350 | /********************************************************/ |
346 | /* UBX Parser */ |
351 | /* UBX Parser */ |
347 | /********************************************************/ |
352 | /********************************************************/ |
348 | void UBX_Parser(u8 c) |
353 | void UBX_RxParser(u8 c) |
- | 354 | { |
|
349 | { |
355 | static ubxState_t ubxState = UBXSTATE_IDLE; |
350 | static ubxState_t ubxState = UBXSTATE_IDLE; |
356 | static u8 ubxclass = UBX_CLASS_UNDEF; |
351 | static u16 msglen; |
357 | static u16 msglen; |
Line 365... | Line 371... | ||
365 | if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2; |
371 | if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2; |
366 | else ubxState = UBXSTATE_IDLE; // out of synchronization |
372 | else ubxState = UBXSTATE_IDLE; // out of synchronization |
367 | break; |
373 | break; |
Line 368... | Line 374... | ||
368 | 374 | ||
369 | case UBXSTATE_SYNC2: // check msg class to be NAV |
375 | case UBXSTATE_SYNC2: // check msg class to be NAV |
370 | if (c == UBX_CLASS_NAV) ubxState = UBXSTATE_CLASS; |
376 | ubxclass = c; |
371 | else ubxState = UBXSTATE_IDLE; // unsupported message class |
377 | ubxState = UBXSTATE_CLASS; |
Line 372... | Line 378... | ||
372 | break; |
378 | break; |
373 | 379 | ||
374 | case UBXSTATE_CLASS: // check message identifier |
380 | case UBXSTATE_CLASS: // check message identifier |
- | 381 | switch(ubxclass) |
|
- | 382 | { |
|
- | 383 | case UBX_CLASS_NAV: |
|
375 | switch(c) |
384 | switch(c) |
376 | { |
385 | { |
377 | case UBX_ID_POSLLH: // geodetic position |
386 | case UBX_ID_POSLLH: // geodetic position |
378 | ubxP = (u8 *)&UbxPosLlh; // data start pointer |
387 | ubxP = (u8 *)&UbxPosLlh; // data start pointer |
379 | ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer |
388 | ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer |
380 | ubxSp = (u8 *)&UbxPosLlh.Status; // status pointer |
389 | ubxSp = (u8 *)&UbxPosLlh.Status; // status pointer |
381 | break; |
390 | break; |
382 | 391 | ||
383 | case UBX_ID_SOL: // navigation solution |
392 | case UBX_ID_SOL: // navigation solution |
384 | ubxP = (u8 *)&UbxSol; // data start pointer |
393 | ubxP = (u8 *)&UbxSol; // data start pointer |
- | 394 | ubxEp = (u8 *)(&UbxSol + 1); // data end pointer |
|
- | 395 | ubxSp = (u8 *)&UbxSol.Status; // status pointer |
|
- | 396 | break; |
|
- | 397 | ||
- | 398 | case UBX_ID_VELNED: // velocity vector in tangent plane |
|
- | 399 | ubxP = (u8 *)&UbxVelNed; // data start pointer |
|
- | 400 | ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer |
|
- | 401 | ubxSp = (u8 *)&UbxVelNed.Status; // status pointer |
|
- | 402 | break; |
|
- | 403 | ||
- | 404 | default: // unsupported identifier |
|
- | 405 | ubxState = UBXSTATE_IDLE; |
|
385 | ubxEp = (u8 *)(&UbxSol + 1); // data end pointer |
406 | return; |
Line -... | Line 407... | ||
- | 407 | } |
|
- | 408 | break; |
|
- | 409 | ||
- | 410 | case UBX_CLASS_ACK: |
|
- | 411 | switch(c) |
|
- | 412 | { |
|
- | 413 | case UBX_ID_ACK_ACK: |
|
386 | ubxSp = (u8 *)&UbxSol.Status; // status pointer |
414 | UbxAck.Ack = 1; |
- | 415 | break; |
|
- | 416 | ||
- | 417 | case UBX_ID_ACK_NAK: |
|
- | 418 | UbxAck.Ack = 0; |
|
- | 419 | break; |
|
- | 420 | ||
- | 421 | default: |
|
387 | break; |
422 | ubxState = UBXSTATE_IDLE; |
388 | 423 | return; |
|
389 | case UBX_ID_VELNED: // velocity vector in tangent plane |
424 | } |
390 | ubxP = (u8 *)&UbxVelNed; // data start pointer |
425 | ubxP = (u8 *)&(UbxAck.clsID); // data start pointer |
Line 391... | Line 426... | ||
391 | ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer |
426 | ubxEp = (u8 *)(&UbxAck + 1); // data end pointer |
392 | ubxSp = (u8 *)&UbxVelNed.Status; // status pointer |
427 | ubxSp = (u8 *)&UbxAck.Status; // status pointer |
393 | break; |
428 | break; |
394 | 429 | ||
395 | default: // unsupported identifier |
430 | default: // unsupported class |
396 | ubxState = UBXSTATE_IDLE; |
431 | ubxState = UBXSTATE_IDLE; |
Line 418... | Line 453... | ||
418 | // if the old data are not processed so far then break parsing now |
453 | // if the old data are not processed so far then break parsing now |
419 | // to avoid writing new data in ISR during reading by another function |
454 | // to avoid writing new data in ISR during reading by another function |
420 | if ( *ubxSp == NEWDATA ) |
455 | if ( *ubxSp == NEWDATA ) |
421 | { |
456 | { |
422 | ubxState = UBXSTATE_IDLE; |
457 | ubxState = UBXSTATE_IDLE; |
423 | Update_GPSData(); //update GPS info respectively |
458 | if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively |
424 | } |
459 | } |
425 | else // data invalid or allready processd |
460 | else // data invalid or allready processd |
426 | { |
461 | { |
427 | *ubxSp = INVALID; // mark invalid during buffer filling |
462 | *ubxSp = INVALID; // mark invalid during buffer filling |
428 | ubxState = UBXSTATE_DATA; |
463 | ubxState = UBXSTATE_DATA; |
Line 454... | Line 489... | ||
454 | 489 | ||
455 | case UBXSTATE_CKB: |
490 | case UBXSTATE_CKB: |
456 | if (c == ckb) |
491 | if (c == ckb) |
457 | { |
492 | { |
458 | *ubxSp = NEWDATA; // new data are valid |
493 | *ubxSp = NEWDATA; // new data are valid |
459 | Update_GPSData(); //update GPS info respectively |
494 | if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively |
460 | } |
495 | } |
461 | else |
496 | else |
462 | { // if checksum not match then set data invalid |
497 | { // if checksum not match then set data invalid |
463 | *ubxSp = INVALID; |
498 | *ubxSp = INVALID; |
Line 469... | Line 504... | ||
469 | ubxState = UBXSTATE_IDLE; |
504 | ubxState = UBXSTATE_IDLE; |
470 | break; |
505 | break; |
Line 471... | Line 506... | ||
471 | 506 | ||
472 | } |
507 | } |
- | 508 | } |
|
- | 509 | ||
- | 510 | u8 UBX_CreateMsg(Buffer_t* pBuff, u8 Class, u8 Id, u8* pData, u16 Len) |
|
- | 511 | { |
|
- | 512 | u16 i; |
|
- | 513 | u8 cka = 0, ckb = 0; |
|
- | 514 | // check if buffer is available |
|
- | 515 | if(pBuff->Locked == TRUE) return(0); |
|
- | 516 | // check if buffer size is sufficient |
|
- | 517 | if(pBuff->Size < 8 + Len) return(0); |
|
- | 518 | // lock the buffer |
|
- | 519 | pBuff->Locked = TRUE; |
|
- | 520 | // start at begin |
|
- | 521 | pBuff->Position = 0; |
|
- | 522 | pBuff->pData[pBuff->Position++] = UBX_SYNC1_CHAR; |
|
- | 523 | pBuff->pData[pBuff->Position++] = UBX_SYNC2_CHAR; |
|
- | 524 | pBuff->pData[pBuff->Position++] = Class; |
|
- | 525 | pBuff->pData[pBuff->Position++] = Id; |
|
- | 526 | pBuff->pData[pBuff->Position++] = (u8)(Len); |
|
- | 527 | pBuff->pData[pBuff->Position++] = (u8)(Len>>8); |
|
- | 528 | memcpy(&(pBuff->pData[pBuff->Position]), pData, Len); |
|
- | 529 | pBuff->Position += Len; |
|
- | 530 | // calculate checksum |
|
- | 531 | for(i=2;i<pBuff->Position;i++) |
|
- | 532 | { |
|
- | 533 | cka += pBuff->pData[i]; |
|
- | 534 | ckb += cka; |
|
- | 535 | } |
|
- | 536 | pBuff->pData[pBuff->Position++] = cka; |
|
- | 537 | pBuff->pData[pBuff->Position++] = ckb; |
|
- | 538 | pBuff->DataBytes = pBuff->Position; |
|
- | 539 | pBuff->Position = 0; // reset buffer position for transmision |
|
- | 540 | return(1); |