// // NOTE: TABSTOP 4 // // *************************************************************************** // *************************************************************************** // *************************************************************************** // *** *** // *** Copyright 2000 by Jack Bates jack@floatingdoghead.net *** // *** *** // *** LEGAL DISCLAIMER AND RIGHTS POSTING: *** // *** *** // *** THIS IS A FREE TOOL OF HACK BY JACK. *** // *** *** // *** This program is free software; you can redistribute it and/or *** // *** modify it under the terms of the GNU General Public License *** // *** as published by the Free Software Foundation; either version 2 *** // *** of the License, or (at your option) any later version. *** // *** *** // *** This program is distributed in the hope that it will be useful, *** // *** but WITHOUT ANY WARRANTY; without even the implied warranty of *** // *** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *** // *** GNU General Public License for more details. *** // *** *** // *** You should have received a copy of the GNU General Public License *** // *** along with this program; if not, write to: *** // *** *** // *** Free Software Foundation, Inc. *** // *** 59 Temple Place - Suite 330 *** // *** Boston, MA 02111-1307, USA. *** // *** *** // *** THE FULL GPL LICENSE TEXT IS ALSO AVAILABLE AT http://www.fsf.org *** // *** *** // *** IN ADDITION: *** // *** *** // *** I ACCEPT NO LIABILITY WHATSOEVER IN REGARDS TO YOUR USE OF THIS *** // *** _SOURCE_CODE_. I DIDN'T WRITE THIS TO GET SUED, IT IS INTENDED *** // *** FOR EDUCATIONAL USE IN THE STUDY OF HOW THE INTERNET WORKS. *** // *** I DO NOT PROVIDE ANY FORM OF SUPPORT FOR THIS _SOURCE_CODE_. *** // *** NO QUESTIONS PLEASE - THE ANSWER IS IN THE SOURCE!!! *** // *** *** // *** IF YOU FEEL THE NEED TO INCORPORATE THIS CODE (OR DERIVATIVE) INTO *** // *** ANY PRODUCT THAT DOES NOT USE THE GPL AS ITS COPYRIGHT ENFORCEMENT *** // *** MECHANISM, YOU ARE OBLIGATED TO ARRANGE LICENSING WITH ME. *** // *** *** // *** MY TIME IS MONEY. THIS IS A FREE PUBLIC SERVICE. *** // *** *** // *** CAVEAT EMPTOR AND YOU GET WHAT YOU PAY FOR!!! *** // *** *** // *** WHILE THIS MAY SOUND GRUFF, I IMPLORE YOU TO _HAVE_A_NICE_DAY_!!!_ *** // *** *** // *************************************************************************** // *************************************************************************** // *************************************************************************** // // Version: 0.10 // // Date: 2000/06/04 22:24 PDT // // Tested under: Caldera OpenLinux 2.2 i386 2.2.12 kernel no X windows thin // standard ttyS0 serial hardware parameters // CSS Labs 486DX33 32MB // 16550A UART // // "thin" means no atd, crond, active ssh, tail -f, or other // "frequent waking" program. // // Do note that I have encountered some CPUs that are incapable // of getting accurate timings out of the serial port hardware. // This effect was verified using the same serial card on two // different CPUs. // // Compilation: Standard compile: gcc -Wall -o em_timed em_timed.c // // Define the following macros to produce interesting effects: // // #define JAB_SAMPLE // print info on every aggregated sample // // LOTS of output to stdout // #define JAB_SUMMARY // print info on every summary before // // calling adjdrift() // #define JAB_MSG // print info on every type 1000 msg // // TONS of output to stdout // #define JAB_DEBUG // other interesting information // #define JAB_NO_OUT // suppress LOCKED and ADJUST messages // // essentially silencing the program // // with the exception of hard error // // Please ignore the above ban on email and feel free to drop me a line if // you've successfully ported this to other hardware or software environments. // // *************************************************************************** // *************************************************************************** // *************************************************************************** // // Details of this program: // // You can connect a Delorme EarthMate GPS unit to a serial port of your // Linux machine and this program will sync your kernel clock to GPS time. // The Delorme EarthMate is an inexpensive GPS unit without a display. It // is intended to be connected directly to a computer via a 9-pin RS-232 // port. I have mounted mine on top of my house in a weatherproof // enclosure with a clear view of the sky. It very rarely loses lock. The // times reported from the unit are in units of nanoseconds. In my opinion // the EarthMate is a quality instrument. If it survives outdoors I will // be even more impressed. We'll see how that goes. // // Do note that this is a userspace program and does a fair amount // (though unnoticable from a CPU load perspective) of number crunching to // achieve reasonable results. As may be expected, this program works // significantly better when used on an unloaded system. // // On a totally unloaded, antiquated 486DX33 processor with a 16550A UART, // this program keeps the kernel clock oscillating about GPS time as // follows (note that Your Mileage May Vary-perhaps considerably): // // appx. 50% of the time within 1ms // appx. 83% of the time within 2ms // appx. 93% of the time within 3ms // appx. 98.7% of the time within 4ms // appx. 99.8% of the time within 5ms // > 99.95% of the time within 5.7ms // // No deltas >~5.7ms existed in the output which was more than 1800 // ADJUST lines long after attaining quiescent run state. // // These statistics are in regards to oscillations once the program has // been running for a while and managed to drift the clock into the // general neighborhood of GPS time and raised its slew interval into the // range of appx. 20,000 Sec. // // I believe that ntpd may be run in such a manner that it reports the // local system clock to clients. I intend further research in this area // and perhaps the release of a bootable floppy that is a self-contained // "thin" ntp server. More on this later. // // ntpd has a reference clock for the Rockwell Jupiter GPS chip. The // Delorme EarthMate is based upon the same chip, but is brain-dead, in // that it seemingly cannot be compelled operate in the various interesting // modes supported by the Jupiter (i.e. the GPS time-sync Pulse Per Second // mode). It seems to ignore everything sent to it other than the // initialization response of "EARTHA" (without the quotes), after // which it spews Rockwell binary data of just a few types. // // The type 1000 message contains time, gps position and velocity // readings and is sent at the rate appx. 1 per 1.000010 seconds. // Measurements in regards to the receipt of this type of message are what // drives the clock adjustment algorightm implemented here. // // Other Algorighmic Details: // // Summary: a collection of samples. // Sample: a collection of readings that are received by this userspace // program such that their timing has a high-degree of linear // rhythm. // // Upon collection of the first summary, a decision is made as to whether // to "hard set" the clock by the observed average delta for the first // summary. If the clock is off by more than the macro MAX_KERPLUNK // microseconds, the clock will be hard-set. // // The program collects "samples". A sample is defined as several type // 1000 messages received in a very linear rhythmic progression in time. // I.e. the jitter in receipt times to this userspace program must be less // than ten microseconds betweeen two messages for them to be considered // part of a sample. The macros MIN_SAMPLE and MAX_SAMPLE control the // minimum and maximum number of messages to be aggregated into these // samples. The timing statistics collected at receipt of these messages // are averaged. // // The enforcement of linear rhythm in the collection of samples tends to // highly effectively throw out the ugly, unusable data that is collected // during periods of system load, as the timing of the messages hitting // this userspace program becomes influenced to a large degree by the // process scheduling of the kernel. This influence is clearly observable // when looking at the timing of individual type 1000 messages by compiling // with the macro JAB_MSG defined. // // Many samples are rolled into a summary. The length of a summary in // samples grows over time, but is bounded by the macro MAX_SUMMARY. The // slope of the line that passes through all of the samples is calculated // over the summary via a simple linear regression. The average delta and // the average time that the delta was observed is also calculated (this // must be calculated to do the linear regression). // // Those statistics are fed into adjdrift() which attempts to intelligently // interpret the data in regards to a small amount of historical state that // it maintains between calls. adjdrift() makes the adjtimex() call after // cooking the data a bit. The recipe for cooking the data could perhaps // be improved upon to yield better results. Do note that I've done quite // a bit of tweaking in the KISS (Keep It Simple Stupid) department to // effect this. None of my tweaks has successfully dampened the // oscillations about GPS time better than this very simple version. Most // of them made it far worse by presuming that the the slope calculated by // a linear regression is highly error prone (which it is) and should be // further diminished in influence. This only prolonged the effect of of // overshooting the zero mark, causing much higher amplitudes in // oscillation. // // The object was to keep the code readable by even beginner C programmers // while providing lock within a few mS. For all intents and purposes, // I consider this synchronized. // // NOTE: The call to adjdrift() uses the data averaged over a summary // to specify the clock offset. This offset is certainly old news. // If the clock is drifting quickly, the offset at the time that // ajdrift() is called may be quite different from the actual // calculated offset provided to the function. // // NOTE: There is a mod that _may_ significantly increase the // accuracy of the linear regression used to determine the slope // present in a summary of data. I believe that removing outlying // points (particularly those with latencies that are HIGH) and // then recalculating a regression on the remaining data will // produce more consistent results. I have not verified this by // experimentation, however. // // NOTE: This program uses setpriority() and adjtimex(). Both of these // system calls are used in a manner in which the program must // have root privilege to set kernel process parameters. // // NOTE: There is no attempt to adjust for the latency encountered due // to the fact that transmitting a byte over a serial line and // processing it through the various Unix software layers takes // time. The data is obviously a bit stale before it is consumed // by this program. // // NOTE: There is no attempt to adjust for the latency encountered due // to the fact that the 16550 UART chip may buffer several // characters before interrupting the OS to inform of received data. // // NOTE: The baud rate that the EarthMate uses is 9600, mandating the // use of a 16550 UART, at least on slower processors. I have not // experimented with 16450 UARTs on faster CPUs. // // NOTE: You will notice references to kernel timestamping in the code. // This feature is not yet implemented. My intent is to capture // a timestamp in the kernel ISR for every 0xFF character received. // 0xFF is the first character in any message of the Rockwell binary // protocol. Any message-embedded 0xFF would also generate a // timestamp (to keep the kernel patch absolutely to a minimum), so // this program will need to be enhanced to deal with that. // // NOTE: The Earthmate may be successfully powered by the regulated // +5v supply that is on virtually every power connector in a PC // computer. I fashioned a cable that runs serial and power on one // 6-conductor piece of phone cable. The ground and +5v were tapped // from a disk drive power connector and connected to the battery // contacts of the receiver. For the serial pinouts, wiring hints // and a PDF version of the Rockwell binary format specification, I // would direct you to: // // http://www.c2-tech.com/~steveb/ka9mva/earthmate.htm // // BUG: Do note that the slew interval calculated during the second call // to adjdrift() (first non-zero delta, but I don't think that should // matter) is wrong. I am seeing a ~7 hour offset being stored into // last_time_observed during the first execution of adjdrift(). 7 // hours just happens to be the UTC offset this time of year here. I // am currently at a loss to explain this "feature". On the third // call, things magically clear up... // // *************************************************************************** // *************************************************************************** // *************************************************************************** #include #include #include #include #include #include #include #include #include #include #include #include #include // missing definitions for glibc on stock Caldera OpenLinux 2.2 extern long long llabs(long long); // minimum and maximum lengths of a sample. #define MIN_SAMPLE (5) #define MAX_SAMPLE (20) // maximum number of microseconds of delta to allow without "hard setting" // the system clock by that delta upon collection of the first summary. #define MAX_KERPLUNK (1000000LL) // this controls the length in samples of the first summary, the max summary // and the increment in the size of summaries between summaries. a summary has // a linear regression applied to determine slope. other elements of inter- // summary state are applied to effect a hysteresis effect on the oscillatory // nature of how this program controls the kernel clock. #define MAX_SUMMARY (40) #define STARTING_SUMMARY (10) #define SUMMARY_INCREMENT (2) // maximum drift between the timings of consecutively-received // type 1000 messages to allow their inclusion in a sample. #define DRIFT_THRESHOLD (10LL) // number of calls to adjdrift() before limiting the slope by delta state #define INITIAL_SEEK_CALLS (3) // initial/minimum, maximum and increment to the multiplying factor for // determination of slew interval #define L_INITIAL_SLEW_MULTIPLIER (10LL) #define L_MAX_SLEW_MULTIPLIER (20LL) #define L_SLEW_MULTIPLIER_INCREMENT (2LL) // if delta is less than this in usec, increase the slew interval multiplier #define MAX_DELTA_SLEW_INCREMENT (1500LL) // else if delta is greater than this in usec, divide slew multiplier by two #define MAX_DELTA_SLEW_HALVE (5000LL) // else if delta is greater than this, decrease the slew interval multplier #define MIN_DELTA_SLEW_DECREMENT (2500LL) // whether to expect a timestamp for every 0xFF received on the serial port. // this will become a command-line parameter to be used in conjunction with // an altered linux/drivers/char/serial.c. int kernel_timestamp = 0; typedef struct { long long time_observed; long long delta; int weight; } history_t; int open_serial(char *filename) { struct termios ti; int fd; int flags = 0; // open w/ O_NDELAY to avoid hardware flow control blocking the open // sounds weird, but it can happen... (but not with the EarthMate) fd = open(filename, O_RDWR | O_NDELAY); if (fd < 0) { fprintf(stderr, "%s: open errno %d", filename, errno); exit(1); } // get flags if ((flags = fcntl(fd, F_GETFL, 0)) == -1) { fprintf(stderr, "%s: get flags errno %d", filename, errno); close(fd); exit(1); } // return us to the state of blocking being allowed flags &= ~O_NDELAY; // set flags if (fcntl(fd, F_SETFL, flags) == -1) { fprintf(stderr, "%s: set flags errno %d", filename, errno); close(fd); exit(1); } if (ioctl(fd, TCGETS, &ti) != 0) { fprintf(stderr, "%d: TCGETS errno %d", fd, errno); close(fd); exit(1); } // this is to turn on the kernel timestamp for every 0xFF received // see the replacement for linux/drivers/char/serial.c included with this distribution if (kernel_timestamp) ti.c_iflag = 0100000; else ti.c_iflag = 0; ti.c_oflag = 0; ti.c_lflag = 0; ti.c_cflag &= (~CBAUD); ti.c_cflag |= B9600; ti.c_cflag &= (~CSIZE); ti.c_cflag |= CS8; ti.c_cflag &= ~PARENB; ti.c_cflag &= ~PARODD; ti.c_cflag |= HUPCL; ti.c_cc[VMIN] = 1; ti.c_cc[VTIME] = 0; if (ioctl(fd, TCSETS, &ti) != 0) { fprintf(stderr, "%d: TCSETS errno %d", fd, errno); close(fd); exit(1); } return fd; } void printchar(char c) { if (isprint(c)) { fprintf(stdout, "%c", c & 0xFF); } else { fprintf(stdout, "<%02X>", (int) c & 0xFF); if (c == 0x0a) fprintf(stdout, "\n"); } fflush(stdout); } // attempt to match some input // returns 0 on success, -1 on failure int match(int fd, char *str) { char *cursor; char c; //fprintf(stdout, "match: %s\n", str); cursor = str; while (read(fd, &c, 1) > 0) { //printchar(c); if (c == *cursor) { //fprintf(stdout, "match: matched "); //printchar(*cursor); cursor++; //fprintf(stdout, " next "); //printchar(*cursor); //fprintf(stdout, "\n"); if (*cursor == '\0') { //fprintf(stdout, "match: matched %s\n", str); //fflush(stdout); return 0; } } else { cursor = str; } } return -1; } unsigned short em_checksum(unsigned short *w, int n) { unsigned short csum = 0; while (n--) csum += *(w++); csum = -csum; return csum; } #define EARTHA "EARTHA\r\n\0" // initialize the GPS unit so that it starts spewing Rockwell binary messages int eartha(int fd) { alarm(30); if (match(fd, EARTHA) == -1) { alarm(0); fprintf(stderr, "EARTHA not found\n"); exit(1); } alarm(0); //fprintf(stdout, "eartha: echoing init string\n"); //fflush(stdout); write(fd, EARTHA, strlen(EARTHA)); //fprintf(stdout, "eartha: returning\n"); //fflush(stdout); return 0; } int read_chars(int fd, unsigned char *buf, int n) { int rc, count; unsigned char *cursor; //fprintf(stdout, "read_chars get %d\n", n); //fflush(stdout); count = 0; while (count < n) { rc = read(fd, buf + count, n - count); if (rc == -1) return -1; for (cursor = buf + count; cursor < buf + count + rc; cursor++); // printchar(*cursor); count += rc; } return n; } char lcsign(long l) { if (l > 0) return '+'; if (l < 0) return '-'; return ' '; } char llcsign(long long ll) { if (ll > 0) return '+'; if (ll < 0) return '-'; return ' '; } char fcsign(double f) { if (f > 0.0000000000001) return '+'; if (f < -0.0000000000001) return '-'; return ' '; } void legend() { #ifdef JAB_MSG fprintf(stdout, "\nns kt ktdelta gt gtdelta kt - gt drift driftdelta\n"); #endif } // if tick is way out of whack, adjust it to be sane with defaults. // if it's not out of whack, just leave it alone. void timex_sanity() { struct timex stmx; stmx.modes = 0; adjtimex(&stmx); if (9900 < stmx.tick && stmx.tick < 10100) { #ifdef JAB_DEBUG fprintf(stdout, "TICKINIT sane tick %ld freq %ld\n", stmx.tick, stmx.freq); #endif return; } #ifdef JAB_DEBUG fprintf(stdout, "TICKINIT insane tick %ld freq %ld - set to default\n", stmx.tick, stmx.freq); #endif stmx.tick = 10000; stmx.freq = 0; stmx.modes = ADJ_FREQUENCY | ADJ_TICK; adjtimex(&stmx); } #define L_FREQ_PER_TICK_UNIT (6553600L) #define LL_FREQ_PER_TICK_UNIT (6553600LL) #define LL_ONE_MILLION (1000000LL) #define LL_ONE_BILLION (1000000000LL) #define LL_FREQADJ_LIMIT LL_ONE_BILLION #define D_FREQ_PER_PPM (65536.0) #define D_ONE_MILLION (1000000.0) #define D_PPM_SCALING_FACTOR ( ((double) LL_FREQ_PER_TICK_UNIT) / 100.0 ) long long gettime64() { long long rv; struct timeval tv; gettimeofday(&tv, NULL); rv = ((long long) tv.tv_sec) * LL_ONE_MILLION; rv += ((long long) tv.tv_usec); return rv; } void adjdrift(double slope, long long delta, long long time_observed) { static long long call_counter = 0; static long slew_multiplier = L_INITIAL_SLEW_MULTIPLIER; static double last_fa_delta_ppm = 0.0; static long long last_time_observed = 0; double fa_slope_ppm = 0.0; double fa_delta_ppm = 0.0; double adjusted_slope_ppm = 0.0; long long adjusted_time_observed = 0LL; long long adjusted_delta = 0LL; long slew_interval = 0L; long tick; long freq; long long freqadj; long tickadj; int rc; struct timex stmx; #ifdef JAB_DEBUG fprintf(stdout, "STATE call_counter %lld slew_multiplier %ld last_fa_delta_ppm %.6f last_time_observed %lld\n", call_counter, slew_multiplier, last_fa_delta_ppm, last_time_observed); #endif // first time called? if (call_counter == 0) { last_time_observed = gettime64(); // we will make no change for delta on the first call adjusted_delta = 0; } else { adjusted_delta = delta; // slew multiplier adjustments // this is an attempt at tightening the adjustments when we've been // close for a while and to effect a quick response to wildly out-of // bounds conditions... if (llabs(delta) < MAX_DELTA_SLEW_INCREMENT) slew_multiplier += L_SLEW_MULTIPLIER_INCREMENT; else if (llabs(delta) > MAX_DELTA_SLEW_HALVE) slew_multiplier /= 2; else if (llabs(delta) > MIN_DELTA_SLEW_DECREMENT) slew_multiplier -= L_SLEW_MULTIPLIER_INCREMENT; // bounding if (slew_multiplier > L_MAX_SLEW_MULTIPLIER) slew_multiplier = L_MAX_SLEW_MULTIPLIER; if (slew_multiplier < L_INITIAL_SLEW_MULTIPLIER) slew_multiplier = L_INITIAL_SLEW_MULTIPLIER; } // get kernel clock parameters stmx.modes = 0; adjtimex(&stmx); tick = stmx.tick; freq = stmx.freq; // calculate ppm change due to slope fa_slope_ppm = -slope * D_ONE_MILLION; // this number may be limited by delta-based state adjusted_slope_ppm = fa_slope_ppm; if (time_observed != 0L) { adjusted_time_observed = time_observed; slew_interval = ((long) ((adjusted_time_observed - last_time_observed) / LL_ONE_MILLION)) * slew_multiplier; #ifdef JAB_DEBUG fprintf(stdout, "SLEWCALC adjusted_time_observed: %lld.%06lld last_time_observed: %lld.%06lld\n", adjusted_time_observed / LL_ONE_MILLION, adjusted_time_observed % LL_ONE_MILLION, last_time_observed / LL_ONE_MILLION, last_time_observed % LL_ONE_MILLION); #endif } else { slew_interval = 0; // UGLY!!! adjusted_time_observed = gettime64(); #ifdef JAB_DEBUG fprintf(stdout, "VIRGIN set adjusted_time_observed to now: %lld.%06lld\n", adjusted_time_observed / LL_ONE_MILLION, adjusted_time_observed % LL_ONE_MILLION); #endif } // slew-interval is the the half-life of the current delta on the "curve" of convergence if (slew_interval != 0L) { fa_delta_ppm = -(((double) delta) / ((double) slew_interval)); } // don't limit slope adjustment until we've been called a few times if (call_counter >= INITIAL_SEEK_CALLS) { // limit is the sum of the current delta and the last delta adjusted_slope_ppm = (fabs(fa_delta_ppm) + fabs(last_fa_delta_ppm)); if (fa_slope_ppm < 0) adjusted_slope_ppm = -adjusted_slope_ppm; if (fabs(fa_slope_ppm) <= fabs(adjusted_slope_ppm)) adjusted_slope_ppm = fa_slope_ppm; } // adjustment to frequency freqadj = (long long) (fa_delta_ppm * D_PPM_SCALING_FACTOR); freqadj += (long long) (adjusted_slope_ppm * D_PPM_SCALING_FACTOR); // sanity test... if (freqadj > LL_FREQADJ_LIMIT || freqadj < -LL_FREQADJ_LIMIT) { long long adjusted_freqadj; if (freqadj < 0) adjusted_freqadj = -LL_FREQADJ_LIMIT; if (freqadj > 0) adjusted_freqadj = LL_FREQADJ_LIMIT; #ifdef JAB_DEBUG fprintf(stdout, "LIMIT freqadj %c%lld -> %c%lld\n", llcsign(freqadj), llabs(freqadj), llcsign(adjusted_freqadj), llabs(adjusted_freqadj)); #endif freqadj = adjusted_freqadj; } // divide the frequency adjustment into tick units tickadj = (long) (freqadj / LL_FREQ_PER_TICK_UNIT); freqadj %= LL_FREQ_PER_TICK_UNIT; // adjust stmx.tick += tickadj; stmx.freq += (long) freqadj; // bound freq range while (stmx.freq > L_FREQ_PER_TICK_UNIT) { stmx.tick++; stmx.freq -= L_FREQ_PER_TICK_UNIT; } while (stmx.freq < -L_FREQ_PER_TICK_UNIT) { stmx.tick--; stmx.freq += L_FREQ_PER_TICK_UNIT; } // make the adjustment stmx.modes = ADJ_FREQUENCY | ADJ_TICK; rc = adjtimex(&stmx); #ifndef JAB_NO_OUT fprintf(stdout, "ADJUST " "%06lld " "delta %c%lld.%06lld " "slmult %03ld slint %05ld " "fadlt %c%.6f " "faslp %c%.6f " "%c " "rfaslp %c%.6f " "ktick %05ld -> %05ld " "kfreq %c%07ld -> %c%07ld " "rc %d\n", call_counter, llcsign(delta), llabs(delta / LL_ONE_MILLION), llabs(delta % LL_ONE_MILLION), slew_multiplier, slew_interval, fcsign(fa_delta_ppm), fabs(fa_delta_ppm), fcsign(adjusted_slope_ppm), fabs(adjusted_slope_ppm), // yes, they teach you never to do this in school, // but it was just assigned above... (adjusted_slope_ppm == fa_slope_ppm) ? ' ' : '*', fcsign(fa_slope_ppm), fabs(fa_slope_ppm), tick, stmx.tick, lcsign(freq), labs(freq), lcsign(stmx.freq), labs(stmx.freq), rc); fflush(stdout); #endif last_fa_delta_ppm = fa_delta_ppm; last_time_observed = adjusted_time_observed; call_counter++; } long long set_time_by_delta(long long delta64) { struct timeval stv; struct timezone stz; long long optime64; long long now64; // this fancy thing does a lame attempt at trying to calculate // the amount of time required to do the math and set the clock stz.tz_minuteswest = 0; stz.tz_dsttime = 0; now64 = gettime64() - delta64; stv.tv_sec = (long) (now64 / LL_ONE_MILLION); stv.tv_usec = (long) (now64 % LL_ONE_MILLION); settimeofday(&stv, &stz); gettimeofday(&stv, NULL); optime64 = ((long long) stv.tv_sec) * LL_ONE_MILLION; optime64 += stv.tv_usec; optime64 -= now64; now64 = gettime64() + optime64; stv.tv_sec = (long) (now64 / LL_ONE_MILLION); stv.tv_usec = (long) (now64 % LL_ONE_MILLION); settimeofday(&stv, NULL); #ifdef JAB_DEBUG fprintf(stdout, "KERPLUNK time was set by delta to %ld.%06ld\n", stv.tv_sec, stv.tv_usec); fflush(stdout); #endif return now64; } void handle_summary(history_t *summary, int summary_len) { // inter-summary state static long long last_sum_time = 0; static long long last_sum_delta = 0; static long long summaries_given = 0; // statistics long long min_time_observed, max_time_observed; long long min_delta, max_delta; long long sum_time_observed, sum_delta; long sum_weight; long long Sxx, Sxy, accum; double slope; int i; sum_time_observed = 0; sum_delta = 0; sum_weight = 0; min_time_observed = summary[0].time_observed; max_time_observed = summary[0].time_observed; min_delta = summary[0].delta; max_delta = summary[0].delta; for (i = 0; i < summary_len; i++) { sum_time_observed += summary[i].time_observed; sum_delta += summary[i].delta; sum_weight += summary[i].weight; if (summary[i].time_observed < min_time_observed) min_time_observed = summary[i].time_observed; if (summary[i].time_observed > max_time_observed) max_time_observed = summary[i].time_observed; if (summary[i].delta < min_delta) min_delta = summary[i].delta; if (summary[i].delta > max_delta) max_delta = summary[i].delta; } sum_time_observed /= summary_len; sum_delta /= summary_len; // calculate linear regression variables Sxx = 0; Sxy = 0; for (i = 0; i < summary_len; i++) { accum = (sum_time_observed - summary[i].time_observed); accum *= accum; Sxx += accum; accum = (sum_delta - summary[i].delta); accum *= (sum_time_observed - summary[i].time_observed); Sxy += accum; } slope = (double) (((double) Sxy) / ((double) Sxx)); #ifdef JAB_SUMMARY fprintf(stdout, "SUMMARY %06lld N %05ld " "time %c%ld.%06ld " "%c%ld.%06ld " "%c%ld.%06ld " "delta %c%ld.%06ld " "%c%ld.%06ld " "%c%ld.%06ld " "slope %c%.6f\n", summaries_given, sum_weight, llcsign(min_time_observed), labs((long) (min_time_observed / LL_ONE_MILLION)), labs((long) (min_time_observed % LL_ONE_MILLION)), llcsign(sum_time_observed), labs((long) (sum_time_observed / LL_ONE_MILLION)), labs((long) (sum_time_observed % LL_ONE_MILLION)), llcsign(max_time_observed), labs((long) (max_time_observed / LL_ONE_MILLION)), labs((long) (max_time_observed % LL_ONE_MILLION)), llcsign(min_delta), labs((long) (min_delta / LL_ONE_MILLION)), labs((long) (min_delta % LL_ONE_MILLION)), llcsign(sum_delta), labs((long) (sum_delta / LL_ONE_MILLION)), labs((long) (sum_delta % LL_ONE_MILLION)), llcsign(max_delta), labs((long) (max_delta / LL_ONE_MILLION)), labs((long) (max_delta % LL_ONE_MILLION)), fcsign(slope), fabs(slope) * D_ONE_MILLION); fflush(stdout); #endif if (summaries_given == 0) { // note this adjust has no delta component... // we begin delta adjustments on the next summary adjdrift(slope, 0LL, 0LL); // only do a KERPLUNK if we're more than a second out of whack if (llabs(sum_delta) > MAX_KERPLUNK) { set_time_by_delta(sum_delta); sum_delta = 0; sum_time_observed = gettime64(); } } else { adjdrift(slope, sum_delta, sum_time_observed); } last_sum_time = sum_time_observed; last_sum_delta = sum_delta; summaries_given++; } #define reset_sample() sample_count = 0 #define reset_summary() summary_count = 0 #define LEGEND_INTERVAL (20) // handle the Rockwell 1000 message // contains time, gps position, velocity vectors, etc // we only care about the time void handle1000(struct timeval *tv, unsigned short *msg) { // inter-sample state static long long lastdelta = 0; static long long lastdrift = 0; static long long lastgtime = 0; static long long lastktime = 0; static int summary_count = 0; static int legend_count = LEGEND_INTERVAL; static int sample_count = 0; static int locked = 1; // the current sample static history_t sample[MAX_SAMPLE]; // the current summary static history_t summary[MAX_SUMMARY]; // number of samples currently in a summary static int summary_len = STARTING_SUMMARY; struct tm stm; int i; // observations for this sample long long ktime; long long gtime; long gsecs; long gusecs; long utcoffset; // printable statistics for this sample long long delta; long long drift; long long driftdelta; long long gtimedelta; long long ktimedelta; // statistics long long min_time_observed, max_time_observed; long long min_delta, max_delta; long long sum_time_observed, sum_delta; // is the data valid? if ((msg[4] & 0x0004) != 0) { // if we lose lock, throw away history reset_sample(); reset_summary(); if (locked == 1) { // transitioned to not locked #ifndef JAB_NO_MSG fprintf(stdout, "UNLOCKED %ld.%06ld %02u", tv->tv_sec, tv->tv_usec, msg[6]); fflush(stdout); #endif locked = 0; } else { // when not locked, drop a period a second to stdout #ifndef JAB_NO_MSG fprintf(stdout, "."); fflush(stdout); #endif } return; } else if (locked == 0) { // transitioned to locked #ifndef JAB_NO_MSG fprintf(stdout, "\nLOCKED %ld.%06ld %02u\n", tv->tv_sec, tv->tv_usec, msg[6]); fflush(stdout); #endif locked = 1; } // parse Rockwell 1000 message into a struct tm for conversion to Unix-style Julian date stm.tm_mday = msg[13]; stm.tm_mon = msg[14] - 1; stm.tm_year = msg[15] - 1900; stm.tm_hour = msg[16]; stm.tm_min = msg[17]; stm.tm_sec = msg[18]; stm.tm_gmtoff = 0; gsecs = mktime(&stm); gusecs = (*((long *) &msg[19])) / 1000; legend_count++; if (legend_count > LEGEND_INTERVAL) { legend_count = 0; legend(); } gtime = ((long long) gsecs) * LL_ONE_MILLION + ((long long) gusecs); ktime = ((long long) tv->tv_sec) * LL_ONE_MILLION + ((long long) tv->tv_usec); utcoffset = localtime(&gsecs)->tm_gmtoff; delta = ktime - gtime - (((long long) utcoffset) * LL_ONE_MILLION); drift = delta - lastdelta; driftdelta = drift - lastdrift; gtimedelta = gtime - lastgtime; ktimedelta = ktime - lastktime; #ifdef JAB_MSG if (lastgtime != 0) { fprintf(stdout, "%02u " "%ld.%06ld " "%c%ld.%06ld " "%ld.%06ld " "%c%ld.%06ld " "%c%ld.%06ld " "%c%ld.%06ld " "%c%ld.%06ld", msg[6], tv->tv_sec, tv->tv_usec, llcsign(ktimedelta), ((long) (ktimedelta / LL_ONE_MILLION)), labs((long) (ktimedelta % LL_ONE_MILLION)), gsecs, gusecs, llcsign(gtimedelta), ((long) (gtimedelta / LL_ONE_MILLION)), labs((long) (gtimedelta % LL_ONE_MILLION)), llcsign(delta), labs(((long) (delta / LL_ONE_MILLION))), labs((long) (delta % LL_ONE_MILLION)), llcsign(drift), labs(((long) (drift / LL_ONE_MILLION))), labs((long) (drift % LL_ONE_MILLION)), llcsign(driftdelta), labs(((long) (driftdelta / LL_ONE_MILLION))), labs((long) (driftdelta % LL_ONE_MILLION))); } #endif sample[sample_count].time_observed = gtime; sample[sample_count].delta = delta; sample_count++; if (-DRIFT_THRESHOLD < driftdelta && driftdelta < DRIFT_THRESHOLD && sample_count <= MAX_SAMPLE) { #ifdef JAB_MSG fprintf(stdout, " *\n"); fflush(stdout); #endif } else { #ifdef JAB_MSG fprintf(stdout, "\n"); fflush(stdout); #endif sample_count--; if (sample_count >= MIN_SAMPLE) { sum_time_observed = 0; sum_delta = 0; min_time_observed = sample[0].time_observed; max_time_observed = sample[0].time_observed; min_delta = sample[0].delta; max_delta = sample[0].delta; for (i = 0; i < sample_count; i++) { sum_delta += sample[i].delta; sum_time_observed += sample[i].time_observed; if (sample[i].delta < min_delta) min_delta = sample[i].delta; if (sample[i].delta > max_delta) max_delta = sample[i].delta; if (sample[i].time_observed < min_time_observed) min_time_observed = sample[i].time_observed; if (sample[i].time_observed > max_time_observed) max_time_observed = sample[i].time_observed; } sum_delta /= sample_count; sum_time_observed /= sample_count; #ifdef JAB_SAMPLE fprintf(stdout, "SAMPLE %06d N %05d time %c%ld.%06ld %c%ld.%06ld %c%ld.%06ld " "delta %c%ld.%06ld %c%ld.%06ld %c%ld.%06ld\n", summary_count, sample_count, llcsign(min_time_observed), labs((long) (min_time_observed / LL_ONE_MILLION)), labs((long) (min_time_observed % LL_ONE_MILLION)), llcsign(sum_time_observed), labs((long) (sum_time_observed / LL_ONE_MILLION)), labs((long) (sum_time_observed % LL_ONE_MILLION)), llcsign(max_time_observed), labs((long) (max_time_observed / LL_ONE_MILLION)), labs((long) (max_time_observed % LL_ONE_MILLION)), llcsign(min_delta), labs((long) (min_delta / LL_ONE_MILLION)), labs((long) (min_delta % LL_ONE_MILLION)), llcsign(sum_delta), labs((long) (sum_delta / LL_ONE_MILLION)), labs((long) (sum_delta % LL_ONE_MILLION)), llcsign(max_delta), labs((long) (max_delta / LL_ONE_MILLION)), labs((long) (max_delta % LL_ONE_MILLION))); fflush(stdout); #endif summary[summary_count].delta = sum_delta; summary[summary_count].time_observed = sum_time_observed; summary[summary_count].weight = sample_count; summary_count++; if (summary_count >= summary_len) { handle_summary(summary, summary_count); reset_summary(); summary_len += SUMMARY_INCREMENT; if (summary_len >= MAX_SUMMARY) summary_len = MAX_SUMMARY; } legend(); legend_count = 0; } reset_sample(); } fflush(stdout); lastktime = ktime; lastgtime = gtime; lastdrift = drift; lastdelta = delta; } struct rockwell_header { unsigned short sync; unsigned short id; unsigned short ndata; unsigned short flags; unsigned short csum; }; #define MAX_BUF (1024) void rockwell_framer(int fd) { unsigned char c; int bytes; struct rockwell_header hdr; struct timeval tv; unsigned short buf[MAX_BUF]; unsigned short cksum; int rc; while (1) { while (1) { rc = read(fd, &c, 1); gettimeofday(&tv, NULL); if (rc != 1) break; // 0xFF is always the first byte of a header if (c == 0xFF) break; } if (rc != 1 || read(fd, &c, 1) == -1) { fprintf(stderr, "read returns unhappy\n"); exit(1); } // 0x81 is always the second byte of a header if (c != 0x81) continue; // we are now sync'd up hdr.sync = 0x81FF; // get the rest of the header if (read_chars(fd, ((char *) (&hdr)) + 2, 8) != 8) exit(1); cksum = em_checksum((unsigned short *) &hdr, 4); if (cksum != hdr.csum) { fprintf(stdout, "header checksum failure %04X != %04X\n", cksum, hdr.csum); fflush(stdout); continue; } // read the rest of the message bytes = hdr.ndata * sizeof(unsigned short); if (read_chars(fd, (char *) buf, bytes) != bytes) exit(1); if (read_chars(fd, (char *) &cksum, sizeof(unsigned short)) != sizeof(unsigned short)) exit(1); if (cksum != em_checksum(buf, hdr.ndata)) { fprintf(stdout, "data checksum error\n"); fflush(stdout); continue; } if (hdr.id == 1000) handle1000(&tv, buf); } } int main(int argc, char *argv[]) { int fd; if (argc != 2) { fprintf(stderr, "usage: %s dev\n", argv[0]); exit(1); } // close stdin - we don't need it close(0); // act like the daemon we are and auto-background if (fork() != 0) exit(0); // disconnect from controlling tty fd = open("/dev/tty", O_RDWR|O_NOCTTY); if (fd >= 0) { ioctl(fd, TIOCNOTTY, NULL); close(fd); } setpgrp(); // default the kernel timex in case of INSANITY timex_sanity(); // open serial port fd = open_serial(argv[1]); // handle EarthMate initialization eartha(fd); // set to very high priority setpriority(PRIO_PROCESS, getpid(), -20); // handle EarthMate data stream rockwell_framer(fd); // should never get here close(fd); return 0; }