Rev 247 | Rev 264 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 247 | Rev 263 | ||
---|---|---|---|
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 162... | Line 163... | ||
162 | 163 | ||
163 | //------------------------------------------------------------------------------------ |
164 | //------------------------------------------------------------------------------------ |
Line 164... | Line 165... | ||
164 | // global variables |
165 | // global variables |
165 | 166 | ||
166 | // local buffers for the incomming ubx messages |
167 | // local buffers for the incomming ubx messages |
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}; |
168 | ubx_nav_sol_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID}; |
168 | volatile ubx_nav_posllh_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID}; |
169 | ubx_nav_posllh_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID}; |
Line 169... | Line 170... | ||
169 | volatile ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID}; |
170 | ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID}; |
170 | volatile ubxmsg_t UbxMsg; |
171 | ubxmsg_t UbxMsg; |
171 | 172 | ||
Line 274... | Line 275... | ||
274 | } |
275 | } |
Line 275... | Line 276... | ||
275 | 276 | ||
276 | /********************************************************/ |
277 | /********************************************************/ |
277 | /* Upate GPS data stcructure */ |
278 | /* Upate GPS data stcructure */ |
278 | /********************************************************/ |
279 | /********************************************************/ |
279 | void Update_GPSData (void) |
280 | void Update_GPSData(void) |
280 | { |
- | |
281 | static u32 Msg_Count_Timeout = 0; |
- | |
282 | static u8 Msg_Count = 0; |
281 | { |
283 | static u32 LastTimeStamp = 0; |
282 | static u32 LastTimeStamp = 0; |
284 | u32 TimeStamp; |
- | |
285 | - | ||
286 | // the timeout is used to detect the delay between two message sets |
- | |
287 | // and is used for synchronisation so that always a set is collected |
283 | u32 TimeStamp; |
288 | // that belongs together |
- | |
289 | // _______NAVSOL|POSLLH|VELNED|___________________NAVSOL|POSLLH|VELNED|_____________ |
- | |
290 | // | 8ms | 8ms | 184 ms | | | |
- | |
291 | // msg_count: 0 1 2 0 1 2 |
- | |
292 | - | ||
293 | if(CheckDelay(Msg_Count_Timeout)) Msg_Count = 0; |
- | |
294 | else Msg_Count++; |
- | |
Line 295... | Line 284... | ||
295 | Msg_Count_Timeout = SetDelay(100); // reset ubx msg timeout |
284 | static u32 last_itow = 0; |
296 | 285 | ||
297 | // if a new set of ubx messages was collected |
286 | // if a new set of ubx messages was collected |
298 | if((Msg_Count >= 2)) |
287 | if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA)) |
299 | { // if set is complete |
288 | { // and the itow is equal (same time base) |
- | 289 | if((UbxSol.itow == UbxPosLlh.itow) && (UbxPosLlh.itow == UbxVelNed.itow)) |
|
- | 290 | { |
|
300 | if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA)) |
291 | DebugOut.Analog[23] = (u16)(UbxSol.itow-last_itow); |
301 | { |
292 | last_itow = UbxSol.itow; // update last itow |
302 | UBX_Timeout = SetDelay(UBX_TIMEOUT); |
- | |
303 | DebugOut.Analog[9]++; |
293 | UBX_Timeout = SetDelay(UBX_TIMEOUT); |
304 | 294 | DebugOut.Analog[9]++; |
|
305 | // update GPS data only if the status is INVALID or PROCESSED and the last ubx message was received within less than 100 ms |
295 | // update GPS data only if the status is INVALID or PROCESSED |
306 | if(GPSData.Status != NEWDATA) // if last data were processed |
296 | if(GPSData.Status != NEWDATA) |
307 | { // wait for new data at all neccesary ubx messages |
297 | { // wait for new data at all neccesary ubx messages |
308 | GPSData.Status = INVALID; |
298 | GPSData.Status = INVALID; |
309 | // update message cycle time |
299 | // update message cycle time |
Line 328... | Line 318... | ||
328 | GPSData.Speed_East = UbxVelNed.VEL_E; |
318 | GPSData.Speed_East = UbxVelNed.VEL_E; |
329 | GPSData.Speed_North = UbxVelNed.VEL_N; |
319 | GPSData.Speed_North = UbxVelNed.VEL_N; |
330 | GPSData.Speed_Top = -UbxVelNed.VEL_D; |
320 | GPSData.Speed_Top = -UbxVelNed.VEL_D; |
331 | GPSData.Speed_Ground = UbxVelNed.GSpeed; |
321 | GPSData.Speed_Ground = UbxVelNed.GSpeed; |
332 | GPSData.Heading = UbxVelNed.Heading; |
322 | GPSData.Heading = UbxVelNed.Heading; |
333 | 323 | ||
334 | GPSData.Status = NEWDATA; // new data available |
324 | GPSData.Status = NEWDATA; // new data available |
335 | } // EOF if(GPSData.Status != NEWDATA) |
325 | } // EOF if(GPSData.Status != NEWDATA) |
336 | } // EOF all ubx messages received |
- | |
337 | // set state to collect new data |
326 | // set state to collect new data |
338 | UbxSol.Status = PROCESSED; // ready for new data |
327 | UbxSol.Status = PROCESSED; // ready for new data |
339 | UbxPosLlh.Status = PROCESSED; // ready for new data |
328 | UbxPosLlh.Status = PROCESSED; // ready for new data |
340 | UbxVelNed.Status = PROCESSED; // ready for new data |
329 | UbxVelNed.Status = PROCESSED; // ready for new data |
341 | } |
330 | } // EOF all itow are equal |
- | 331 | } // EOF all ubx messages received |
|
342 | } |
332 | } |
Line 343... | Line 333... | ||
343 | 333 | ||
344 | 334 | ||
345 | /********************************************************/ |
335 | /********************************************************/ |
346 | /* UBX Parser */ |
336 | /* UBX Parser */ |
347 | /********************************************************/ |
337 | /********************************************************/ |
348 | void UBX_RxParser(u8 c) |
338 | void UBX_RxParser(u8 c) |
349 | { |
339 | { |
350 | static ubxState_t ubxState = UBXSTATE_IDLE; |
340 | static ubxState_t ubxState = UBXSTATE_IDLE; |
351 | static u8 ubxclass; |
341 | static ubxmsghdr_t RxHdr; |
352 | static u8 ubxid; |
342 | static u8 RxData[UBX_MSG_DATA_SIZE]; |
353 | static u16 msglen; |
- | |
Line 354... | Line 343... | ||
354 | static u8 cka, ckb; |
343 | static u16 RxBytes = 0; |
355 | static u8 *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered |
344 | static u8 cka, ckb; |
356 | 345 | ||
Line 367... | Line 356... | ||
367 | if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2; |
356 | if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2; |
368 | else ubxState = UBXSTATE_IDLE; // out of synchronization |
357 | else ubxState = UBXSTATE_IDLE; // out of synchronization |
369 | break; |
358 | break; |
Line 370... | Line 359... | ||
370 | 359 | ||
371 | case UBXSTATE_SYNC2: // check msg class to be NAV |
360 | case UBXSTATE_SYNC2: // check msg class to be NAV |
372 | ubxclass = c; |
361 | RxHdr.Class = c; |
373 | ubxState = UBXSTATE_CLASS; |
362 | ubxState = UBXSTATE_CLASS; |
Line 374... | Line 363... | ||
374 | break; |
363 | break; |
375 | 364 | ||
376 | case UBXSTATE_CLASS: // check message identifier |
365 | case UBXSTATE_CLASS: // check message identifier |
377 | ubxid = c; |
366 | RxHdr.Id = c; |
378 | ubxState = UBXSTATE_LEN1; |
367 | ubxState = UBXSTATE_LEN1; |
379 | cka = ubxclass + ubxid; |
368 | cka = RxHdr.Class + RxHdr.Id; |
Line 380... | Line 369... | ||
380 | ckb = ubxclass + cka; |
369 | ckb = RxHdr.Class + cka; |
381 | break; |
370 | break; |
382 | 371 | ||
383 | case UBXSTATE_LEN1: // 1st message length byte |
372 | case UBXSTATE_LEN1: // 1st message length byte |
384 | msglen = (u16)c; // lowbyte first |
373 | RxHdr.Length = (u16)c; // lowbyte first |
385 | cka += c; |
374 | cka += c; |
Line 386... | Line 375... | ||
386 | ckb += cka; |
375 | ckb += cka; |
387 | ubxState = UBXSTATE_LEN2; |
376 | ubxState = UBXSTATE_LEN2; |
388 | break; |
- | |
389 | - | ||
390 | case UBXSTATE_LEN2: // 2nd message length byte |
- | |
391 | msglen += ((u16)c)<<8; // high byte last |
377 | break; |
392 | cka += c; |
378 | |
393 | ckb += cka; |
- | |
394 | - | ||
395 | switch(ubxclass) |
- | |
396 | { |
- | |
397 | case UBX_CLASS_NAV: |
- | |
398 | switch(ubxid) |
- | |
399 | { |
- | |
400 | case UBX_ID_POSLLH: // geodetic position |
- | |
401 | ubxP = (u8 *)&UbxPosLlh; // data start pointer |
- | |
402 | ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer |
- | |
403 | ubxSp = (u8 *)&UbxPosLlh.Status; // status pointer |
- | |
404 | break; |
- | |
405 | - | ||
406 | case UBX_ID_SOL: // navigation solution |
- | |
407 | ubxP = (u8 *)&UbxSol; // data start pointer |
- | |
408 | ubxEp = (u8 *)(&UbxSol + 1); // data end pointer |
- | |
409 | ubxSp = (u8 *)&UbxSol.Status; // status pointer |
- | |
410 | break; |
- | |
411 | - | ||
412 | case UBX_ID_VELNED: // velocity vector in tangent plane |
- | |
413 | ubxP = (u8 *)&UbxVelNed; // data start pointer |
- | |
414 | ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer |
- | |
415 | ubxSp = (u8 *)&UbxVelNed.Status; // status pointer |
379 | case UBXSTATE_LEN2: // 2nd message length byte |
416 | break; |
- | |
417 | - | ||
418 | default: // unsupported identifier |
- | |
419 | ubxState = UBXSTATE_IDLE; |
- | |
420 | return; |
- | |
421 | } |
- | |
422 | break; |
- | |
423 | - | ||
424 | default: // other classes |
380 | RxHdr.Length += ((u16)c)<<8; // high byte last |
425 | if(UbxMsg.Status == NEWDATA) ubxState = UBXSTATE_IDLE; |
- | |
426 | else if(((UbxMsg.Hdr.Class&UbxMsg.ClassMask) == (ubxclass&UbxMsg.ClassMask)) && ((UbxMsg.Hdr.Id&UbxMsg.IdMask) == (ubxid&UbxMsg.IdMask))) |
- | |
427 | { // buffer is free and message matches to filter criteria |
- | |
428 | UbxMsg.Status = INVALID; |
- | |
429 | UbxMsg.Hdr.Class = ubxclass; |
- | |
430 | UbxMsg.Hdr.Id = ubxid; |
- | |
431 | UbxMsg.Hdr.Length = msglen; |
- | |
432 | ubxP = (u8 *)&(UbxMsg.Data); // data start pointer |
- | |
433 | ubxEp = (u8 *)(&UbxMsg + 1); // data end pointer |
- | |
434 | ubxSp = (u8 *)&UbxMsg.Status; // status pointer |
381 | if (RxHdr.Length >= UBX_MSG_DATA_SIZE) |
435 | } |
382 | { |
436 | else ubxState = UBXSTATE_IDLE; |
383 | ubxState = UBXSTATE_IDLE; |
437 | break; |
- | |
438 | } |
- | |
439 | if(ubxState != UBXSTATE_IDLE) |
- | |
440 | { |
384 | DebugOut.Analog[25]++; |
441 | // if the old data are not processed so far then break parsing now |
385 | } |
442 | // to avoid writing new data in ISR during reading by another function |
- | |
443 | if ( *ubxSp == NEWDATA ) |
- | |
444 | { |
386 | else |
445 | ubxState = UBXSTATE_IDLE; |
- | |
446 | if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively |
- | |
447 | } |
387 | { |
448 | else // data invalid or allready processed |
- | |
449 | { |
388 | cka += c; |
450 | *ubxSp = INVALID; // mark invalid during buffer filling |
389 | ckb += cka; |
Line 451... | Line 390... | ||
451 | ubxState = UBXSTATE_DATA; |
390 | RxBytes = 0; // reset data byte counter |
452 | } |
391 | ubxState = UBXSTATE_DATA; |
453 | } |
392 | } |
454 | break; |
393 | break; |
455 | 394 | ||
456 | case UBXSTATE_DATA: // collecting data |
395 | case UBXSTATE_DATA: // collecting data |
457 | if (ubxP < ubxEp) |
396 | if (RxBytes < UBX_MSG_DATA_SIZE) |
458 | { |
397 | { |
459 | *ubxP++ = c; // copy curent data byte if any space is left |
398 | RxData[RxBytes++] = c; // copy curent data byte if any space is left |
460 | cka += c; |
399 | cka += c; |
461 | ckb += cka; |
400 | ckb += cka; |
- | 401 | if (RxBytes >= RxHdr.Length) ubxState = UBXSTATE_CKA; // switch to next state if all data have been received |
|
462 | if (--msglen == 0) ubxState = UBXSTATE_CKA; // switch to next state if all data was read |
402 | } |
463 | } |
403 | else // rx buffer overrun |
Line 464... | Line 404... | ||
464 | else // rx buffer overrun |
404 | { |
465 | { |
405 | ubxState = UBXSTATE_IDLE; |
466 | ubxState = UBXSTATE_IDLE; |
406 | DebugOut.Analog[25]++; |
467 | } |
407 | } |
468 | break; |
- | |
469 | 408 | break; |
|
- | 409 | ||
470 | case UBXSTATE_CKA: |
410 | case UBXSTATE_CKA: |
471 | if (c == cka) ubxState = UBXSTATE_CKB; |
411 | if (c == cka) ubxState = UBXSTATE_CKB; |
Line 472... | Line 412... | ||
472 | else |
412 | else |
473 | { |
413 | { |
- | 414 | ubxState = UBXSTATE_IDLE; |
|
- | 415 | DebugOut.Analog[24]++; |
|
- | 416 | } |
|
474 | *ubxSp = INVALID; |
417 | break; |
- | 418 | ||
- | 419 | case UBXSTATE_CKB: |
|
- | 420 | if (c == ckb) |
|
475 | ubxState = UBXSTATE_IDLE; |
421 | { // checksum is ok |
- | 422 | ||
- | 423 | switch(RxHdr.Class) |
|
- | 424 | { |
|
- | 425 | case UBX_CLASS_NAV: |
|
476 | } |
426 | switch(RxHdr.Id) |
- | 427 | { |
|
- | 428 | case UBX_ID_POSLLH: // geodetic position |
|
- | 429 | memcpy((u8*)&UbxPosLlh, RxData, RxHdr.Length); |
|
- | 430 | UbxPosLlh.Status = NEWDATA; |
|
- | 431 | break; |
|
- | 432 | ||
- | 433 | case UBX_ID_VELNED: // velocity vector in tangent plane |
|
- | 434 | memcpy((u8*)&UbxVelNed, RxData, RxHdr.Length); |
|
- | 435 | UbxVelNed.Status = NEWDATA; |
|
- | 436 | break; |
|
- | 437 | ||
- | 438 | case UBX_ID_SOL: // navigation solution |
|
- | 439 | memcpy((u8*)&UbxSol, RxData, RxHdr.Length); |
|
477 | break; |
440 | UbxSol.Status = NEWDATA; |
- | 441 | break; |
|
478 | 442 | ||
- | 443 | default: |
|
- | 444 | break; |
|
- | 445 | } // EOF switch(Id) |
|
- | 446 | Update_GPSData(); |
|
- | 447 | break; |
|
- | 448 | ||
- | 449 | default: |
|
479 | case UBXSTATE_CKB: |
450 | break; |
480 | if (c == ckb) |
451 | } // EOF switch(class) |
- | 452 | ||
- | 453 | // check generic msg filter |
|
- | 454 | if(UbxMsg.Status != NEWDATA) |
|
- | 455 | { // msg buffer is free |
|
- | 456 | if(((UbxMsg.Hdr.Class&UbxMsg.ClassMask) == (RxHdr.Class&UbxMsg.ClassMask)) && ((UbxMsg.Hdr.Id&UbxMsg.IdMask) == (RxHdr.Id&UbxMsg.IdMask))) |
|
- | 457 | { // msg matches to the filter criteria |
|
- | 458 | UbxMsg.Status = INVALID; |
|
481 | { |
459 | UbxMsg.Hdr.Class = RxHdr.Class; |
- | 460 | UbxMsg.Hdr.Id = RxHdr.Id; |
|
- | 461 | UbxMsg.Hdr.Length = RxHdr.Length; |
|
- | 462 | if(UbxMsg.Hdr.Length <= UBX_MSG_DATA_SIZE) |
|
- | 463 | { // copy data block |
|
482 | *ubxSp = NEWDATA; // new data are valid |
464 | memcpy(UbxMsg.Data, RxData, RxHdr.Length); |
483 | if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively |
465 | UbxMsg.Status = NEWDATA; |
Line 484... | Line 466... | ||
484 | } |
466 | } |
485 | else |
467 | } // EOF filter matches |
Line 524... | Line 506... | ||
524 | pBuff->pData[pBuff->Position++] = ckb; |
506 | pBuff->pData[pBuff->Position++] = ckb; |
525 | pBuff->DataBytes = pBuff->Position; |
507 | pBuff->DataBytes = pBuff->Position; |
526 | pBuff->Position = 0; // reset buffer position for transmision |
508 | pBuff->Position = 0; // reset buffer position for transmision |
527 | return(1); |
509 | return(1); |
528 | } |
510 | } |
- | 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 |