ntp-shm

#!/usr/bin/perl

# part of this code is from: https://github.com/doug1/psip-time/blob/master/psip-time.pl
# This module is free software. You can redistribute it and/or
# modify it under the terms of the Artistic License 2.0.
#
# 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.
#
# example /etc/ntp.conf:
# server 127.127.28.0 minpoll 4 maxpoll 4
# fudge 127.127.28.0 time1 0.000 refid GPS stratum 0
#
# to run this program:
# ./ntp-shm </dev/ttyACM0
# Adjust /dev/ttyACM0 to match your device (see dmesg). Needs to run as root (shm permissions)

use strict;
use Time::HiRes qw(gettimeofday tv_interval usleep);
use IPC::SysV qw(IPC_PRIVATE IPC_RMID S_IRUSR S_IWUSR);
use IO::Handle;
use FindBin;
use lib "$FindBin::Bin/perl-lib";
use USBGPS::Filter::Percentile;
use USBGPS::SerialData;
use USBGPS::ClockSample::PPS;
use USBGPS::ClockSample::MidSecond;
use Inline "C";
use constant {
  NTP_SHMID => 0x4e545030
};

my $st_interval_estimate = 10500000; # frequency of the timer on the microcontroller

my $last_pps_obj;
sub parseline {
  my($filter, $SerialData, $shmid, $line, $now, $interval) = @_;

  if($line =~ /^G [\d,]+/) {
    $SerialData->parse($now,$line,$interval);
    return;
  }

  if($interval > 1.003 or $interval < 0.997) {
    print "skip (local timer=$interval s)\n";
    return; # skip un-even pulses
  }

  if(not $SerialData->state->lock) {
    print "skip (no gps lock)\n";
    return;
  }

  if($line =~ /^P (\d+) (\d+) (\d+) (\d+) (\d+)/) {
    my($time_pps, $time_now, $st_interval, $usb_time, $usb_polls) = ($1,$2,$3,$4,$5);

    if($st_interval > 10501000 or $st_interval < 10499999) {
      print "skip (remote timer=$st_interval HZ)\n";
      return; # skip PPS pulses that happen outside of a 500ppm window
    }
    $st_interval_estimate = $st_interval_estimate * 0.8 + $st_interval * 0.2;

    $last_pps_obj = USBGPS::ClockSample::PPS->new(
        time_pps => $time_pps,
        time_now => $time_now,
        st_interval => $st_interval,
        st_interval_estimate => $st_interval_estimate,
        usb_time => $usb_time,
        usb_polls => $usb_polls,
        now => $now,
        GPSState => $SerialData->state
        );
    if(not $last_pps_obj->valid) {
      $last_pps_obj = undef;
      return;
    }
    check_for_sample_complete($filter, $SerialData->state, $last_pps_obj, $shmid, $now);
    if(abs($last_pps_obj->offset) > 0.0001) {
      chomp($line);
      print $last_pps_obj->offset ."/$line/$st_interval_estimate/".$now->[0]."\n"; 
    }
  } elsif($line =~ /^P (\d+) (\d+)$/) {
    my($time_now, $usb_time) = ($1,$2);
    if(not defined($last_pps_obj)) {
      print "partial time without pps sample\n";
      return;
    }
    my $midsecond_obj = USBGPS::ClockSample::MidSecond->new(
        time_now => $time_now,
        usb_time => $usb_time,
        pps_data => $last_pps_obj,
        now => $now,
        GPSState => $SerialData->state
        );
    if(not $midsecond_obj->valid) {
      return;
    }
    check_for_sample_complete($filter, $SerialData->state, $midsecond_obj, $shmid, $now);
    if(abs($midsecond_obj->offset) > 0.0001) {
      chomp($line);
      print $midsecond_obj->offset ."/$line/".$now->[0]."\n"; 
    }
  } else {
    print "bad line, $line\n";
    return;
  }
}

my $sample_start = time();
my(@samples);
my($last_samples);
sub check_for_sample_complete {
  my($filter, $GPSState, $clocksample, $shmid, $now) = @_;

  push(@samples, $clocksample);

  if($sample_start < $now->[0] - 15) {
    print PPSSAMPLES time()." O ".join(" ", map { $_->offset * 1000000 } @samples)."\n";
    print PPSSAMPLES "R ".join(" ", map { $_->remote_time->float } @samples)."\n";
    my $ntp_info;
    if(@samples > 7) { # we had coverage for more than half the time period
      $ntp_info = send_time_to_ntp($filter,$shmid,@samples);
    }
    @samples = sort { $a->offset <=> $b->offset } @samples;
    my $st_interval;
    if(defined($last_pps_obj)) {
      $st_interval = $last_pps_obj->st_interval;
    }
    printf LOGFILE ("%d %0.6f %0.6f %d %s %d %s\n",time(),$samples[0]->offset,$samples[-1]->offset,$#samples,$GPSState->logfile_string(),$st_interval,$ntp_info);
    if($samples[-1]->offset - $samples[0]->offset > 0.001 or $#samples != $last_samples) {
      printf("min = %0.6f max = %0.6f size=%0.6f count=%d hz=%d\n",$samples[0]->offset,$samples[-1]->offset,$samples[-1]->offset - $samples[0]->offset,$#samples,$st_interval_estimate);
      $last_samples = $#samples;
    }
    @samples = ();
    $sample_start = $now->[0];
  }
}

sub send_time_to_ntp {
  my($filter,$shmid,@samples) = @_;

  my($sample,$filter_stats) = $filter->filter(\@samples);

  $sample->ntp_message->send_to_ntp($shmid);

  return sprintf("%0.6f %s",$sample->offset,$filter_stats);
}

my $shmid = shmget(NTP_SHMID, 96, S_IRUSR | S_IWUSR);
die "shmget: $!" if ( $shmid < 0 );

open(LOGFILE, ">>gps.log") or die("unable to open gps.log: $!");
LOGFILE->autoflush(1);

open(PPSSAMPLES, ">>pps.log") or die("unable to open pps.log: $!");
PPSSAMPLES->autoflush(1);

# line format:
# P 1305295299 1305299665 10500794 23
# ^ time_pps   time_now   frequenc usb_transfer
# ^mark
# G fixtype,fixsatcount,satcount,avgsnr,day,month,year,hour,minute,second,valid,timestamp
my($now, @last, $line);
my $filter = new USBGPS::Filter::Percentile();
my $SerialData = new USBGPS::SerialData();
my $last_p = 1;
while(my($char,$now_s,$now_us,$ioctl_status) = timeread(fileno(STDIN))) {
  $now = [$now_s,$now_us];
  my $last_i;
  if($char eq "G") {
    $last_i = 0;
  } elsif($char eq "P") {
    $last_i = $last_p % 2 + 1;
    $last_p = $last_i;
    if($ioctl_status != 0) {
      my $lastmsg = tv_interval($last[0],$now);
      printf("[%0.6f] ioctl : %s]\n",$lastmsg,$ioctl_status);
    }
  }
  my $interval = tv_interval($last[$last_i],$now);
  $last[$last_i] = $now;

  $line = $char;
  while($char ne "\n") {
    sysread(STDIN, $char, 1);
    $line .= $char;
  }
  if($line =~ /^P/ and $ioctl_status < 0) { # throw it away
  } else {
    parseline($filter, $SerialData, $shmid, $line, $now, $interval);
  }
}

__END__
__C__
#include <sched.h>
#include <unistd.h>
#include <sys/time.h>
#include <sys/ioctl.h>

#define P_MSG_TIME 0x5461

int set_rr(int priority) {
  struct sched_param param;

  param.sched_priority = priority;
  return sched_setscheduler(0, SCHED_RR, &param);  
}

void timeread(int fd) {
  char data;
  int status;
  struct timeval now;
  struct timespec pps;
  Inline_Stack_Vars;

  if(read(fd, &data, 1) != 1) {
    Inline_Stack_Reset;
    Inline_Stack_Done;
    return;
  }
  if(data == 'P') {
    status = ioctl(fd, P_MSG_TIME, &pps);
    if(status < 0) {
      status = -errno;
      gettimeofday(&now,NULL);
    } else {
      now.tv_sec = pps.tv_sec;
      now.tv_usec = pps.tv_nsec/1000;
    }
  } else {
    gettimeofday(&now,NULL);
  }

  Inline_Stack_Reset;
  Inline_Stack_Push(sv_2mortal(newSVpvn(&data, 1)));
  Inline_Stack_Push(sv_2mortal(newSViv(now.tv_sec)));
  Inline_Stack_Push(sv_2mortal(newSViv(now.tv_usec)));
  Inline_Stack_Push(sv_2mortal(newSViv(status)));
  Inline_Stack_Done;
}