diff options
Diffstat (limited to 'contrib/ntp/ntpd/refclock_mx4200.c')
| -rw-r--r-- | contrib/ntp/ntpd/refclock_mx4200.c | 404 |
1 files changed, 165 insertions, 239 deletions
diff --git a/contrib/ntp/ntpd/refclock_mx4200.c b/contrib/ntp/ntpd/refclock_mx4200.c index f08bb23..3c520b0 100644 --- a/contrib/ntp/ntpd/refclock_mx4200.c +++ b/contrib/ntp/ntpd/refclock_mx4200.c @@ -47,11 +47,7 @@ # include <config.h> #endif -#if defined(REFCLOCK) && defined(CLOCK_MX4200) && defined(PPS) - -#include <stdio.h> -#include <ctype.h> -#include <sys/types.h> +#if defined(REFCLOCK) && defined(CLOCK_MX4200) && defined(HAVE_PPSAPI) #include "ntpd.h" #include "ntp_io.h" @@ -59,11 +55,11 @@ #include "ntp_unixtime.h" #include "ntp_stdlib.h" +#include <stdio.h> +#include <ctype.h> + #include "mx4200.h" -#ifdef HAVE_SYS_TIME_H -# include <sys/time.h> -#endif #ifdef HAVE_SYS_TERMIOS_H # include <sys/termios.h> #endif @@ -73,7 +69,7 @@ #ifndef HAVE_STRUCT_PPSCLOCKEV struct ppsclockev { -# ifdef HAVE_TIMESPEC +# ifdef HAVE_STRUCT_TIMESPEC struct timespec tv; # else struct timeval tv; @@ -82,6 +78,14 @@ struct ppsclockev { }; #endif /* ! HAVE_STRUCT_PPSCLOCKEV */ +#ifdef HAVE_TIMEPPS_H +# include <timepps.h> +#else +# ifdef HAVE_SYS_TIMEPPS_H +# include <sys/timepps.h> +# endif +#endif + /* * This driver supports the Magnavox Model MX 4200 GPS Receiver * adapted to precision timing applications. It requires the @@ -97,7 +101,7 @@ struct ppsclockev { /* * Check this every time you edit the code! */ -#define YEAR_RIGHT_NOW 1998 +#define YEAR_LAST_MODIFIED 2000 /* * GPS Definitions @@ -117,13 +121,6 @@ struct ppsclockev { /* * Position Averaging. - * Reference: Dr. Thomas A. Clark's Totally Accurate Clock (TAC) files at - * ftp://aleph.gsfc.nasa.gov/GPS/totally.accurate.clock/ - * For a 6-channel Motorola Oncore, he indicates that good nominal - * HDOP and VDOP are 1.50 and 2.00 respectively. Given the relationship - * HDOP^2 = NDOP^2 + EDOP^2 and assuming EDOP and NDOP are equal, we - * have a nominal NDOP = EDOP = sqrt((HDOP*HDOP)/2). An 8-channel - * Oncore does well with HDOP=1.20 and VDOP=1.70. */ #define INTERVAL 1 /* Interval between position measurements (s) */ #define AVGING_TIME 24 /* Number of hours to average */ @@ -141,18 +138,16 @@ struct mx4200unit { double avg_lon; /* average longitude */ double avg_alt; /* average height */ double central_meridian; /* central meridian */ - double filt_lat; /* latitude filter length */ - double filt_lon; /* longitude filter length */ - double filt_alt; /* height filter length */ - double edop; /* EDOP (east DOP) */ - double ndop; /* NDOP (north DOP) */ - double vdop; /* VDOP (vertical DOP) */ + double N_fixes; /* Number of position measurements */ int last_leap; /* leap second warning */ u_int moving; /* mobile platform? */ u_long sloppyclockflag; /* fudge flags */ u_int known; /* position known yet? */ u_long clamp_time; /* when to stop postion averaging */ u_long log_time; /* when to print receiver status */ + pps_handle_t pps_h; + pps_params_t pps_p; + pps_info_t pps_i; }; static char pmvxg[] = "PMVXG"; @@ -181,14 +176,13 @@ static void mx4200_poll P((int, struct peer *)); static char * mx4200_parse_t P((struct peer *)); static char * mx4200_parse_p P((struct peer *)); -static char * mx4200_parse_d P((struct peer *)); static char * mx4200_parse_s P((struct peer *)); #ifdef QSORT_USES_VOID_P int mx4200_cmpl_fp P((const void *, const void *)); #else int mx4200_cmpl_fp P((const l_fp *, const l_fp *)); #endif /* not QSORT_USES_VOID_P */ -static void mx4200_config P((struct peer *)); +static int mx4200_config P((struct peer *)); static void mx4200_ref P((struct peer *)); static void mx4200_send P((struct peer *, char *, ...)) __attribute__ ((format (printf, 2, 3))); @@ -239,6 +233,7 @@ mx4200_start( * Allocate unit structure */ if (!(up = (struct mx4200unit *) emalloc(sizeof(struct mx4200unit)))) { + perror("emalloc"); (void) close(fd); return (0); } @@ -263,8 +258,7 @@ mx4200_start( memcpy((char *)&pp->refid, REFID, 4); /* Ensure the receiver is properly configured */ - mx4200_config(peer); - return (1); + return mx4200_config(peer); } @@ -290,7 +284,7 @@ mx4200_shutdown( /* * mx4200_config - Configure the receiver */ -static void +static int mx4200_config( struct peer *peer ) @@ -299,6 +293,7 @@ mx4200_config( int add_mode; register struct mx4200unit *up; struct refclockproc *pp; + int mode; pp = peer->procptr; up = (struct mx4200unit *)pp->unitptr; @@ -324,16 +319,52 @@ mx4200_config( up->avg_lon = 0.0; up->avg_alt = 0.0; up->central_meridian = NOT_INITIALIZED; - up->filt_lat = 0.0; - up->filt_lon = 0.0; - up->filt_alt = 0.0; - up->edop = 1; - up->ndop = 1; - up->vdop = 1; + up->N_fixes = 0.0; up->last_leap = 0; /* LEAP_NOWARNING */ up->clamp_time = current_time + (AVGING_TIME * 60 * 60); up->log_time = current_time + SLEEPTIME; + if (time_pps_create(pp->io.fd, &up->pps_h) < 0) { + perror("time_pps_create"); + msyslog(LOG_ERR, + "mx4200_config: time_pps_create failed: %m"); + return (0); + } + if (time_pps_getcap(up->pps_h, &mode) < 0) { + msyslog(LOG_ERR, + "mx4200_config: time_pps_getcap failed: %m"); + return (0); + } + + if (time_pps_getparams(up->pps_h, &up->pps_p) < 0) { + msyslog(LOG_ERR, + "mx4200_config: time_pps_getparams failed: %m"); + return (0); + } + + /* nb. only turn things on, if someone else has turned something + * on before we get here, leave it alone! + */ + + up->pps_p.mode = PPS_CAPTUREASSERT | PPS_TSFMT_TSPEC; + up->pps_p.mode &= mode; /* only set what is legal */ + + if (time_pps_setparams(up->pps_h, &up->pps_p) < 0) { + perror("time_pps_setparams"); + msyslog(LOG_ERR, + "mx4200_config: time_pps_setparams failed: %m"); + exit(1); + } + + if (time_pps_kcbind(up->pps_h, PPS_KC_HARDPPS, PPS_CAPTUREASSERT, + PPS_TSFMT_TSPEC) < 0) { + perror("time_pps_kcbind"); + msyslog(LOG_ERR, + "mx4200_config: time_pps_kcbind failed: %m"); + exit(1); + } + + /* * "007" Control Port Configuration * Zero the output list (do it twice to flush possible junk) @@ -419,7 +450,7 @@ mx4200_config( 500, /* max time error in ns */ 0, /* user bias in ns */ 1); /* output "830" sentences to control port */ - /* Multi-satellite mode */ + /* Multi-satellite mode */ /* * Output position information (to calculate fixed installation @@ -431,21 +462,6 @@ mx4200_config( add_mode = 1; /* add to list */ } - /* - * "007" Control Port Configuration - * Output "022" DOPs - */ - mx4200_send(peer, "%s,%03d,%03d,%d,%d,,%d,,,", pmvxg, - PMVXG_S_PORTCONF, - PMVXG_D_DOPS, /* control port output block Label */ - 0, /* clear current output control list (0=no) */ - add_mode, /* add/delete sentences from list (1=add, 2=del) */ - /* must be null */ - INTERVAL); /* sentence output rate (sec) */ - /* precision for position output */ - /* nmea version for cga & gll output */ - /* pass-through control */ - /* * "007" Control Port Configuration @@ -456,11 +472,13 @@ mx4200_config( PMVXG_D_PHV, /* control port output block Label */ 0, /* clear current output control list (0=no) */ add_mode, /* add/delete sentences from list (1=add, 2=del) */ - /* must be null */ + /* must be null */ INTERVAL); /* sentence output rate (sec) */ - /* precision for position output */ - /* nmea version for cga & gll output */ - /* pass-through control */ + /* precision for position output */ + /* nmea version for cga & gll output */ + /* pass-through control */ + + return (1); } /* @@ -490,21 +508,6 @@ mx4200_ref( /* * "007" Control Port Configuration - * Stop outputting "022" DOPs - */ - mx4200_send(peer, "%s,%03d,%03d,%d,%d,,,,,", pmvxg, - PMVXG_S_PORTCONF, - PMVXG_D_DOPS, /* control port output block Label */ - 0, /* clear current output control list (0=no) */ - 2); /* add/delete sentences from list (2=delete) */ - /* must be null */ - /* sentence output rate (sec) */ - /* precision for position output */ - /* nmea version for cga & gll output */ - /* pass-through control */ - - /* - * "007" Control Port Configuration * Stop outputting "021" position, height, velocity reports */ mx4200_send(peer, "%s,%03d,%03d,%d,%d,,,,,", pmvxg, @@ -590,7 +593,7 @@ mx4200_ref( lons, /* longitude DDDMM.MMMM */ ewc, /* east/west */ alt, /* Altitude */ - 1); /* Altitude Reference (0=WGS84 ellipsoid, 1=MSL geoid) */ + 1); /* Altitude Reference (0=WGS84 ellipsoid, 1=MSL geoid)*/ msyslog(LOG_DEBUG, "mx4200: reconfig to fixed location: %s %c, %s %c, %.2f m", @@ -766,10 +769,14 @@ mx4200_receive( sentence_type = strtol(cp, &cp, 10); /* - * "000" Status message + * Process the sentence according to its type. */ + switch (sentence_type) { - if (sentence_type == PMVXG_D_STATUS) { + /* + * "000" Status message + */ + case PMVXG_D_STATUS: /* * XXX * Since we configure the receiver to not give us status @@ -785,52 +792,35 @@ mx4200_receive( } mx4200_debug(peer, "mx4200_receive: reset receiver\n"); mx4200_config(peer); - return; - } + break; /* * "021" Position, Height, Velocity message, * if we are still averaging our position */ - if (sentence_type == PMVXG_D_PHV && !up->known) { - /* - * Parse the message, calculating our averaged position. - */ - if ((cp = mx4200_parse_p(peer)) != NULL) { - mx4200_debug(peer, "mx4200_receive: pos: %s\n", cp); - return; - } - mx4200_debug(peer, - "mx4200_receive: position avg %.9f %.9f %.4f\n", - up->avg_lat, up->avg_lon, up->avg_alt); - mx4200_debug(peer, - "mx4200_receive: position len %.4f %.4f %.4f\n", - up->filt_lat, up->filt_lon, up->filt_alt); - mx4200_debug(peer, - "mx4200_receive: position dop %.1f %.1f %.1f\n", - up->ndop, up->edop, up->vdop); - /* - * Reinitialize as a reference station - * if position is well known. - */ - if (current_time > up->clamp_time) { - up->known++; - mx4200_debug(peer, "mx4200_receive: reconfiguring!\n"); - mx4200_ref(peer); - } - return; - } - - /* - * "022" DOPs, if we are still averaging our position - */ - if (sentence_type == PMVXG_D_DOPS && !up->known) { - if ((cp = mx4200_parse_d(peer)) != NULL) { - mx4200_debug(peer, "mx4200_receive: dop: %s\n", cp); - return; + case PMVXG_D_PHV: + if (!up->known) { + /* + * Parse the message, calculating our averaged position. + */ + if ((cp = mx4200_parse_p(peer)) != NULL) { + mx4200_debug(peer, "mx4200_receive: pos: %s\n", cp); + return; + } + mx4200_debug(peer, + "mx4200_receive: position avg %f %.9f %.9f %.4f\n", + up->N_fixes, up->avg_lat, up->avg_lon, up->avg_alt); + /* + * Reinitialize as a reference station + * if position is well known. + */ + if (current_time > up->clamp_time) { + up->known++; + mx4200_debug(peer, "mx4200_receive: reconfiguring!\n"); + mx4200_ref(peer); + } } - return; - } + break; /* * Print to the syslog: @@ -838,21 +828,20 @@ mx4200_receive( * "030" Software Configuration * "523" Time Recovery Parameters Currently in Use */ - if (sentence_type == PMVXG_D_MODEDATA || - sentence_type == PMVXG_D_SOFTCONF || - sentence_type == PMVXG_D_TRECOVUSEAGE ) { + case PMVXG_D_MODEDATA: + case PMVXG_D_SOFTCONF: + case PMVXG_D_TRECOVUSEAGE: + if ((cp = mx4200_parse_s(peer)) != NULL) { mx4200_debug(peer, "mx4200_receive: multi-record: %s\n", cp); - return; } - return; - } + break; /* * "830" Time Recovery Results message */ - if (sentence_type == PMVXG_D_TRECOVOUT) { + case PMVXG_D_TRECOVOUT: /* * Capture the last PPS signal. @@ -911,12 +900,16 @@ mx4200_receive( * Turn off the flag and return */ up->polled = 0; - return; - } + break; /* * Ignore all other sentence types */ + default: + break; + + } /* switch (sentence_type) */ + return; } @@ -969,17 +962,19 @@ mx4200_parse_t( struct mx4200unit *up; char time_mark_valid, time_sync, op_mode; int sentence_type, valid; - int year, day_of_year, month, day_of_month, hour, minute, second, leapsec; + int year, day_of_year, month, day_of_month; + int hour, minute, second, leapsec; int oscillator_offset, time_mark_error, time_bias; pp = peer->procptr; up = (struct mx4200unit *)pp->unitptr; leapsec = 0; /* Not all receivers output leap second warnings (!) */ - sscanf(pp->a_lastcode, "$PMVXG,%d,%c,%d,%d,%d,%d:%d:%d,%c,%c,%d,%d,%d,%d", + sscanf(pp->a_lastcode, + "$PMVXG,%d,%c,%d,%d,%d,%d:%d:%d,%c,%c,%d,%d,%d,%d", &sentence_type, &time_mark_valid, &year, &month, &day_of_month, - &hour, &minute, &second, &time_sync, &op_mode, &oscillator_offset, - &time_mark_error, &time_bias, &leapsec); + &hour, &minute, &second, &time_sync, &op_mode, + &oscillator_offset, &time_mark_error, &time_bias, &leapsec); if (sentence_type != PMVXG_D_TRECOVOUT) return ("wrong rec-type"); @@ -1029,7 +1024,7 @@ mx4200_parse_t( * (Certainly can't be any year before this code was last altered!) */ if (day_of_month > 31 || month > 12 || - day_of_month < 1 || month < 1 || year < YEAR_RIGHT_NOW) { + day_of_month < 1 || month < 1 || year < YEAR_LAST_MODIFIED) { mx4200_debug(peer, "mx4200_parse_t: bad date (%4d-%02d-%02d)\n", year, month, day_of_month); @@ -1250,7 +1245,7 @@ mx4200_parse_p( struct refclockproc *pp; struct mx4200unit *up; int sentence_type, mode; - double mtime, lat, lon, alt, geoid, vele, veln, weight; + double mtime, lat, lon, alt, geoid, vele, veln; char north_south, east_west; pp = peer->procptr; @@ -1259,9 +1254,10 @@ mx4200_parse_p( /* Should never happen! */ if (up->moving) return ("mobile platform - no pos!"); - sscanf ( pp->a_lastcode, "$PMVXG,%d,%lf,%lf,%c,%lf,%c,%lf,%lf,%lf,%lf,%d", - &sentence_type, &mtime, &lat, &north_south, &lon, &east_west, &alt, - &geoid, &vele, &veln, &mode); + sscanf ( pp->a_lastcode, + "$PMVXG,%d,%lf,%lf,%c,%lf,%c,%lf,%lf,%lf,%lf,%d", + &sentence_type, &mtime, &lat, &north_south, &lon, &east_west, + &alt, &geoid, &vele, &veln, &mode); /* Sentence type */ if (sentence_type != PMVXG_D_PHV) @@ -1324,92 +1320,22 @@ mx4200_parse_p( if (lon > 180.0) lon -= 360.0; /* - * Calculate running weighted averages + * Calculate running averages */ - weight = 1. / up->edop; - weight *= weight; - up->avg_lon = (up->filt_lon * up->avg_lon) + (weight * lon); - up->filt_lon += weight; - up->avg_lon = up->avg_lon / up->filt_lon; - - weight = 1. / up->ndop; - weight *= weight; - up->avg_lat = (up->filt_lat * up->avg_lat) + (weight * lat); - up->filt_lat += weight; - up->avg_lat = up->avg_lat / up->filt_lat; - - weight = 1. / up->vdop; - weight *= weight; - up->avg_alt = (up->filt_alt * up->avg_alt) + (weight * alt); - up->filt_alt += weight; - up->avg_alt = up->avg_alt / up->filt_alt; - mx4200_debug(peer, - "mx4200_receive: position rdg %.9f %.9f %.4f (CM=%.9f)\n", - lat, lon, alt, up->central_meridian); + up->avg_lon = (up->N_fixes * up->avg_lon) + lon; + up->avg_lat = (up->N_fixes * up->avg_lat) + lat; + up->avg_alt = (up->N_fixes * up->avg_alt) + alt; - return (NULL); -} + up->N_fixes += 1.0; -/* - * Parse a mx4200 DOP sentence. - * - * A typical message looks like this. Checksum has already been stripped. - * - * $PMVXG,022,SSSSSS.SSEE.E,NN.N,VV.V,XX,XX,XX,XX,XX,XX - * - * Field Field Contents - * ----- -------------- - * Block Label: $PMVXG - * Sentence Type: 022=DOPs. The DOP values in this sentence - * correspond to the satellites listed. The PRNs in - * the message are listed in receiver channel number order - * 1 UTC measurement time (seconds into week) - * 2 EDOP (east DOP) - * 3 NDOP (north DOP) - * 4 VDOP (vertical DOP) - * 5 PRN on channel 1 - * 6 PRN on channel 2 - * 7 PRN on channel 3 - * 8 PRN on channel 4 - * 9 PRN on channel 5 - * 10 PRN on channel 6 - * 11 PRN on channel 7 (12-channel receivers only) - * 12 PRN on channel 8 (12-channel receivers only) - * 13 PRN on channel 9 (12-channel receivers only) - * 14 PRN on channel 10 (12-channel receivers only) - * 15 PRN on channel 11 (12-channel receivers only) - * 16 PRN on channel 12 (12-channel receivers only) - */ -static char * -mx4200_parse_d( - struct peer *peer - ) -{ - struct refclockproc *pp; - struct mx4200unit *up; - int sentence_type; - double mtime, edop, ndop, vdop; + up->avg_lon /= up->N_fixes; + up->avg_lat /= up->N_fixes; + up->avg_alt /= up->N_fixes; - pp = peer->procptr; - up = (struct mx4200unit *)pp->unitptr; - - /* Should never happen! */ - if (up->moving) return ("mobile platform - no dop!"); - - sscanf ( pp->a_lastcode, "$PMVXG,%d,%lf,%lf,%lf,%lf", - &sentence_type, &mtime, &edop, &ndop, &vdop); - - /* Sentence type */ - if (sentence_type != PMVXG_D_DOPS) - return ("wrong rec-type"); - - /* Update values */ - if (edop <= 0.0 || ndop <= 0.0 || vdop <= 0.0) - return ("nonpositive dop"); - up->edop = edop; - up->ndop = ndop; - up->vdop = vdop; + mx4200_debug(peer, + "mx4200_receive: position rdg %.0f: %.9f %.9f %.4f (CM=%.9f)\n", + up->N_fixes, lat, lon, alt, up->central_meridian); return (NULL); } @@ -1551,19 +1477,19 @@ mx4200_parse_s( case PMVXG_D_STATUS: msyslog(LOG_DEBUG, - "mx4200: status: %s", pp->a_lastcode); + "mx4200: status: %s", pp->a_lastcode); break; case PMVXG_D_MODEDATA: msyslog(LOG_DEBUG, - "mx4200: mode data: %s", pp->a_lastcode); + "mx4200: mode data: %s", pp->a_lastcode); break; case PMVXG_D_SOFTCONF: msyslog(LOG_DEBUG, - "mx4200: firmware configuration: %s", pp->a_lastcode); + "mx4200: firmware configuration: %s", pp->a_lastcode); break; case PMVXG_D_TRECOVUSEAGE: msyslog(LOG_DEBUG, - "mx4200: time recovery parms: %s", pp->a_lastcode); + "mx4200: time recovery parms: %s", pp->a_lastcode); break; default: return ("wrong rec-type"); @@ -1573,7 +1499,7 @@ mx4200_parse_s( } /* - * Process a PPS signal, returning a timestamp. + * Process a PPS signal, placing a timestamp in pp->lastrec. */ static int mx4200_pps( @@ -1584,13 +1510,7 @@ mx4200_pps( struct refclockproc *pp; struct mx4200unit *up; - int request; -#ifdef HAVE_CIOGETEV - request = CIOGETEV; -#endif -#ifdef HAVE_TIOCGPPSEV - request = TIOCGPPSEV; -#endif + struct timespec timeout; pp = peer->procptr; up = (struct mx4200unit *)pp->unitptr; @@ -1598,41 +1518,47 @@ mx4200_pps( /* * Grab the timestamp of the PPS signal. */ - temp_serial = up->ppsev.serial; - if (ioctl(fdpps, request, (caddr_t)&up->ppsev) < 0) { - /* XXX Actually, if this fails, we're pretty much screwed */ + temp_serial = up->pps_i.assert_sequence; + timeout.tv_sec = 0; + timeout.tv_nsec = 0; + if (time_pps_fetch(up->pps_h, PPS_TSFMT_TSPEC, &(up->pps_i), + &timeout) < 0) { mx4200_debug(peer, - "mx4200_pps: CIOGETEV/TIOCGPPSEV: serial=%d, fdpps=%d, %s\n", - up->ppsev.serial, fdpps, strerror(errno)); + "mx4200_pps: time_pps_fetch: serial=%d, %s\n", + up->pps_i.assert_sequence, strerror(errno)); refclock_report(peer, CEVNT_FAULT); return(1); } - if (temp_serial == up->ppsev.serial) { + if (temp_serial == up->pps_i.assert_sequence) { mx4200_debug(peer, - "mx4200_pps: ppsev serial not incrementing: %d\n", - up->ppsev.serial); + "mx4200_pps: assert_sequence serial not incrementing: %d\n", + up->pps_i.assert_sequence); refclock_report(peer, CEVNT_FAULT); return(1); } - /* * Check pps serial number against last one */ - if (up->lastserial + 1 != up->ppsev.serial && up->lastserial != 0) { - if (up->ppsev.serial == up->lastserial) + if (up->lastserial + 1 != up->pps_i.assert_sequence && + up->lastserial != 0) { + if (up->pps_i.assert_sequence == up->lastserial) { mx4200_debug(peer, "mx4200_pps: no new pps event\n"); - else + } else { mx4200_debug(peer, "mx4200_pps: missed %d pps events\n", - up->ppsev.serial - up->lastserial - 1); + up->pps_i.assert_sequence - up->lastserial - 1); + } refclock_report(peer, CEVNT_FAULT); } - up->lastserial = up->ppsev.serial; + up->lastserial = up->pps_i.assert_sequence; /* * Return the timestamp in pp->lastrec */ - up->ppsev.tv.tv_sec += (u_int32) JAN_1970; - TVTOTS(&up->ppsev.tv,&pp->lastrec); + + pp->lastrec.l_ui = up->pps_i.assert_timestamp.tv_sec + + (u_int32) JAN_1970; + pp->lastrec.l_uf = ((double)(up->pps_i.assert_timestamp.tv_nsec) * + 4.2949672960) + 0.5; return(0); } |
