@@ -64,6 +64,7 @@ uint32_t Ubx::init(
6464 drdyPort_ = drdy_port;
6565 drdyPin_ = drdy_pin;
6666 hasPps_ = has_pps;
67+ ppsHz_ = 1 ; // To match top of second.
6768
6869 dtimeout_ = 1000000 ; // 1 seconds
6970 timeout_ = 0 ;
@@ -75,14 +76,14 @@ uint32_t Ubx::init(
7576 baud_ = baud;
7677
7778 ubxProtocol_ = ubx_protocol;
78-
79+ ubx_. pps = 0 ;
7980 groupDelay_ = 0 ;
8081
8182 // gotNav_ = false;
8283 gotPvt_ = 0 ;
83- gotTime_ = 0 ;
84- gotEcefP_ = 0 ;
85- gotEcefV_ = 0 ;
84+ // gotTime_ = 0;
85+ // gotEcefP_ = 0;
86+ // gotEcefV_ = 0;
8687
8788 // USART initialization
8889 huart_->Instance = huart_instance;
@@ -181,17 +182,20 @@ uint32_t Ubx::init(
181182 error |= (uint16_t ) cfgMsg (0x01 , 0x20 , 0 ); // NAV-TIMEGPS
182183 error |= (uint16_t ) cfgMsg (0x01 , 0x01 , 0 ); // NAV-POSECEF (length 20)
183184 error |= (uint16_t ) cfgMsg (0x01 , 0x11 , 0 ); // NAV-VELECEF (length 20)
185+ error |= (uint16_t ) cfgMsg (0x01 , 0x20 , 0 ); // NAV-TIMEGPS (length 16)
186+ error |= (uint16_t ) cfgMsg (0x01 , 0x01 , 0 ); // NAV-POSECEF (length 20)
187+ error |= (uint16_t ) cfgMsg (0x01 , 0x11 , 0 ); // NAV-VELECEF (length 20)
184188
185189 // Enable these messages
186- error |= (uint16_t ) cfgMsg (0x01 , 0x20 , 1 ); // NAV-TIMEGPS (length 16)
190+ // error |= (uint16_t) cfgMsg(0x01, 0x20, 1); // NAV-TIMEGPS (length 16)
187191 error |= (uint16_t ) cfgMsg (0x01 , 0x07 , 1 ); // NAV-PVT (length 92)
188- error |= (uint16_t ) cfgMsg (0x01 , 0x01 , 1 ); // NAV-POSECEF (length 20)
189- error |= (uint16_t ) cfgMsg (0x01 , 0x11 , 1 ); // NAV-VELECEF (length 20)
192+ // error |= (uint16_t) cfgMsg(0x01, 0x01, 1); // NAV-POSECEF (length 20)
193+ // error |= (uint16_t) cfgMsg(0x01, 0x11, 1); // NAV-VELECEF (length 20)
190194
191195 // Set GPS Configuration (already done in pollCfgPrtM9() for UBX_M9)
192196 if (ubxProtocol_ == UBX_M8) {
193197 error |= (uint16_t ) cfgRate (sampleRateHz_); // Nav rate 0x06 0x08
194- error |= (uint16_t ) cfgTp5 (sampleRateHz_ ); // PPS rate 0x06 0x31
198+ error |= (uint16_t ) cfgTp5 (ppsHz_ ); // PPS rate 0x06 0x31
195199 error |= (uint16_t ) cfgNav5 (); // airplane mode 0x06 0x24
196200 }
197201
@@ -242,37 +246,46 @@ void Ubx::endDma(void)
242246 tm.tm_mday = ubx_.pvt .day ;
243247 tm.tm_mon = ubx_.pvt .month - 1 ;
244248 tm.tm_year = ubx_.pvt .year - 1900 ;
245- ubx_.unix_seconds = mktime (&tm);
249+ ubx_.unix_nanos = ubx_.pvt .nano ;
250+
246251 if (ubx_.pvt .nano <0 )
247252 {
248- ubx_. unix_seconds -= 1 ;
249- ubx_.pvt . nano += 1000000000 ;
253+ tm. tm_sec -- ;
254+ ubx_.unix_nanos += 1000000000 ;
250255 }
251- } else if ((p.cl == 0x01 ) && (p.id == 0x20 )) {
252- gotTime_ = time64.Us ();
253- memcpy ((uint8_t *) (&ubx_.time ), p.payload , sizeof (ubx_.time ));
254- } else if ((p.cl == 0x01 ) && (p.id == 0x01 )) {
255- gotEcefP_ = time64.Us ();
256- memcpy ((uint8_t *) (&ubx_.ecefp ), p.payload , sizeof (ubx_.ecefp ));
257- } else if ((p.cl == 0x01 ) && (p.id == 0x11 )) {
258- gotEcefV_ = time64.Us ();
259- memcpy ((uint8_t *) (&ubx_.ecefv ), p.payload , sizeof (ubx_.ecefv ));
256+
257+ ubx_.unix_seconds = mktime (&tm);
260258 }
259+ // else if ((p.cl == 0x01) && (p.id == 0x20)) {
260+ // gotTime_ = time64.Us();
261+ // memcpy((uint8_t *) (&ubx_.time), p.payload, sizeof(ubx_.time));
262+ // } else if ((p.cl == 0x01) && (p.id == 0x01)) {
263+ // gotEcefP_ = time64.Us();
264+ // memcpy((uint8_t *) (&ubx_.ecefp), p.payload, sizeof(ubx_.ecefp));
265+ // } else if ((p.cl == 0x01) && (p.id == 0x11)) {
266+ // gotEcefV_ = time64.Us();
267+ // memcpy((uint8_t *) (&ubx_.ecefv), p.payload, sizeof(ubx_.ecefv));
268+ // }
269+
270+ if (gotPvt_) { // && gotTime_ && gotEcefP_ && gotEcefV_) {
271+ // ubx_.drdy = time64.Us(); // usTime();
261272
262- if (gotPvt_ && gotTime_ && gotEcefP_ && gotEcefV_) {
263273 ubx_.header .timestamp = time64.Us (); // usTime();
264274
265- ubx_.drdy = drdy_;
266- if (!hasPps_) ubx_.pps = 0 ; // ubx_.drdy - 25000; // fake number if we don't have PPS hooked up
267-
268- if (ubx_.pps > ubx_.drdy ) ubx_.pps -= 1000000 / sampleRateHz_;
269- ubx_.groupDelay = ubx_.drdy - ubx_.pps ;
275+ if (!hasPps_) { // just use current time.
276+ ubx_.pps = 0 ; // zero means error
277+ } else if (ubx_.pps !=0 ) { // adjust to time of validity
278+ // uint64_t period_us = 1000000/sampleRateHz_;
279+ // ubx_.header.timestamp = ubx_.pps + (((ubx_.header.timestamp - ubx_.pps) % 1000000 )/period_us)*period_us;
280+ ubx_.header .timestamp = ubx_.pps + ubx_.unix_nanos /1000 ;
281+ }
282+ // ubx_.groupDelay = 0;
270283
271284 rxFifo_.write ((uint8_t *) &ubx_, sizeof (ubx_));
272285 gotPvt_ = 0 ;
273- gotTime_ = 0 ;
274- gotEcefP_ = 0 ;
275- gotEcefV_ = 0 ;
286+ // gotTime_ = 0;
287+ // gotEcefP_ = 0;
288+ // gotEcefV_ = 0;
276289 drdy_ = 0 ;
277290 }
278291 }
@@ -315,7 +328,6 @@ bool Ubx::parseByte(uint8_t c, UbxFrame * p)
315328 p->A += c;
316329 p->B += p->A ;
317330
318- if ((p->cl == 0x01 ) && (p->id == 0x07 )) drdy_ = time64.Us () - UBX_DMA_BUFFER_SIZE * 10000000 / baud_;
319331 } else if (n == 4 ) // length LSB
320332 {
321333 p->length = (uint16_t ) c;
@@ -351,49 +363,6 @@ bool Ubx::parseByte(uint8_t c, UbxFrame * p)
351363
352364void Ubx::pps (uint64_t pps_timestamp) { ubx_.pps = pps_timestamp; }
353365
354- bool Ubx::display (void )
355- {
356- UbxPacket p;
357-
358- char name_pvt[] = " Ubx (pvt)" ;
359- char name_time[] = " Ubx (time)" ;
360- char name_ecefp[] = " Ubx (ecefp)" ;
361- char name_ecefv[] = " Ubx (ecefv)" ;
362-
363- if (rxFifo_.read ((uint8_t *) &p, sizeof (p))) {
364- misc_header (name_pvt, p.drdy , p.header .timestamp , p.groupDelay );
365- misc_printf (" %10.3f ms | " , (double ) (p.header .timestamp - p.pps ) / 1000 .);
366- misc_printf (" iTOW %10u | " , p.pvt .iTOW );
367- misc_printf (" %02u/%02u/%04u " , p.pvt .month , p.pvt .day , p.pvt .year );
368- misc_printf (" %02u:%02u:%09.6f" , p.pvt .hour , p.pvt .min , (double ) p.pvt .sec + (double ) p.pvt .nano * 1e-9 );
369- misc_printf (" %14.8f deg %14.8f deg | " , (double ) p.pvt .lat * 1e-7 , (double ) p.pvt .lon * 1e-7 );
370- misc_printf (" numSV %02d | " , p.pvt .numSV );
371- misc_printf (" Fix %02d\n " , p.pvt .fixType );
372-
373- misc_header (name_time, p.drdy , p.header .timestamp , p.groupDelay );
374- misc_printf (" %10.3f ms | " , (double ) (p.header .timestamp - p.pps ) / 1000 .);
375- misc_printf (" iTOW %10u | " , p.time .iTOW );
376- misc_printf (" TOW %14.3f ms | valid 0x%02X\n " , (double ) p.time .iTOW + (double ) p.time .fTOW / 1000 , p.time .valid );
377-
378- misc_header (name_ecefp, p.drdy , p.header .timestamp , p.groupDelay );
379- misc_printf (" %10.3f ms | " , (double ) (p.header .timestamp - p.pps ) / 1000 .);
380- misc_printf (" iTOW %10u | " , p.ecefp .iTOW );
381- misc_printf (" %10d %10d %10d %10u cm\n " , p.ecefp .ecefX , p.ecefp .ecefY , p.ecefp .ecefZ , p.ecefp .pAcc );
382-
383- misc_header (name_ecefv, p.drdy , p.header .timestamp , p.groupDelay );
384- misc_printf (" %10.3f ms | " , (double ) (p.header .timestamp - p.pps ) / 1000 .);
385- misc_printf (" iTOW %10u | " , p.ecefv .iTOW );
386- misc_printf (" %10d %10d %10d %10u cm/s\n " , p.ecefv .ecefVX , p.ecefv .ecefVY , p.ecefv .ecefVZ , p.ecefv .sAcc );
387- } else {
388- misc_printf (" %s\n " , name_pvt);
389- misc_printf (" %s\n " , name_time);
390- misc_printf (" %s\n " , name_ecefp);
391- misc_printf (" %s\n " , name_ecefv);
392- }
393-
394- return 1 ;
395- }
396-
397366// /////////////////////////////////////////////////////////////////////////////
398367// Packet Stuff
399368
@@ -520,12 +489,12 @@ uint16_t Ubx::cfgTp5(uint32_t hz)
520489 payload[3 ] = 0x00 ; // reserved
521490 SET (payload + 4 , 0x0000 , int16_t ); // antenna delay
522491 SET (payload + 6 , 0x0000 , int16_t ); // rf delay
523- SET (payload + 8 , period_us, uint32_t );
524- SET (payload + 12 , period_us / 2 , uint32_t );
525- SET (payload + 16 , pulse_len_us, uint32_t ); // no pulse when not locked =0 ?
526- SET (payload + 20 , pulse_len_us / 2 , uint32_t );
492+ SET (payload + 8 , period_us, uint32_t ); // period when not locked
493+ SET (payload + 12 , period_us, uint32_t ); // period when locked
494+ SET (payload + 16 , pulse_len_us, uint32_t );
495+ SET (payload + 20 , pulse_len_us, uint32_t );
527496 SET (payload + 24 , 0x0000 , uint32_t ); // delay
528- SET (payload + 28 , 0x01F7 , uint32_t ); // 0001 1111 1111 0111 = 0x01F7
497+ SET (payload + 28 , 0x0077 , uint32_t ); // 0001 1111 1111 0111 = 0x01F7 // 0000 0000 0111 0111 = 0x0077
529498
530499 return tx (message, SFG_TP5_LENGTH);
531500}
@@ -578,7 +547,7 @@ uint16_t Ubx::cfgMsg(uint8_t cl, uint8_t id, uint8_t decimation_rate)
578547
579548uint16_t Ubx::cfgM9 (uint32_t baud, uint16_t sampleRateHz)
580549{
581- uint8_t cfg_message[64 ] = {0 };
550+ uint8_t cfg_message[128 ] = {0 };
582551 uint16_t length = 0 ;
583552 uint8_t * p = cfg_message;
584553 // Header
@@ -595,8 +564,21 @@ uint16_t Ubx::cfgM9(uint32_t baud, uint16_t sampleRateHz)
595564 SETKV (p, 0x40520001 , baud, uint32_t ); // baud rate (8 bytes)
596565 SETKV (p, 0x30210001 , 1000 / sampleRateHz, uint16_t ); // output rate in milliseconds (6 bytes)
597566 SETKV (p, 0x30210002 , 1 , uint16_t ); // 1 data output per nav measurement (6 bytes)
567+ SETKV (p, 0x20210003 , 0 , uint8_t ); // CFG-RATE-TIMEREF 0 = UTC
598568 SETKV (p, 0x20110021 , 8 , uint8_t ); // CFG-NAVSPG-DYNMODEL 8 = 4G Airborne (5 bytes)
599569 SETKV (p, 0x20110011 , 3 , uint8_t ); // CFG-NAVSPG-FIXMODE 3 = Auto 2/3D (5 bytes)
570+ SETKV (p, 0x20050023 , 0 , uint8_t ); // CFG-TP-PULSE_DEF = 0 set period in us
571+ SETKV (p, 0x20050030 , 1 , uint8_t ); // CFG-TP-PULSE_LENGTH_DEF = 1 set pulse length in us
572+ SETKV (p, 0x40050002 , 1000000 , uint32_t ); // CFG-TP-PERIOD_TP1
573+ SETKV (p, 0x40050003 , 1000000 , uint32_t ); // CFG-TP-PERIOD_LOCK_TP1 = 1000000 us (1 second)
574+ SETKV (p, 0x40050003 , 1000000 , uint32_t ); // CFG-TP-PERIOD_LOCK_TP1 = 1000000 us (1 second)
575+ SETKV (p, 0x40050004 , 500 , uint32_t ); // CFG-TP-LEN_TP1 = 500 us (0.5 ms)
576+ SETKV (p, 0x40050005 , 1000 , uint32_t ); // CFG-TP-LEN_LOCK_TP1 = 1000 us (1ms)
577+ SETKV (p, 0x10050008 , 1 , uint8_t ); // CFG-TP-SYNC_GNSS_TP1
578+ SETKV (p, 0x1005000a , 1 , uint8_t ); // CFG-TP-ALIGN_TO_TOW_TP1
579+ SETKV (p, 0x2005000c , 1 , uint8_t ); // CFG-TP-POL_TP1 1= rising edge
580+ SETKV (p, 0x10050009 , 1 , uint8_t ); // CFG-TP-USE_LOCKED_TP1
581+ SETKV (p, 0x2005000c , 0 , uint8_t ); // CFG-TP-TIMEGRID_TP1, 0 = UTC
600582
601583 // compute and insert message length length
602584 length = p - cfg_message - 6 ;
@@ -652,3 +634,50 @@ uint32_t Ubx::pollBaudM9(void)
652634 }
653635 return 0 ;
654636}
637+
638+ bool Ubx::display (void )
639+ {
640+ UbxPacket p;
641+
642+ char name_pvt[] = " Ubx (pvt)" ;
643+ // char name_time[] = "Ubx (time)";
644+ // char name_ecefp[] = "Ubx (ecefp)";
645+ // char name_ecefv[] = "Ubx (ecefv)";
646+
647+ if (rxFifo_.read ((uint8_t *) &p, sizeof (p))) {
648+ // misc_header(name_pvt, p.drdy, p.header.timestamp, p.groupDelay);
649+ misc_header (name_pvt, p.pps , p.header .timestamp , 0 );
650+ misc_printf (" | pps %10.6f s | " , (double )p.pps * 1e-6 );
651+ misc_printf (" iTOW %10.3f s | " , (double )p.pvt .iTOW /1000 );
652+ misc_printf (" %02u/%02u/%04u " , p.pvt .month , p.pvt .day , p.pvt .year );
653+
654+ misc_printf (" %02u:%02u:%02u.%+010d" , p.pvt .hour , p.pvt .min , p.pvt .sec , p.pvt .nano );
655+
656+ misc_printf (" %14.8f deg %14.8f deg | " , (double ) p.pvt .lat * 1e-7 , (double ) p.pvt .lon * 1e-7 );
657+ misc_printf (" numSV %02d | " , p.pvt .numSV );
658+ misc_printf (" Fix %02d\n " , p.pvt .fixType );
659+
660+ // misc_header(name_time, p.drdy, p.header.timestamp, p.groupDelay);
661+ // misc_printf("%10.3f ms | ", (double) (p.header.timestamp - p.pps) / 1000.);
662+ // misc_printf(" iTOW %10u | ", p.time.iTOW);
663+ // misc_printf(" TOW %14.3f ms | valid 0x%02X\n", (double) p.time.iTOW + (double) p.time.fTOW / 1000, p.time.valid);
664+ //
665+ // misc_header(name_ecefp, p.drdy, p.header.timestamp, p.groupDelay);
666+ // misc_printf("%10.3f ms | ", (double) (p.header.timestamp - p.pps) / 1000.);
667+ // misc_printf(" iTOW %10u | ", p.ecefp.iTOW);
668+ // misc_printf(" %10d %10d %10d %10u cm\n", p.ecefp.ecefX, p.ecefp.ecefY, p.ecefp.ecefZ, p.ecefp.pAcc);
669+ //
670+ // misc_header(name_ecefv, p.drdy, p.header.timestamp, p.groupDelay);
671+ // misc_printf("%10.3f ms | ", (double) (p.header.timestamp - p.pps) / 1000.);
672+ // misc_printf(" iTOW %10u | ", p.ecefv.iTOW);
673+ // misc_printf(" %10d %10d %10d %10u cm/s\n", p.ecefv.ecefVX, p.ecefv.ecefVY, p.ecefv.ecefVZ, p.ecefv.sAcc);
674+ } else {
675+ misc_printf (" %s\n " , name_pvt);
676+ // misc_printf("%s\n", name_time);
677+ // misc_printf("%s\n", name_ecefp);
678+ // misc_printf("%s\n", name_ecefv);
679+ }
680+
681+ return 1 ;
682+ }
683+
0 commit comments