Rev 245 | Rev 261 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 245 | Rev 247 | ||
|---|---|---|---|
| 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> |
- | |
| 58 | #include "91x_lib.h" |
57 | #include "91x_lib.h" |
| 59 | #include "uart1.h" |
58 | #include "uart1.h" |
| 60 | #include "ubx.h" |
59 | #include "ubx.h" |
| 61 | #include "led.h" |
60 | #include "led.h" |
| 62 | #include "timer1.h" |
61 | #include "timer1.h" |
| Line 86... | Line 85... | ||
| 86 | 85 | ||
| 87 | // message sync bytes |
86 | // message sync bytes |
| 88 | #define UBX_SYNC1_CHAR 0xB5 |
87 | #define UBX_SYNC1_CHAR 0xB5 |
| 89 | #define UBX_SYNC2_CHAR 0x62 |
88 | #define UBX_SYNC2_CHAR 0x62 |
| 90 | // protocoll identifiers |
- | |
| 91 | #define UBX_CLASS_UNDEF 0x00 |
89 | // protocoll identifiers |
| 92 | // navigation class |
90 | // navigation class |
| 93 | #define UBX_CLASS_NAV 0x01 |
91 | #define UBX_CLASS_NAV 0x01 |
| 94 | #define UBX_ID_POSLLH 0x02 |
92 | #define UBX_ID_POSLLH 0x02 |
| 95 | #define UBX_ID_SOL 0x06 |
93 | #define UBX_ID_SOL 0x06 |
| 96 | #define UBX_ID_VELNED 0x12 |
- | |
| 97 | // acknowledge class |
- | |
| 98 | #define UBX_CLASS_ACK 0x05 |
- | |
| 99 | #define UBX_ID_ACK_NAK 0x00 |
- | |
| - | 94 | #define UBX_ID_VELNED 0x12 |
|
| 100 | #define UBX_ID_ACK_ACK 0x01 |
95 | |
| 101 | // ------------------------------------------------------------------------------------------------ |
96 | // ------------------------------------------------------------------------------------------------ |
| Line 102... | Line 97... | ||
| 102 | // typedefs |
97 | // typedefs |
| Line 170... | Line 165... | ||
| 170 | 165 | ||
| 171 | // local buffers for the incomming ubx messages |
166 | // local buffers for the incomming ubx messages |
| 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}; |
167 | 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}; |
168 | 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}; |
169 | volatile ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID}; |
| Line 175... | Line 170... | ||
| 175 | volatile ubx_ack_t UbxAck = {0,0,0,INVALID}; |
170 | volatile ubxmsg_t UbxMsg; |
| 176 | 171 | ||
| 177 | // shared buffer |
172 | // shared buffer |
| Line 270... | Line 265... | ||
| 270 | { |
265 | { |
| 271 | // mark msg buffers invalid |
266 | // mark msg buffers invalid |
| 272 | UbxSol.Status = INVALID; |
267 | UbxSol.Status = INVALID; |
| 273 | UbxPosLlh.Status = INVALID; |
268 | UbxPosLlh.Status = INVALID; |
| 274 | UbxVelNed.Status = INVALID; |
269 | UbxVelNed.Status = INVALID; |
| 275 | UbxAck.Status = INVALID; |
270 | UbxMsg.Status = INVALID; |
| 276 | GPSData.Status = INVALID; |
271 | GPSData.Status = INVALID; |
| Line 277... | Line 272... | ||
| 277 | 272 | ||
| 278 | UBX_Timeout = SetDelay(2 * UBX_Timeout); |
273 | UBX_Timeout = SetDelay(2 * UBX_Timeout); |
| Line 351... | Line 346... | ||
| 351 | /* UBX Parser */ |
346 | /* UBX Parser */ |
| 352 | /********************************************************/ |
347 | /********************************************************/ |
| 353 | void UBX_RxParser(u8 c) |
348 | 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 u8 ubxclass; |
| - | 352 | static u8 ubxid; |
|
| 357 | static u16 msglen; |
353 | static u16 msglen; |
| 358 | static u8 cka, ckb; |
354 | static u8 cka, ckb; |
| 359 | static u8 *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered |
355 | static u8 *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered |
| Line 376... | Line 372... | ||
| 376 | ubxclass = c; |
372 | ubxclass = c; |
| 377 | ubxState = UBXSTATE_CLASS; |
373 | ubxState = UBXSTATE_CLASS; |
| 378 | break; |
374 | break; |
| Line 379... | Line 375... | ||
| 379 | 375 | ||
| - | 376 | case UBXSTATE_CLASS: // check message identifier |
|
| - | 377 | ubxid = c; |
|
| - | 378 | ubxState = UBXSTATE_LEN1; |
|
| - | 379 | cka = ubxclass + ubxid; |
|
| - | 380 | ckb = ubxclass + cka; |
|
| - | 381 | break; |
|
| - | 382 | ||
| - | 383 | case UBXSTATE_LEN1: // 1st message length byte |
|
| - | 384 | msglen = (u16)c; // lowbyte first |
|
| - | 385 | cka += c; |
|
| - | 386 | ckb += cka; |
|
| - | 387 | ubxState = UBXSTATE_LEN2; |
|
| - | 388 | break; |
|
| - | 389 | ||
| - | 390 | case UBXSTATE_LEN2: // 2nd message length byte |
|
| - | 391 | msglen += ((u16)c)<<8; // high byte last |
|
| - | 392 | cka += c; |
|
| - | 393 | ckb += cka; |
|
| 380 | case UBXSTATE_CLASS: // check message identifier |
394 | |
| 381 | switch(ubxclass) |
395 | switch(ubxclass) |
| 382 | { |
396 | { |
| 383 | case UBX_CLASS_NAV: |
397 | case UBX_CLASS_NAV: |
| 384 | switch(c) |
398 | switch(ubxid) |
| 385 | { |
399 | { |
| 386 | case UBX_ID_POSLLH: // geodetic position |
400 | case UBX_ID_POSLLH: // geodetic position |
| 387 | ubxP = (u8 *)&UbxPosLlh; // data start pointer |
401 | ubxP = (u8 *)&UbxPosLlh; // data start pointer |
| 388 | ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer |
402 | ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer |
| Line 405... | Line 419... | ||
| 405 | ubxState = UBXSTATE_IDLE; |
419 | ubxState = UBXSTATE_IDLE; |
| 406 | return; |
420 | return; |
| 407 | } |
421 | } |
| 408 | break; |
422 | break; |
| Line 409... | Line 423... | ||
| 409 | 423 | ||
| 410 | case UBX_CLASS_ACK: |
424 | default: // other classes |
| 411 | switch(c) |
- | |
| - | 425 | if(UbxMsg.Status == NEWDATA) ubxState = UBXSTATE_IDLE; |
|
| 412 | { |
426 | else if(((UbxMsg.Hdr.Class&UbxMsg.ClassMask) == (ubxclass&UbxMsg.ClassMask)) && ((UbxMsg.Hdr.Id&UbxMsg.IdMask) == (ubxid&UbxMsg.IdMask))) |
| 413 | case UBX_ID_ACK_ACK: |
427 | { // buffer is free and message matches to filter criteria |
| 414 | UbxAck.Ack = 1; |
428 | UbxMsg.Status = INVALID; |
| 415 | break; |
- | |
| 416 | 429 | UbxMsg.Hdr.Class = ubxclass; |
|
| 417 | case UBX_ID_ACK_NAK: |
430 | UbxMsg.Hdr.Id = ubxid; |
| 418 | UbxAck.Ack = 0; |
431 | UbxMsg.Hdr.Length = msglen; |
| 419 | break; |
- | |
| 420 | 432 | ubxP = (u8 *)&(UbxMsg.Data); // data start pointer |
|
| 421 | default: |
433 | ubxEp = (u8 *)(&UbxMsg + 1); // data end pointer |
| 422 | ubxState = UBXSTATE_IDLE; |
- | |
| 423 | return; |
434 | ubxSp = (u8 *)&UbxMsg.Status; // status pointer |
| 424 | } |
- | |
| 425 | ubxP = (u8 *)&(UbxAck.clsID); // data start pointer |
- | |
| 426 | ubxEp = (u8 *)(&UbxAck + 1); // data end pointer |
- | |
| 427 | ubxSp = (u8 *)&UbxAck.Status; // status pointer |
- | |
| 428 | break; |
- | |
| 429 | - | ||
| 430 | default: // unsupported class |
435 | } |
| 431 | ubxState = UBXSTATE_IDLE; |
436 | else ubxState = UBXSTATE_IDLE; |
| 432 | break; |
437 | break; |
| 433 | } |
438 | } |
| 434 | if (ubxState != UBXSTATE_IDLE) |
439 | if(ubxState != UBXSTATE_IDLE) |
| 435 | { |
- | |
| 436 | ubxState = UBXSTATE_LEN1; |
- | |
| 437 | cka = UBX_CLASS_NAV + c; |
- | |
| 438 | ckb = UBX_CLASS_NAV + cka; |
- | |
| 439 | } |
- | |
| 440 | break; |
- | |
| 441 | - | ||
| 442 | case UBXSTATE_LEN1: // 1st message length byte |
- | |
| 443 | msglen = (u16)c; // lowbyte first |
- | |
| 444 | cka += c; |
- | |
| 445 | ckb += cka; |
- | |
| 446 | ubxState = UBXSTATE_LEN2; |
- | |
| 447 | break; |
- | |
| 448 | - | ||
| 449 | case UBXSTATE_LEN2: // 2nd message length byte |
- | |
| 450 | msglen += ((u16)c)<<8; // high byte last |
- | |
| 451 | cka += c; |
- | |
| 452 | ckb += cka; |
440 | { |
| 453 | // if the old data are not processed so far then break parsing now |
441 | // if the old data are not processed so far then break parsing now |
| 454 | // to avoid writing new data in ISR during reading by another function |
442 | // to avoid writing new data in ISR during reading by another function |
| 455 | if ( *ubxSp == NEWDATA ) |
443 | if ( *ubxSp == NEWDATA ) |
| 456 | { |
444 | { |
| 457 | ubxState = UBXSTATE_IDLE; |
445 | ubxState = UBXSTATE_IDLE; |
| 458 | if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively |
446 | if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively |
| 459 | } |
447 | } |
| 460 | else // data invalid or allready processd |
448 | else // data invalid or allready processed |
| 461 | { |
449 | { |
| 462 | *ubxSp = INVALID; // mark invalid during buffer filling |
450 | *ubxSp = INVALID; // mark invalid during buffer filling |
| - | 451 | ubxState = UBXSTATE_DATA; |
|
| 463 | ubxState = UBXSTATE_DATA; |
452 | } |
| 464 | } |
453 | } |
| Line 465... | Line 454... | ||
| 465 | break; |
454 | break; |
| 466 | 455 | ||
| Line 505... | Line 494... | ||
| 505 | break; |
494 | break; |
| Line 506... | Line 495... | ||
| 506 | 495 | ||
| 507 | } |
496 | } |
| Line 508... | Line 497... | ||
| 508 | } |
497 | } |
| 509 | 498 | ||
| 510 | u8 UBX_CreateMsg(Buffer_t* pBuff, u8 Class, u8 Id, u8* pData, u16 Len) |
499 | u8 UBX_CreateMsg(Buffer_t* pBuff, u8* pData, u16 Len) |
| 511 | { |
500 | { |
| 512 | u16 i; |
501 | u16 i; |
| 513 | u8 cka = 0, ckb = 0; |
502 | u8 cka = 0, ckb = 0; |
| Line 519... | Line 508... | ||
| 519 | pBuff->Locked = TRUE; |
508 | pBuff->Locked = TRUE; |
| 520 | // start at begin |
509 | // start at begin |
| 521 | pBuff->Position = 0; |
510 | pBuff->Position = 0; |
| 522 | pBuff->pData[pBuff->Position++] = UBX_SYNC1_CHAR; |
511 | pBuff->pData[pBuff->Position++] = UBX_SYNC1_CHAR; |
| 523 | pBuff->pData[pBuff->Position++] = UBX_SYNC2_CHAR; |
512 | pBuff->pData[pBuff->Position++] = UBX_SYNC2_CHAR; |
| 524 | pBuff->pData[pBuff->Position++] = Class; |
- | |
| 525 | pBuff->pData[pBuff->Position++] = Id; |
513 | for(i=0;i<Len;i++) |
| - | 514 | { |
|
| 526 | pBuff->pData[pBuff->Position++] = (u8)(Len); |
515 | pBuff->pData[pBuff->Position++] = pData[i]; |
| 527 | pBuff->pData[pBuff->Position++] = (u8)(Len>>8); |
- | |
| 528 | memcpy(&(pBuff->pData[pBuff->Position]), pData, Len); |
- | |
| 529 | pBuff->Position += Len; |
516 | } |
| 530 | // calculate checksum |
517 | // calculate checksum |
| 531 | for(i=2;i<pBuff->Position;i++) |
518 | for(i=2;i<pBuff->Position;i++) |
| 532 | { |
519 | { |
| 533 | cka += pBuff->pData[i]; |
520 | cka += pBuff->pData[i]; |
| 534 | ckb += cka; |
521 | ckb += cka; |