Rev 245 | Rev 263 | 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; |