1 /* 2 * This software was developed by the Computer Systems Engineering group 3 * at Lawrence Berkeley Laboratory under DARPA contract BG 91-66. 4 * 5 * Copyright (c) 1992 The Regents of the University of California. 6 * All rights reserved. 7 * 8 * Redistribution and use in source and binary forms, with or without 9 * modification, are permitted provided that the following conditions 10 * are met: 11 * 1. Redistributions of source code must retain the above copyright 12 * notice, this list of conditions and the following disclaimer. 13 * 2. Redistributions in binary form must reproduce the above copyright 14 * notice, this list of conditions and the following disclaimer in the 15 * documentation and/or other materials provided with the distribution. 16 * 3. All advertising materials mentioning features or use of this software 17 * must display the following acknowledgement: 18 * This product includes software developed by the University of 19 * California, Lawrence Berkeley Laboratory. 20 * 4. The name of the University may not be used to endorse or promote 21 * products derived from this software without specific prior 22 * written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND 25 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 30 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 31 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 33 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 34 * SUCH DAMAGE. 35 */ 36 37 /* 38 * Modified: Marc Brett <marc.brett@westgeo.com> Sept, 1999. 39 * 40 * 1. Added support for alternate PPS schemes, with code mostly 41 * copied from the Oncore driver (Thanks, Poul-Henning Kamp). 42 * This code runs on SunOS 4.1.3 with ppsclock-1.6a1 and Solaris 7. 43 */ 44 45 46 #ifdef HAVE_CONFIG_H 47 # include <config.h> 48 #endif 49 50 #if defined(REFCLOCK) && defined(CLOCK_MX4200) && defined(PPS) 51 52 #include <stdio.h> 53 #include <ctype.h> 54 #include <sys/types.h> 55 56 #include "ntpd.h" 57 #include "ntp_io.h" 58 #include "ntp_refclock.h" 59 #include "ntp_unixtime.h" 60 #include "ntp_stdlib.h" 61 62 #include "mx4200.h" 63 64 #ifdef HAVE_SYS_TIME_H 65 # include <sys/time.h> 66 #endif 67 #ifdef HAVE_SYS_TERMIOS_H 68 # include <sys/termios.h> 69 #endif 70 #ifdef HAVE_SYS_PPSCLOCK_H 71 # include <sys/ppsclock.h> 72 #endif 73 74 #ifndef HAVE_STRUCT_PPSCLOCKEV 75 struct ppsclockev { 76 # ifdef HAVE_TIMESPEC 77 struct timespec tv; 78 # else 79 struct timeval tv; 80 # endif 81 u_int serial; 82 }; 83 #endif /* ! HAVE_STRUCT_PPSCLOCKEV */ 84 85 /* 86 * This driver supports the Magnavox Model MX 4200 GPS Receiver 87 * adapted to precision timing applications. It requires the 88 * ppsclock line discipline or streams module described in the 89 * Line Disciplines and Streams Drivers page. It also requires a 90 * gadget box and 1-PPS level converter, such as described in the 91 * Pulse-per-second (PPS) Signal Interfacing page. 92 * 93 * It's likely that other compatible Magnavox receivers such as the 94 * MX 4200D, MX 9212, MX 9012R, MX 9112 will be supported by this code. 95 */ 96 97 /* 98 * Check this every time you edit the code! 99 */ 100 #define YEAR_RIGHT_NOW 1998 101 102 /* 103 * GPS Definitions 104 */ 105 #define DEVICE "/dev/gps%d" /* device name and unit */ 106 #define SPEED232 B4800 /* baud */ 107 108 /* 109 * Radio interface parameters 110 */ 111 #define PRECISION (-18) /* precision assumed (about 4 us) */ 112 #define REFID "GPS\0" /* reference id */ 113 #define DESCRIPTION "Magnavox MX4200 GPS Receiver" /* who we are */ 114 #define DEFFUDGETIME 0 /* default fudge time (ms) */ 115 116 #define SLEEPTIME 32 /* seconds to wait for reconfig to complete */ 117 118 /* 119 * Position Averaging. 120 * Reference: Dr. Thomas A. Clark's Totally Accurate Clock (TAC) files at 121 * ftp://aleph.gsfc.nasa.gov/GPS/totally.accurate.clock/ 122 * For a 6-channel Motorola Oncore, he indicates that good nominal 123 * HDOP and VDOP are 1.50 and 2.00 respectively. Given the relationship 124 * HDOP^2 = NDOP^2 + EDOP^2 and assuming EDOP and NDOP are equal, we 125 * have a nominal NDOP = EDOP = sqrt((HDOP*HDOP)/2). An 8-channel 126 * Oncore does well with HDOP=1.20 and VDOP=1.70. 127 */ 128 #define INTERVAL 1 /* Interval between position measurements (s) */ 129 #define AVGING_TIME 24 /* Number of hours to average */ 130 #define USUAL_EDOP 1.06066 /* used for normalizing EDOP */ 131 #define USUAL_NDOP 1.06066 /* used for normalizing NDOP */ 132 #define USUAL_VDOP 2.00 /* used for normalizing VDOP */ 133 #define NOT_INITIALIZED -9999. /* initial pivot longitude */ 134 135 /* 136 * MX4200 unit control structure. 137 */ 138 struct mx4200unit { 139 u_int pollcnt; /* poll message counter */ 140 u_int polled; /* Hand in a time sample? */ 141 u_int lastserial; /* last pps serial number */ 142 struct ppsclockev ppsev; /* PPS control structure */ 143 double avg_lat; /* average latitude */ 144 double avg_lon; /* average longitude */ 145 double avg_alt; /* average height */ 146 double central_meridian; /* central meridian */ 147 double filt_lat; /* latitude filter length */ 148 double filt_lon; /* longitude filter length */ 149 double filt_alt; /* height filter length */ 150 double edop; /* EDOP (east DOP) */ 151 double ndop; /* NDOP (north DOP) */ 152 double vdop; /* VDOP (vertical DOP) */ 153 int last_leap; /* leap second warning */ 154 u_int moving; /* mobile platform? */ 155 u_long sloppyclockflag; /* fudge flags */ 156 u_int known; /* position known yet? */ 157 u_long clamp_time; /* when to stop postion averaging */ 158 u_long log_time; /* when to print receiver status */ 159 }; 160 161 static char pmvxg[] = "PMVXG"; 162 163 /* XXX should be somewhere else */ 164 #ifdef __GNUC__ 165 #if __GNUC__ < 2 || (__GNUC__ == 2 && __GNUC_MINOR__ < 5) 166 #ifndef __attribute__ 167 #define __attribute__(args) 168 #endif 169 #endif 170 #else 171 #ifndef __attribute__ 172 #define __attribute__(args) 173 #endif 174 #endif 175 /* XXX end */ 176 177 /* 178 * Function prototypes 179 */ 180 static int mx4200_start P((int, struct peer *)); 181 static void mx4200_shutdown P((int, struct peer *)); 182 static void mx4200_receive P((struct recvbuf *)); 183 static void mx4200_poll P((int, struct peer *)); 184 185 static char * mx4200_parse_t P((struct peer *)); 186 static char * mx4200_parse_p P((struct peer *)); 187 static char * mx4200_parse_d P((struct peer *)); 188 static char * mx4200_parse_s P((struct peer *)); 189 #ifdef QSORT_USES_VOID_P 190 int mx4200_cmpl_fp P((const void *, const void *)); 191 #else 192 int mx4200_cmpl_fp P((const l_fp *, const l_fp *)); 193 #endif /* not QSORT_USES_VOID_P */ 194 static void mx4200_config P((struct peer *)); 195 static void mx4200_ref P((struct peer *)); 196 static void mx4200_send P((struct peer *, char *, ...)) 197 __attribute__ ((format (printf, 2, 3))); 198 static u_char mx4200_cksum P((char *, int)); 199 static int mx4200_jday P((int, int, int)); 200 static void mx4200_debug P((struct peer *, char *, ...)) 201 __attribute__ ((format (printf, 2, 3))); 202 static int mx4200_pps P((struct peer *)); 203 204 /* 205 * Transfer vector 206 */ 207 struct refclock refclock_mx4200 = { 208 mx4200_start, /* start up driver */ 209 mx4200_shutdown, /* shut down driver */ 210 mx4200_poll, /* transmit poll message */ 211 noentry, /* not used (old mx4200_control) */ 212 noentry, /* initialize driver (not used) */ 213 noentry, /* not used (old mx4200_buginfo) */ 214 NOFLAGS /* not used */ 215 }; 216 217 218 219 /* 220 * mx4200_start - open the devices and initialize data for processing 221 */ 222 static int 223 mx4200_start( 224 int unit, 225 struct peer *peer 226 ) 227 { 228 register struct mx4200unit *up; 229 struct refclockproc *pp; 230 int fd; 231 char gpsdev[20]; 232 233 #ifdef HAVE_TIOCGPPSEV 234 #ifdef HAVE_TERMIOS 235 struct termios ttyb; 236 #endif /* HAVE_TERMIOS */ 237 #ifdef HAVE_SYSV_TTYS 238 struct termio ttyb; 239 #endif /* HAVE_SYSV_TTYS */ 240 #ifdef HAVE_BSD_TTYS 241 struct sgttyb ttyb; 242 #endif /* HAVE_BSD_TTYS */ 243 #endif /* HAVE_TIOCGPPSEV */ 244 245 /* 246 * Open serial port 247 */ 248 (void)sprintf(gpsdev, DEVICE, unit); 249 if (!(fd = refclock_open(gpsdev, SPEED232, LDISC_PPS))) { 250 return (0); 251 } 252 #ifdef HAVE_TIOCGPPSEV 253 if (fdpps > 0) { 254 /* 255 * Truly nasty hack in order to get this to work on Solaris 7. 256 * Really, refclock_open() should set the port properly, but 257 * it doesn't work (as of ntp-4.0.98a) - almost 99% dropped 258 * PPS signals with "Interrupted system call". Even this 259 * still gives a 5% error rate. 260 */ 261 ttyb.c_iflag = IGNCR; 262 ttyb.c_oflag = 0; 263 ttyb.c_cflag = CS8 | CREAD | CLOCAL; 264 ttyb.c_lflag = ICANON; 265 if (tcsetattr(fdpps, TCSAFLUSH, &ttyb) < 0) { 266 return (0); 267 } 268 } 269 #endif /* HAVE_TIOCGPPSEV */ 270 271 /* 272 * Allocate unit structure 273 */ 274 if (!(up = (struct mx4200unit *) emalloc(sizeof(struct mx4200unit)))) { 275 (void) close(fd); 276 return (0); 277 } 278 memset((char *)up, 0, sizeof(struct mx4200unit)); 279 pp = peer->procptr; 280 pp->io.clock_recv = mx4200_receive; 281 pp->io.srcclock = (caddr_t)peer; 282 pp->io.datalen = 0; 283 pp->io.fd = fd; 284 if (!io_addclock(&pp->io)) { 285 (void) close(fd); 286 free(up); 287 return (0); 288 } 289 pp->unitptr = (caddr_t)up; 290 291 /* 292 * Initialize miscellaneous variables 293 */ 294 peer->precision = PRECISION; 295 pp->clockdesc = DESCRIPTION; 296 memcpy((char *)&pp->refid, REFID, 4); 297 298 /* Ensure the receiver is properly configured */ 299 mx4200_config(peer); 300 return (1); 301 } 302 303 304 /* 305 * mx4200_shutdown - shut down the clock 306 */ 307 static void 308 mx4200_shutdown( 309 int unit, 310 struct peer *peer 311 ) 312 { 313 register struct mx4200unit *up; 314 struct refclockproc *pp; 315 316 pp = peer->procptr; 317 up = (struct mx4200unit *)pp->unitptr; 318 io_closeclock(&pp->io); 319 free(up); 320 } 321 322 323 /* 324 * mx4200_config - Configure the receiver 325 */ 326 static void 327 mx4200_config( 328 struct peer *peer 329 ) 330 { 331 char tr_mode; 332 int add_mode; 333 register struct mx4200unit *up; 334 struct refclockproc *pp; 335 336 pp = peer->procptr; 337 up = (struct mx4200unit *)pp->unitptr; 338 339 /* 340 * Initialize the unit variables 341 * 342 * STRANGE BEHAVIOUR WARNING: The fudge flags are not available 343 * at the time mx4200_start is called. These are set later, 344 * and so the code must be prepared to handle changing flags. 345 */ 346 up->sloppyclockflag = pp->sloppyclockflag; 347 if (pp->sloppyclockflag & CLK_FLAG2) { 348 up->moving = 1; /* Receiver on mobile platform */ 349 msyslog(LOG_DEBUG, "mx4200_config: mobile platform"); 350 } else { 351 up->moving = 0; /* Static Installation */ 352 } 353 up->pollcnt = 2; 354 up->polled = 0; 355 up->known = 0; 356 up->avg_lat = 0.0; 357 up->avg_lon = 0.0; 358 up->avg_alt = 0.0; 359 up->central_meridian = NOT_INITIALIZED; 360 up->filt_lat = 0.0; 361 up->filt_lon = 0.0; 362 up->filt_alt = 0.0; 363 up->edop = USUAL_EDOP; 364 up->ndop = USUAL_NDOP; 365 up->vdop = USUAL_VDOP; 366 up->last_leap = 0; /* LEAP_NOWARNING */ 367 up->clamp_time = current_time + (AVGING_TIME * 60 * 60); 368 up->log_time = current_time + SLEEPTIME; 369 370 /* 371 * "007" Control Port Configuration 372 * Zero the output list (do it twice to flush possible junk) 373 */ 374 mx4200_send(peer, "%s,%03d,,%d,,,,,,", pmvxg, 375 PMVXG_S_PORTCONF, 376 /* control port output block Label */ 377 1); /* clear current output control list (1=yes) */ 378 /* add/delete sentences from list */ 379 /* must be null */ 380 /* sentence output rate (sec) */ 381 /* precision for position output */ 382 /* nmea version for cga & gll output */ 383 /* pass-through control */ 384 mx4200_send(peer, "%s,%03d,,%d,,,,,,", pmvxg, 385 PMVXG_S_PORTCONF, 1); 386 387 /* 388 * Request software configuration so we can syslog the firmware version 389 */ 390 mx4200_send(peer, "%s,%03d", "CDGPQ", PMVXG_D_SOFTCONF); 391 392 /* 393 * "001" Initialization/Mode Control, Part A 394 * Where ARE we? 395 */ 396 mx4200_send(peer, "%s,%03d,,,,,,,,,,", pmvxg, 397 PMVXG_S_INITMODEA); 398 /* day of month */ 399 /* month of year */ 400 /* year */ 401 /* gmt */ 402 /* latitude DDMM.MMMM */ 403 /* north/south */ 404 /* longitude DDDMM.MMMM */ 405 /* east/west */ 406 /* height */ 407 /* Altitude Reference 1=MSL */ 408 409 /* 410 * "001" Initialization/Mode Control, Part B 411 * Start off in 2d/3d coast mode, holding altitude to last known 412 * value if only 3 satellites available. 413 */ 414 mx4200_send(peer, "%s,%03d,%d,,%.1f,%.1f,%d,%d,%d,%c,%d", 415 pmvxg, PMVXG_S_INITMODEB, 416 3, /* 2d/3d coast */ 417 /* reserved */ 418 0.1, /* hor accel fact as per Steve (m/s**2) */ 419 0.1, /* ver accel fact as per Steve (m/s**2) */ 420 10, /* vdop */ 421 10, /* hdop limit as per Steve */ 422 5, /* elevation limit as per Steve (deg) */ 423 'U', /* time output mode (UTC) */ 424 0); /* local time offset from gmt (HHHMM) */ 425 426 /* 427 * "023" Time Recovery Configuration 428 * Get UTC time from a stationary receiver. 429 * (Set field 1 'D' == dynamic if we are on a moving platform). 430 * (Set field 1 'S' == static if we are not moving). 431 * (Set field 1 'K' == known position if we can initialize lat/lon/alt). 432 */ 433 434 if (pp->sloppyclockflag & CLK_FLAG2) 435 up->moving = 1; /* Receiver on mobile platform */ 436 else 437 up->moving = 0; /* Static Installation */ 438 439 up->pollcnt = 2; 440 if (up->moving) { 441 /* dynamic: solve for pos, alt, time, while moving */ 442 tr_mode = 'D'; 443 } else { 444 /* static: solve for pos, alt, time, while stationary */ 445 tr_mode = 'S'; 446 } 447 mx4200_send(peer, "%s,%03d,%c,%c,%c,%d,%d,%d,", pmvxg, 448 PMVXG_S_TRECOVCONF, 449 tr_mode, /* time recovery mode (see above ) */ 450 'U', /* synchronize to UTC */ 451 'A', /* always output a time pulse */ 452 500, /* max time error in ns */ 453 0, /* user bias in ns */ 454 1); /* output "830" sentences to control port */ 455 /* Multi-satellite mode */ 456 457 /* 458 * Output position information (to calculate fixed installation 459 * location) only if we are not moving 460 */ 461 if (up->moving) { 462 add_mode = 2; /* delete from list */ 463 } else { 464 add_mode = 1; /* add to list */ 465 } 466 467 /* 468 * "007" Control Port Configuration 469 * Output "022" DOPs 470 */ 471 mx4200_send(peer, "%s,%03d,%03d,%d,%d,,%d,,,", pmvxg, 472 PMVXG_S_PORTCONF, 473 PMVXG_D_DOPS, /* control port output block Label */ 474 0, /* clear current output control list (0=no) */ 475 add_mode, /* add/delete sentences from list (1=add, 2=del) */ 476 /* must be null */ 477 INTERVAL); /* sentence output rate (sec) */ 478 /* precision for position output */ 479 /* nmea version for cga & gll output */ 480 /* pass-through control */ 481 482 483 /* 484 * "007" Control Port Configuration 485 * Output "021" position, height, velocity reports 486 */ 487 mx4200_send(peer, "%s,%03d,%03d,%d,%d,,%d,,,", pmvxg, 488 PMVXG_S_PORTCONF, 489 PMVXG_D_PHV, /* control port output block Label */ 490 0, /* clear current output control list (0=no) */ 491 add_mode, /* add/delete sentences from list (1=add, 2=del) */ 492 /* must be null */ 493 INTERVAL); /* sentence output rate (sec) */ 494 /* precision for position output */ 495 /* nmea version for cga & gll output */ 496 /* pass-through control */ 497 } 498 499 /* 500 * mx4200_ref - Reconfigure unit as a reference station at a known position. 501 */ 502 static void 503 mx4200_ref( 504 struct peer *peer 505 ) 506 { 507 register struct mx4200unit *up; 508 struct refclockproc *pp; 509 double minute, lat, lon, alt; 510 char lats[16], lons[16]; 511 char nsc, ewc; 512 513 pp = peer->procptr; 514 up = (struct mx4200unit *)pp->unitptr; 515 516 /* Should never happen! */ 517 if (up->moving) return; 518 519 /* 520 * Set up to output status information in the near future 521 */ 522 up->log_time = current_time + SLEEPTIME; 523 524 /* 525 * "007" Control Port Configuration 526 * Stop outputting "022" DOPs 527 */ 528 mx4200_send(peer, "%s,%03d,%03d,%d,%d,,,,,", pmvxg, 529 PMVXG_S_PORTCONF, 530 PMVXG_D_DOPS, /* control port output block Label */ 531 0, /* clear current output control list (0=no) */ 532 2); /* add/delete sentences from list (2=delete) */ 533 /* must be null */ 534 /* sentence output rate (sec) */ 535 /* precision for position output */ 536 /* nmea version for cga & gll output */ 537 /* pass-through control */ 538 539 /* 540 * "007" Control Port Configuration 541 * Stop outputting "021" position, height, velocity reports 542 */ 543 mx4200_send(peer, "%s,%03d,%03d,%d,%d,,,,,", pmvxg, 544 PMVXG_S_PORTCONF, 545 PMVXG_D_PHV, /* control port output block Label */ 546 0, /* clear current output control list (0=no) */ 547 2); /* add/delete sentences from list (2=delete) */ 548 /* must be null */ 549 /* sentence output rate (sec) */ 550 /* precision for position output */ 551 /* nmea version for cga & gll output */ 552 /* pass-through control */ 553 554 /* 555 * "001" Initialization/Mode Control, Part B 556 * Put receiver in fully-constrained 2d nav mode 557 */ 558 mx4200_send(peer, "%s,%03d,%d,,%.1f,%.1f,%d,%d,%d,%c,%d", 559 pmvxg, PMVXG_S_INITMODEB, 560 2, /* 2d nav */ 561 /* reserved */ 562 0.1, /* hor accel fact as per Steve (m/s**2) */ 563 0.1, /* ver accel fact as per Steve (m/s**2) */ 564 10, /* vdop */ 565 10, /* hdop limit as per Steve */ 566 5, /* elevation limit as per Steve (deg) */ 567 'U', /* time output mode (UTC) */ 568 0); /* local time offset from gmt (HHHMM) */ 569 570 /* 571 * "023" Time Recovery Configuration 572 * Get UTC time from a stationary receiver. Solve for time only. 573 * This should improve the time resolution dramatically. 574 */ 575 mx4200_send(peer, "%s,%03d,%c,%c,%c,%d,%d,%d,", pmvxg, 576 PMVXG_S_TRECOVCONF, 577 'K', /* known position: solve for time only */ 578 'U', /* synchronize to UTC */ 579 'A', /* always output a time pulse */ 580 500, /* max time error in ns */ 581 0, /* user bias in ns */ 582 1); /* output "830" sentences to control port */ 583 /* Multi-satellite mode */ 584 585 /* 586 * "000" Initialization/Mode Control - Part A 587 * Fix to our averaged position. 588 */ 589 if (up->central_meridian != NOT_INITIALIZED) { 590 up->avg_lon += up->central_meridian; 591 if (up->avg_lon < -180.0) up->avg_lon += 360.0; 592 if (up->avg_lon > 180.0) up->avg_lon -= 360.0; 593 } 594 595 if (up->avg_lat >= 0.0) { 596 lat = up->avg_lat; 597 nsc = 'N'; 598 } else { 599 lat = up->avg_lat * (-1.0); 600 nsc = 'S'; 601 } 602 if (up->avg_lon >= 0.0) { 603 lon = up->avg_lon; 604 ewc = 'E'; 605 } else { 606 lon = up->avg_lon * (-1.0); 607 ewc = 'W'; 608 } 609 alt = up->avg_alt; 610 minute = (lat - (double)(int)lat) * 60.0; 611 sprintf(lats,"%02d%02.4f", (int)lat, minute); 612 minute = (lon - (double)(int)lon) * 60.0; 613 sprintf(lons,"%03d%02.4f", (int)lon, minute); 614 615 mx4200_send(peer, "%s,%03d,,,,,%s,%c,%s,%c,%.2f,", pmvxg, 616 PMVXG_S_INITMODEA, 617 /* day of month */ 618 /* month of year */ 619 /* year */ 620 /* gmt */ 621 lats, /* latitude DDMM.MMMM */ 622 nsc, /* north/south */ 623 lons, /* longitude DDDMM.MMMM */ 624 ewc, /* east/west */ 625 alt); /* Altitude */ 626 /* Altitude Reference */ 627 628 msyslog(LOG_DEBUG, 629 "mx4200: reconfig to fixed location: %s %c, %s %c, %.2f m", 630 lats, nsc, lons, ewc, alt ); 631 632 } 633 634 /* 635 * mx4200_poll - mx4200 watchdog routine 636 */ 637 static void 638 mx4200_poll( 639 int unit, 640 struct peer *peer 641 ) 642 { 643 register struct mx4200unit *up; 644 struct refclockproc *pp; 645 646 pp = peer->procptr; 647 up = (struct mx4200unit *)pp->unitptr; 648 649 /* 650 * You don't need to poll this clock. It puts out timecodes 651 * once per second. If asked for a timestamp, take note. 652 * The next time a timecode comes in, it will be fed back. 653 */ 654 655 /* 656 * If we haven't had a response in a while, reset the receiver. 657 */ 658 if (up->pollcnt > 0) { 659 up->pollcnt--; 660 } else { 661 refclock_report(peer, CEVNT_TIMEOUT); 662 663 /* 664 * Request a "000" status message which should trigger a 665 * reconfig 666 */ 667 mx4200_send(peer, "%s,%03d", 668 "CDGPQ", /* query from CDU to GPS */ 669 PMVXG_D_STATUS); /* label of desired sentence */ 670 } 671 672 /* 673 * polled every 64 seconds. Ask mx4200_receive to hand in 674 * a timestamp. 675 */ 676 up->polled = 1; 677 pp->polls++; 678 679 /* 680 * Output receiver status information. 681 */ 682 if ((up->log_time > 0) && (current_time > up->log_time)) { 683 up->log_time = 0; 684 /* 685 * Output the following messages once, for debugging. 686 * "004" Mode Data 687 * "523" Time Recovery Parameters 688 */ 689 mx4200_send(peer, "%s,%03d", "CDGPQ", PMVXG_D_MODEDATA); 690 mx4200_send(peer, "%s,%03d", "CDGPQ", PMVXG_D_TRECOVUSEAGE); 691 } 692 } 693 694 static char char2hex[] = "0123456789ABCDEF"; 695 696 /* 697 * mx4200_receive - receive gps data 698 */ 699 static void 700 mx4200_receive( 701 struct recvbuf *rbufp 702 ) 703 { 704 register struct mx4200unit *up; 705 struct refclockproc *pp; 706 struct peer *peer; 707 char *cp; 708 int sentence_type; 709 u_char ck; 710 711 /* 712 * Initialize pointers and read the timecode and timestamp. 713 */ 714 peer = (struct peer *)rbufp->recv_srcclock; 715 pp = peer->procptr; 716 up = (struct mx4200unit *)pp->unitptr; 717 718 /* 719 * If operating mode has been changed, then reinitialize the receiver 720 * before doing anything else. 721 */ 722 if ((pp->sloppyclockflag & CLK_FLAG2) != 723 (up->sloppyclockflag & CLK_FLAG2)) { 724 up->sloppyclockflag = pp->sloppyclockflag; 725 mx4200_debug(peer, 726 "mx4200_receive: mode switch: reset receiver\n"); 727 mx4200_config(peer); 728 return; 729 } 730 up->sloppyclockflag = pp->sloppyclockflag; 731 732 /* 733 * Read clock output. Automatically handles STREAMS, CLKLDISC. 734 */ 735 pp->lencode = refclock_gtlin(rbufp, pp->a_lastcode, BMAX, &pp->lastrec); 736 737 /* 738 * There is a case where <cr><lf> generates 2 timestamps. 739 */ 740 if (pp->lencode == 0) 741 return; 742 743 up->pollcnt = 2; 744 pp->a_lastcode[pp->lencode] = '\0'; 745 record_clock_stats(&peer->srcadr, pp->a_lastcode); 746 mx4200_debug(peer, "mx4200_receive: %d %s\n", 747 pp->lencode, pp->a_lastcode); 748 749 /* 750 * The structure of the control port sentences is based on the 751 * NMEA-0183 Standard for interfacing Marine Electronics 752 * Navigation Devices (Version 1.5) 753 * 754 * $PMVXG,XXX, ....................*CK<cr><lf> 755 * 756 * $ Sentence Start Identifier (reserved char) 757 * (Start-of-Sentence Identifier) 758 * P Special ID (Proprietary) 759 * MVX Originator ID (Magnavox) 760 * G Interface ID (GPS) 761 * , Field Delimiters (reserved char) 762 * XXX Sentence Type 763 * ...... Data 764 * * Checksum Field Delimiter (reserved char) 765 * CK Checksum 766 * <cr><lf> Carriage-Return/Line Feed (reserved chars) 767 * (End-of-Sentence Identifier) 768 * 769 * Reject if any important landmarks are missing. 770 */ 771 cp = pp->a_lastcode + pp->lencode - 3; 772 if (cp < pp->a_lastcode || *pp->a_lastcode != '$' || cp[0] != '*' ) { 773 mx4200_debug(peer, "mx4200_receive: bad format\n"); 774 refclock_report(peer, CEVNT_BADREPLY); 775 return; 776 } 777 778 /* 779 * Check and discard the checksum 780 */ 781 ck = mx4200_cksum(&pp->a_lastcode[1], pp->lencode - 4); 782 if (char2hex[ck >> 4] != cp[1] || char2hex[ck & 0xf] != cp[2]) { 783 mx4200_debug(peer, "mx4200_receive: bad checksum\n"); 784 refclock_report(peer, CEVNT_BADREPLY); 785 return; 786 } 787 *cp = '\0'; 788 789 /* 790 * Get the sentence type. 791 */ 792 sentence_type = 0; 793 if ((cp = strchr(pp->a_lastcode, ',')) == NULL) { 794 mx4200_debug(peer, "mx4200_receive: no sentence\n"); 795 refclock_report(peer, CEVNT_BADREPLY); 796 return; 797 } 798 cp++; 799 sentence_type = strtol(cp, &cp, 10); 800 801 /* 802 * "000" Status message 803 */ 804 805 if (sentence_type == PMVXG_D_STATUS) { 806 /* 807 * XXX 808 * Since we configure the receiver to not give us status 809 * messages and since the receiver outputs status messages by 810 * default after being reset to factory defaults when sent the 811 * "$PMVXG,018,C\r\n" message, any status message we get 812 * indicates the reciever needs to be initialized; thus, it is 813 * not necessary to decode the status message. 814 */ 815 if ((cp = mx4200_parse_s(peer)) != NULL) { 816 mx4200_debug(peer, 817 "mx4200_receive: status: %s\n", cp); 818 } 819 mx4200_debug(peer, "mx4200_receive: reset receiver\n"); 820 mx4200_config(peer); 821 return; 822 } 823 824 /* 825 * "021" Position, Height, Velocity message, 826 * if we are still averaging our position 827 */ 828 if (sentence_type == PMVXG_D_PHV && !up->known) { 829 /* 830 * Parse the message, calculating our averaged position. 831 */ 832 if ((cp = mx4200_parse_p(peer)) != NULL) { 833 mx4200_debug(peer, "mx4200_receive: pos: %s\n", cp); 834 return; 835 } 836 mx4200_debug(peer, 837 "mx4200_receive: position avg %.9f %.9f %.4f\n", 838 up->avg_lat, up->avg_lon, up->avg_alt); 839 mx4200_debug(peer, 840 "mx4200_receive: position len %.4f %.4f %.4f\n", 841 up->filt_lat, up->filt_lon, up->filt_alt); 842 mx4200_debug(peer, 843 "mx4200_receive: position dop %.1f %.1f %.1f\n", 844 up->ndop, up->edop, up->vdop); 845 /* 846 * Reinitialize as a reference station 847 * if position is well known. 848 */ 849 if (current_time > up->clamp_time) { 850 up->known++; 851 mx4200_debug(peer, "mx4200_receive: reconfiguring!\n"); 852 mx4200_ref(peer); 853 } 854 return; 855 } 856 857 /* 858 * "022" DOPs, if we are still averaging our position 859 */ 860 if (sentence_type == PMVXG_D_DOPS && !up->known) { 861 if ((cp = mx4200_parse_d(peer)) != NULL) { 862 mx4200_debug(peer, "mx4200_receive: dop: %s\n", cp); 863 return; 864 } 865 return; 866 } 867 868 /* 869 * Print to the syslog: 870 * "004" Mode Data 871 * "030" Software Configuration 872 * "523" Time Recovery Parameters Currently in Use 873 */ 874 if (sentence_type == PMVXG_D_MODEDATA || 875 sentence_type == PMVXG_D_SOFTCONF || 876 sentence_type == PMVXG_D_TRECOVUSEAGE ) { 877 if ((cp = mx4200_parse_s(peer)) != NULL) { 878 mx4200_debug(peer, 879 "mx4200_receive: multi-record: %s\n", cp); 880 return; 881 } 882 return; 883 } 884 885 /* 886 * "830" Time Recovery Results message 887 */ 888 if (sentence_type == PMVXG_D_TRECOVOUT) { 889 890 /* 891 * Capture the last PPS signal. 892 * Precision timestamp is returned in pp->lastrec 893 */ 894 if (mx4200_pps(peer) != NULL) { 895 mx4200_debug(peer, "mx4200_receive: pps failure\n"); 896 refclock_report(peer, CEVNT_FAULT); 897 return; 898 } 899 900 901 /* 902 * Parse the time recovery message, and keep the info 903 * to print the pretty billboards. 904 */ 905 if ((cp = mx4200_parse_t(peer)) != NULL) { 906 mx4200_debug(peer, "mx4200_receive: time: %s\n", cp); 907 refclock_report(peer, CEVNT_BADREPLY); 908 return; 909 } 910 911 /* 912 * Add the new sample to a median filter. 913 */ 914 if (!refclock_process(pp)) { 915 mx4200_debug(peer,"mx4200_receive: offset: %.6f\n", 916 pp->offset); 917 refclock_report(peer, CEVNT_BADTIME); 918 return; 919 } 920 921 /* 922 * The clock will blurt a timecode every second but we only 923 * want one when polled. If we havn't been polled, bail out. 924 */ 925 if (!up->polled) 926 return; 927 928 /* 929 * Return offset and dispersion to control module. We use 930 * lastrec as both the reference time and receive time in 931 * order to avoid being cute, like setting the reference time 932 * later than the receive time, which may cause a paranoid 933 * protocol module to chuck out the data. 934 */ 935 mx4200_debug(peer, "mx4200_receive: process time: "); 936 mx4200_debug(peer, "%4d-%03d %02d:%02d:%02d at %s, %.6f\n", 937 pp->year, pp->day, pp->hour, pp->minute, pp->second, 938 prettydate(&pp->lastrec), pp->offset); 939 940 refclock_receive(peer); 941 942 /* 943 * We have succeeded in answering the poll. 944 * Turn off the flag and return 945 */ 946 up->polled = 0; 947 return; 948 } 949 950 /* 951 * Ignore all other sentence types 952 */ 953 return; 954 } 955 956 957 /* 958 * Parse a mx4200 time recovery message. Returns a string if error. 959 * 960 * A typical message looks like this. Checksum has already been stripped. 961 * 962 * $PMVXG,830,T,YYYY,MM,DD,HH:MM:SS,U,S,FFFFFF,PPPPP,BBBBBB,LL 963 * 964 * Field Field Contents 965 * ----- -------------- 966 * Block Label: $PMVXG 967 * Sentence Type: 830=Time Recovery Results 968 * This sentence is output approximately 1 second 969 * preceding the 1PPS output. It indicates the 970 * exact time of the next pulse, whether or not the 971 * time mark will be valid (based on operator-specified 972 * error tolerance), the time to which the pulse is 973 * synchronized, the receiver operating mode, 974 * and the time error of the *last* 1PPS output. 975 * 1 char Time Mark Valid: T=Valid, F=Not Valid 976 * 2 int Year: 1993- 977 * 3 int Month of Year: 1-12 978 * 4 int Day of Month: 1-31 979 * 5 int Time of Day: HH:MM:SS 980 * 6 char Time Synchronization: U=UTC, G=GPS 981 * 7 char Time Recovery Mode: D=Dynamic, S=Static, 982 * K=Known Position, N=No Time Recovery 983 * 8 int Oscillator Offset: The filter's estimate of the oscillator 984 * frequency error, in parts per billion (ppb). 985 * 9 int Time Mark Error: The computed error of the *last* pulse 986 * output, in nanoseconds. 987 * 10 int User Time Bias: Operator specified bias, in nanoseconds 988 * 11 int Leap Second Flag: Indicates that a leap second will 989 * occur. This value is usually zero, except during 990 * the week prior to the leap second occurence, when 991 * this value will be set to +1 or -1. A value of 992 * +1 indicates that GPS time will be 1 second 993 * further ahead of UTC time. 994 * 995 */ 996 static char * 997 mx4200_parse_t( 998 struct peer *peer 999 ) 1000 { 1001 struct refclockproc *pp; 1002 struct mx4200unit *up; 1003 char time_mark_valid, time_sync, op_mode; 1004 int sentence_type, valid; 1005 int year, day_of_year, month, day_of_month, hour, minute, second, leapsec; 1006 int oscillator_offset, time_mark_error, time_bias; 1007 1008 pp = peer->procptr; 1009 up = (struct mx4200unit *)pp->unitptr; 1010 1011 leapsec = 0; /* Not all receivers output leap second warnings (!) */ 1012 sscanf(pp->a_lastcode, "$PMVXG,%d,%c,%d,%d,%d,%d:%d:%d,%c,%c,%d,%d,%d,%d", 1013 &sentence_type, &time_mark_valid, &year, &month, &day_of_month, 1014 &hour, &minute, &second, &time_sync, &op_mode, &oscillator_offset, 1015 &time_mark_error, &time_bias, &leapsec); 1016 1017 if (sentence_type != PMVXG_D_TRECOVOUT) 1018 return ("wrong rec-type"); 1019 1020 switch (time_mark_valid) { 1021 case 'T': 1022 valid = 1; 1023 break; 1024 case 'F': 1025 valid = 0; 1026 break; 1027 default: 1028 return ("bad pulse-valid"); 1029 } 1030 1031 switch (time_sync) { 1032 case 'G': 1033 return ("synchronized to GPS; should be UTC"); 1034 case 'U': 1035 break; /* UTC -> ok */ 1036 default: 1037 return ("not synchronized to UTC"); 1038 } 1039 1040 /* 1041 * Check for insane time (allow for possible leap seconds) 1042 */ 1043 if (second > 60 || minute > 59 || hour > 23 || 1044 second < 0 || minute < 0 || hour < 0) { 1045 mx4200_debug(peer, 1046 "mx4200_parse_t: bad time %02d:%02d:%02d", 1047 hour, minute, second); 1048 if (leapsec != 0) 1049 mx4200_debug(peer, " (leap %+d\n)", leapsec); 1050 mx4200_debug(peer, "\n"); 1051 refclock_report(peer, CEVNT_BADTIME); 1052 return ("bad time"); 1053 } 1054 if ( second == 60 ) { 1055 msyslog(LOG_DEBUG, 1056 "mx4200: leap second! %02d:%02d:%02d", 1057 hour, minute, second); 1058 } 1059 1060 /* 1061 * Check for insane date 1062 * (Certainly can't be any year before this code was last altered!) 1063 */ 1064 if (day_of_month > 31 || month > 12 || 1065 day_of_month < 1 || month < 1 || year < YEAR_RIGHT_NOW) { 1066 mx4200_debug(peer, 1067 "mx4200_parse_t: bad date (%4d-%02d-%02d)\n", 1068 year, month, day_of_month); 1069 refclock_report(peer, CEVNT_BADDATE); 1070 return ("bad date"); 1071 } 1072 1073 /* 1074 * Silly Hack for MX4200: 1075 * ASCII message is for *next* 1PPS signal, but we have the 1076 * timestamp for the *last* 1PPS signal. So we have to subtract 1077 * a second. Discard if we are on a month boundary to avoid 1078 * possible leap seconds and leap days. 1079 */ 1080 second--; 1081 if (second < 0) { 1082 second = 59; 1083 minute--; 1084 if (minute < 0) { 1085 minute = 59; 1086 hour--; 1087 if (hour < 0) { 1088 hour = 23; 1089 day_of_month--; 1090 if (day_of_month < 1) { 1091 return ("sorry, month boundary"); 1092 } 1093 } 1094 } 1095 } 1096 1097 /* 1098 * Calculate Julian date 1099 */ 1100 if (!(day_of_year = mx4200_jday(year, month, day_of_month))) { 1101 mx4200_debug(peer, 1102 "mx4200_parse_t: bad julian date %d (%4d-%02d-%02d)\n", 1103 day_of_year, year, month, day_of_month); 1104 refclock_report(peer, CEVNT_BADDATE); 1105 return("invalid julian date"); 1106 } 1107 1108 /* 1109 * Setup leap second indicator 1110 */ 1111 switch (leapsec) { 1112 case 0: 1113 pp->leap = LEAP_NOWARNING; 1114 break; 1115 case 1: 1116 pp->leap = LEAP_ADDSECOND; 1117 break; 1118 case -1: 1119 pp->leap = LEAP_DELSECOND; 1120 break; 1121 default: 1122 pp->leap = LEAP_NOTINSYNC; 1123 } 1124 1125 /* 1126 * Any change to the leap second warning status? 1127 */ 1128 if (leapsec != up->last_leap ) { 1129 msyslog(LOG_DEBUG, 1130 "mx4200: leap second warning: %d to %d (%d)", 1131 up->last_leap, leapsec, pp->leap); 1132 } 1133 up->last_leap = leapsec; 1134 1135 /* 1136 * Copy time data for billboard monitoring. 1137 */ 1138 1139 pp->year = year; 1140 pp->day = day_of_year; 1141 pp->hour = hour; 1142 pp->minute = minute; 1143 pp->second = second; 1144 pp->msec = 0; 1145 pp->usec = 0; 1146 1147 /* 1148 * Toss if sentence is marked invalid 1149 */ 1150 if (!valid || pp->leap == LEAP_NOTINSYNC) { 1151 mx4200_debug(peer, "mx4200_parse_t: time mark not valid\n"); 1152 refclock_report(peer, CEVNT_BADTIME); 1153 return ("pulse invalid"); 1154 } 1155 1156 return (NULL); 1157 } 1158 1159 /* 1160 * Calculate the checksum 1161 */ 1162 static u_char 1163 mx4200_cksum( 1164 register char *cp, 1165 register int n 1166 ) 1167 { 1168 register u_char ck; 1169 1170 for (ck = 0; n-- > 0; cp++) 1171 ck ^= *cp; 1172 return (ck); 1173 } 1174 1175 /* 1176 * Tables to compute the day of year. Viva la leap. 1177 */ 1178 static int day1tab[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; 1179 static int day2tab[] = {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; 1180 1181 /* 1182 * Calculate the the Julian Day 1183 */ 1184 static int 1185 mx4200_jday( 1186 int year, 1187 int month, 1188 int day_of_month 1189 ) 1190 { 1191 register int day, i; 1192 int leap_year; 1193 1194 /* 1195 * Is this a leap year ? 1196 */ 1197 if (year % 4) { 1198 leap_year = 0; /* FALSE */ 1199 } else { 1200 if (year % 100) { 1201 leap_year = 1; /* TRUE */ 1202 } else { 1203 if (year % 400) { 1204 leap_year = 0; /* FALSE */ 1205 } else { 1206 leap_year = 1; /* TRUE */ 1207 } 1208 } 1209 } 1210 1211 /* 1212 * Calculate the Julian Date 1213 */ 1214 day = day_of_month; 1215 1216 if (leap_year) { 1217 /* a leap year */ 1218 if (day > day2tab[month - 1]) { 1219 return (0); 1220 } 1221 for (i = 0; i < month - 1; i++) 1222 day += day2tab[i]; 1223 } else { 1224 /* not a leap year */ 1225 if (day > day1tab[month - 1]) { 1226 return (0); 1227 } 1228 for (i = 0; i < month - 1; i++) 1229 day += day1tab[i]; 1230 } 1231 return (day); 1232 } 1233 1234 /* 1235 * Parse a mx4200 position/height/velocity sentence. 1236 * 1237 * A typical message looks like this. Checksum has already been stripped. 1238 * 1239 * $PMVXG,021,SSSSSS.SS,DDMM.MMMM,N,DDDMM.MMMM,E,HHHHH.H,GGGG.G,EEEE.E,WWWW.W,MM 1240 * 1241 * Field Field Contents 1242 * ----- -------------- 1243 * Block Label: $PMVXG 1244 * Sentence Type: 021=Position, Height Velocity Data 1245 * This sentence gives the receiver position, height, 1246 * navigation mode, and velocity north/east. 1247 * *This sentence is intended for post-analysis 1248 * applications.* 1249 * 1 float UTC measurement time (seconds into week) 1250 * 2 float WGS-84 Lattitude (degrees, minutes) 1251 * 3 char N=North, S=South 1252 * 4 float WGS-84 Longitude (degrees, minutes) 1253 * 5 char E=East, W=West 1254 * 6 float Altitude (meters above mean sea level) 1255 * 7 float Geoidal height (meters) 1256 * 8 float East velocity (m/sec) 1257 * 9 float West Velocity (m/sec) 1258 * 10 int Navigation Mode 1259 * Mode if navigating: 1260 * 1 = Position from remote device 1261 * 2 = 2-D position 1262 * 3 = 3-D position 1263 * 4 = 2-D differential position 1264 * 5 = 3-D differential position 1265 * 6 = Static 1266 * 8 = Position known -- reference station 1267 * 9 = Position known -- Navigator 1268 * Mode if not navigating: 1269 * 51 = Too few satellites 1270 * 52 = DOPs too large 1271 * 53 = Position STD too large 1272 * 54 = Velocity STD too large 1273 * 55 = Too many iterations for velocity 1274 * 56 = Too many iterations for position 1275 * 57 = 3 sat startup failed 1276 * 58 = Command abort 1277 */ 1278 static char * 1279 mx4200_parse_p( 1280 struct peer *peer 1281 ) 1282 { 1283 struct refclockproc *pp; 1284 struct mx4200unit *up; 1285 int sentence_type, mode; 1286 double mtime, lat, lon, alt, geoid, vele, veln, weight; 1287 char north_south, east_west; 1288 1289 pp = peer->procptr; 1290 up = (struct mx4200unit *)pp->unitptr; 1291 1292 /* Should never happen! */ 1293 if (up->moving) return ("mobile platform - no pos!"); 1294 1295 sscanf ( pp->a_lastcode, "$PMVXG,%d,%lf,%lf,%c,%lf,%c,%lf,%lf,%lf,%lf,%d", 1296 &sentence_type, &mtime, &lat, &north_south, &lon, &east_west, &alt, 1297 &geoid, &vele, &veln, &mode); 1298 1299 /* Sentence type */ 1300 if (sentence_type != PMVXG_D_PHV) 1301 return ("wrong rec-type"); 1302 1303 /* 1304 * return if not navigating 1305 */ 1306 if (mode > 10) 1307 return ("not navigating"); 1308 if (mode != 3 && mode != 5) 1309 return ("not navigating in 3D"); 1310 1311 /* Latitude (always +ve) and convert DDMM.MMMM to decimal */ 1312 if (lat < 0.0) return ("negative latitude"); 1313 if (lat > 9000.0) lat = 9000.0; 1314 lat *= 0.01; 1315 lat = ((int)lat) + (((lat - (int)lat)) * 1.6666666666666666); 1316 1317 /* North/South */ 1318 switch (north_south) { 1319 case 'N': 1320 break; 1321 case 'S': 1322 lat *= -1.0; 1323 break; 1324 default: 1325 return ("invalid north/south indicator"); 1326 } 1327 1328 /* Longitude (always +ve) and convert DDDMM.MMMM to decimal */ 1329 if (lon < 0.0) return ("negative longitude"); 1330 if (lon > 180.0) lon = 180.0; 1331 lon *= 0.01; 1332 lon = ((int)lon) + (((lon - (int)lon)) * 1.6666666666666666); 1333 1334 /* East/West */ 1335 switch (east_west) { 1336 case 'E': 1337 break; 1338 case 'W': 1339 lon *= -1.0; 1340 break; 1341 default: 1342 return ("invalid east/west indicator"); 1343 } 1344 1345 /* 1346 * Normalize longitude to near 0 degrees. 1347 * Assume all data are clustered around first reading. 1348 */ 1349 if (up->central_meridian == NOT_INITIALIZED) { 1350 up->central_meridian = lon; 1351 mx4200_debug(peer, 1352 "mx4200_receive: central meridian = %.9f \n", 1353 up->central_meridian); 1354 } 1355 lon -= up->central_meridian; 1356 if (lon < -180.0) lon += 360.0; 1357 if (lon > 180.0) lon -= 360.0; 1358 1359 /* 1360 * Calculate running weighted averages 1361 */ 1362 weight = USUAL_EDOP / up->edop; 1363 weight *= weight; 1364 up->avg_lon = (up->filt_lon * up->avg_lon) + (weight * lon); 1365 up->filt_lon += weight; 1366 up->avg_lon = up->avg_lon / up->filt_lon; 1367 1368 weight = USUAL_NDOP / up->ndop; 1369 weight *= weight; 1370 up->avg_lat = (up->filt_lat * up->avg_lat) + (weight * lat); 1371 up->filt_lat += weight; 1372 up->avg_lat = up->avg_lat / up->filt_lat; 1373 1374 weight = USUAL_VDOP / up->vdop; 1375 weight *= weight; 1376 up->avg_alt = (up->filt_alt * up->avg_alt) + (weight * alt); 1377 up->filt_alt += weight; 1378 up->avg_alt = up->avg_alt / up->filt_alt; 1379 1380 mx4200_debug(peer, 1381 "mx4200_receive: position rdg %.9f %.9f %.4f (CM=%.9f)\n", 1382 lat, lon, alt, up->central_meridian); 1383 1384 return (NULL); 1385 } 1386 1387 /* 1388 * Parse a mx4200 DOP sentence. 1389 * 1390 * A typical message looks like this. Checksum has already been stripped. 1391 * 1392 * $PMVXG,022,SSSSSS.SSEE.E,NN.N,VV.V,XX,XX,XX,XX,XX,XX 1393 * 1394 * Field Field Contents 1395 * ----- -------------- 1396 * Block Label: $PMVXG 1397 * Sentence Type: 022=DOPs. The DOP values in this sentence 1398 * correspond to the satellites listed. The PRNs in 1399 * the message are listed in receiver channel number order 1400 * 1 UTC measurement time (seconds into week) 1401 * 2 EDOP (east DOP) 1402 * 3 NDOP (north DOP) 1403 * 4 VDOP (vertical DOP) 1404 * 5 PRN on channel 1 1405 * 6 PRN on channel 2 1406 * 7 PRN on channel 3 1407 * 8 PRN on channel 4 1408 * 9 PRN on channel 5 1409 * 10 PRN on channel 6 1410 * 11 PRN on channel 7 (12-channel receivers only) 1411 * 12 PRN on channel 8 (12-channel receivers only) 1412 * 13 PRN on channel 9 (12-channel receivers only) 1413 * 14 PRN on channel 10 (12-channel receivers only) 1414 * 15 PRN on channel 11 (12-channel receivers only) 1415 * 16 PRN on channel 12 (12-channel receivers only) 1416 */ 1417 static char * 1418 mx4200_parse_d( 1419 struct peer *peer 1420 ) 1421 { 1422 struct refclockproc *pp; 1423 struct mx4200unit *up; 1424 int sentence_type; 1425 double mtime, edop, ndop, vdop; 1426 1427 pp = peer->procptr; 1428 up = (struct mx4200unit *)pp->unitptr; 1429 1430 /* Should never happen! */ 1431 if (up->moving) return ("mobile platform - no dop!"); 1432 1433 sscanf ( pp->a_lastcode, "$PMVXG,%d,%lf,%lf,%lf,%lf", 1434 &sentence_type, &mtime, &edop, &ndop, &vdop); 1435 1436 /* Sentence type */ 1437 if (sentence_type != PMVXG_D_DOPS) 1438 return ("wrong rec-type"); 1439 1440 /* Update values */ 1441 if (edop <= 0.0 || ndop <= 0.0 || vdop <= 0.0) 1442 return ("nonpositive dop"); 1443 up->edop = edop; 1444 up->ndop = ndop; 1445 up->vdop = vdop; 1446 1447 return (NULL); 1448 } 1449 1450 /* 1451 * Parse a mx4200 Status sentence 1452 * Parse a mx4200 Mode Data sentence 1453 * Parse a mx4200 Software Configuration sentence 1454 * Parse a mx4200 Time Recovery Parameters Currently in Use sentence 1455 * (used only for logging raw strings) 1456 * 1457 * A typical message looks like this. Checksum has already been stripped. 1458 * 1459 * $PMVXG,000,XXX,XX,X,HHMM,X 1460 * 1461 * Field Field Contents 1462 * ----- -------------- 1463 * Block Label: $PMVXG 1464 * Sentence Type: 000=Status. 1465 * Returns status of the receiver to the controller. 1466 * 1 Current Receiver Status: 1467 * ACQ = Satellite re-acquisition 1468 * ALT = Constellation selection 1469 * COR = Providing corrections (for reference stations only) 1470 * IAC = Initial acquisition 1471 * IDL = Idle, no satellites 1472 * NAV = Navigation 1473 * STS = Search the Sky (no almanac available) 1474 * TRK = Tracking 1475 * 2 Number of satellites that should be visible 1476 * 3 Number of satellites being tracked 1477 * 4 Time since last navigation status if not currently navigating 1478 * (hours, minutes) 1479 * 5 Initialization status: 1480 * 0 = Waiting for initialization parameters 1481 * 1 = Initialization completed 1482 * 1483 * A typical message looks like this. Checksum has already been stripped. 1484 * 1485 * $PMVXG,004,C,R,D,H.HH,V.VV,TT,HHHH,VVVV,T 1486 * 1487 * Field Field Contents 1488 * ----- -------------- 1489 * Block Label: $PMVXG 1490 * Sentence Type: 004=Software Configuration. 1491 * Defines the navigation mode and criteria for 1492 * acceptable navigation for the receiver. 1493 * 1 Constrain Altitude Mode: 1494 * 0 = Auto. Constrain altitude (2-D solution) and use 1495 * manual altitude input when 3 sats avalable. Do 1496 * not constrain altitude (3-D solution) when 4 sats 1497 * available. 1498 * 1 = Always constrain altitude (2-D solution). 1499 * 2 = Never constrain altitude (3-D solution). 1500 * 3 = Coast. Constrain altitude (2-D solution) and use 1501 * last GPS altitude calculation when 3 sats avalable. 1502 * Do not constrain altitude (3-D solution) when 4 sats 1503 * available. 1504 * 2 Altitude Reference: (always 0 for MX4200) 1505 * 0 = Ellipsoid 1506 * 1 = Geoid (MSL) 1507 * 3 Differential Navigation Control: 1508 * 0 = Disabled 1509 * 1 = Enabled 1510 * 4 Horizontal Acceleration Constant (m/sec**2) 1511 * 5 Vertical Acceleration Constant (m/sec**2) (0 for MX4200) 1512 * 6 Tracking Elevation Limit (degrees) 1513 * 7 HDOP Limit 1514 * 8 VDOP Limit 1515 * 9 Time Output Mode: 1516 * U = UTC 1517 * L = Local time 1518 * 10 Local Time Offset (minutes) (absent on MX4200) 1519 * 1520 * A typical message looks like this. Checksum has already been stripped. 1521 * 1522 * $PMVXG,030,NNNN,FFF 1523 * 1524 * Field Field Contents 1525 * ----- -------------- 1526 * Block Label: $PMVXG 1527 * Sentence Type: 030=Software Configuration. 1528 * This sentence contains the navigation processor 1529 * and baseband firmware version numbers. 1530 * 1 Nav Processor Version Number 1531 * 2 Baseband Firmware Version Number 1532 * 1533 * A typical message looks like this. Checksum has already been stripped. 1534 * 1535 * $PMVXG,523,M,S,M,EEEE,BBBBBB,C,R 1536 * 1537 * Field Field Contents 1538 * ----- -------------- 1539 * Block Label: $PMVXG 1540 * Sentence Type: 523=Time Recovery Parameters Currently in Use. 1541 * This sentence contains the configuration of the 1542 * time recovery feature of the receiver. 1543 * 1 Time Recovery Mode: 1544 * D = Dynamic; solve for position and time while moving 1545 * S = Static; solve for position and time while stationary 1546 * K = Known position input, solve for time only 1547 * N = No time recovery 1548 * 2 Time Synchronization: 1549 * U = UTC time 1550 * G = GPS time 1551 * 3 Time Mark Mode: 1552 * A = Always output a time pulse 1553 * V = Only output time pulse if time is valid (as determined 1554 * by Maximum Time Error) 1555 * 4 Maximum Time Error - the maximum error (in nanoseconds) for 1556 * which a time mark will be considered valid. 1557 * 5 User Time Bias - external bias in nanoseconds 1558 * 6 Time Message Control: 1559 * 0 = Do not output the time recovery message 1560 * 1 = Output the time recovery message (record 830) to 1561 * Control port 1562 * 2 = Output the time recovery message (record 830) to 1563 * Equipment port 1564 * 7 Reserved 1565 * 8 Position Known PRN (absent on MX 4200) 1566 * 1567 */ 1568 static char * 1569 mx4200_parse_s( 1570 struct peer *peer 1571 ) 1572 { 1573 struct refclockproc *pp; 1574 struct mx4200unit *up; 1575 int sentence_type; 1576 1577 pp = peer->procptr; 1578 up = (struct mx4200unit *)pp->unitptr; 1579 1580 sscanf ( pp->a_lastcode, "$PMVXG,%d", &sentence_type); 1581 1582 /* Sentence type */ 1583 switch (sentence_type) { 1584 1585 case PMVXG_D_STATUS: 1586 msyslog(LOG_DEBUG, 1587 "mx4200: status: %s", pp->a_lastcode); 1588 break; 1589 case PMVXG_D_MODEDATA: 1590 msyslog(LOG_DEBUG, 1591 "mx4200: mode data: %s", pp->a_lastcode); 1592 break; 1593 case PMVXG_D_SOFTCONF: 1594 msyslog(LOG_DEBUG, 1595 "mx4200: firmware configuration: %s", pp->a_lastcode); 1596 break; 1597 case PMVXG_D_TRECOVUSEAGE: 1598 msyslog(LOG_DEBUG, 1599 "mx4200: time recovery parms: %s", pp->a_lastcode); 1600 break; 1601 default: 1602 return ("wrong rec-type"); 1603 } 1604 1605 return (NULL); 1606 } 1607 1608 /* 1609 * Process a PPS signal, returning a timestamp. 1610 */ 1611 static int 1612 mx4200_pps( 1613 struct peer *peer 1614 ) 1615 { 1616 int temp_serial; 1617 struct refclockproc *pp; 1618 struct mx4200unit *up; 1619 1620 int request; 1621 #ifdef HAVE_CIOGETEV 1622 request = CIOGETEV; 1623 #endif 1624 #ifdef HAVE_TIOCGPPSEV 1625 request = TIOCGPPSEV; 1626 #endif 1627 1628 pp = peer->procptr; 1629 up = (struct mx4200unit *)pp->unitptr; 1630 1631 /* 1632 * Grab the timestamp of the PPS signal. 1633 */ 1634 temp_serial = up->ppsev.serial; 1635 if (ioctl(fdpps, request, (caddr_t)&up->ppsev) < 0) { 1636 /* XXX Actually, if this fails, we're pretty much screwed */ 1637 mx4200_debug(peer, 1638 "mx4200_pps: CIOGETEV/TIOCGPPSEV: serial=%d, fdpps=%d, %s\n", 1639 up->ppsev.serial, fdpps, strerror(errno)); 1640 refclock_report(peer, CEVNT_FAULT); 1641 return(1); 1642 } 1643 if (temp_serial == up->ppsev.serial) { 1644 mx4200_debug(peer, 1645 "mx4200_pps: ppsev serial not incrementing: %d\n", 1646 up->ppsev.serial); 1647 refclock_report(peer, CEVNT_FAULT); 1648 return(1); 1649 } 1650 1651 /* 1652 * Check pps serial number against last one 1653 */ 1654 if (up->lastserial + 1 != up->ppsev.serial && up->lastserial != 0) { 1655 if (up->ppsev.serial == up->lastserial) 1656 mx4200_debug(peer, "mx4200_pps: no new pps event\n"); 1657 else 1658 mx4200_debug(peer, "mx4200_pps: missed %d pps events\n", 1659 up->ppsev.serial - up->lastserial - 1); 1660 refclock_report(peer, CEVNT_FAULT); 1661 } 1662 up->lastserial = up->ppsev.serial; 1663 1664 /* 1665 * Return the timestamp in pp->lastrec 1666 */ 1667 up->ppsev.tv.tv_sec += (u_int32) JAN_1970; 1668 TVTOTS(&up->ppsev.tv,&pp->lastrec); 1669 1670 return(0); 1671 } 1672 1673 /* 1674 * mx4200_debug - print debug messages 1675 */ 1676 #if __STDC__ 1677 static void 1678 mx4200_debug(struct peer *peer, char *fmt, ...) 1679 #else 1680 static void 1681 mx4200_debug(peer, fmt, va_alist) 1682 struct peer *peer; 1683 char *fmt; 1684 #endif 1685 { 1686 va_list ap; 1687 struct refclockproc *pp; 1688 struct mx4200unit *up; 1689 1690 if (debug) { 1691 1692 #if __STDC__ 1693 va_start(ap, fmt); 1694 #else 1695 va_start(ap); 1696 #endif 1697 1698 pp = peer->procptr; 1699 up = (struct mx4200unit *)pp->unitptr; 1700 1701 1702 /* 1703 * Print debug message to stdout 1704 * In the future, we may want to get get more creative... 1705 */ 1706 vprintf(fmt, ap); 1707 1708 va_end(ap); 1709 } 1710 } 1711 1712 /* 1713 * Send a character string to the receiver. Checksum is appended here. 1714 */ 1715 static void 1716 #if __STDC__ 1717 mx4200_send(struct peer *peer, char *fmt, ...) 1718 #else 1719 mx4200_send(peer, fmt, va_alist) 1720 struct peer *peer; 1721 char *fmt; 1722 va_dcl 1723 #endif /* __STDC__ */ 1724 { 1725 struct refclockproc *pp; 1726 struct mx4200unit *up; 1727 1728 register char *cp; 1729 register int n, m; 1730 va_list ap; 1731 char buf[1024]; 1732 u_char ck; 1733 1734 #if __STDC__ 1735 va_start(ap, fmt); 1736 #else 1737 va_start(ap); 1738 #endif /* __STDC__ */ 1739 1740 pp = peer->procptr; 1741 up = (struct mx4200unit *)pp->unitptr; 1742 1743 cp = buf; 1744 *cp++ = '$'; 1745 #ifdef notdef 1746 /* BSD is rational */ 1747 n = vsnprintf(cp, sizeof(buf) - 1, fmt, ap); 1748 #else 1749 /* SunOS sucks */ 1750 (void)vsprintf(cp, fmt, ap); 1751 n = strlen(cp); 1752 #endif /* notdef */ 1753 ck = mx4200_cksum(cp, n); 1754 cp += n; 1755 ++n; 1756 #ifdef notdef 1757 /* BSD is rational */ 1758 n += snprintf(cp, sizeof(buf) - n - 5, "*%02X\r\n", ck); 1759 #else 1760 /* SunOS sucks */ 1761 sprintf(cp, "*%02X\r\n", ck); 1762 n += strlen(cp); 1763 #endif /* notdef */ 1764 1765 m = write(pp->io.fd, buf, (unsigned)n); 1766 if (m < 0) 1767 msyslog(LOG_ERR, "mx4200_send: write: %m (%s)", buf); 1768 mx4200_debug(peer, "mx4200_send: %d %s\n", m, buf); 1769 va_end(ap); 1770 } 1771 1772 #else 1773 int refclock_mx4200_bs; 1774 #endif /* REFCLOCK */ 1775