This is an automated email from the ASF dual-hosted git repository.

xiaoxiang781216 pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx-apps.git


The following commit(s) were added to refs/heads/master by this push:
     new e693e7eba system/gnsspps: Add GNSS PPS clock discipline application.
e693e7eba is described below

commit e693e7eba19ba7931e478a2c725eaa99552e29e9
Author: Alan Carvalho de Assis <[email protected]>
AuthorDate: Fri Nov 13 18:38:06 2026 -0300

    system/gnsspps: Add GNSS PPS clock discipline application.
    
    gnsspps synchronizes a NuttX board to GNSS time with the PPS output of
    a GNSS receiver, so that boards with their own receiver share the same
    time scale without a network.
    
    It reads the PPS time stamps as external time stamp (EXTTS) events
    from a PTP clock (/dev/ptpX, for example CONFIG_PTP_PPS) and steers
    the clock with clock_adjtime():
    
    * Acquire: the frequency error is measured from two pulses, then the
      phase is removed with one step.
    * Locked: a PI servo (Kp 0.7, Ki 0.3 by default, as linuxptp) runs on
      every pulse, with outlier rejection and re-acquisition.
    * Holdover: the last frequency is kept when the PPS is lost.
    * The whole seconds come from the NMEA RMC or ZDA sentences of the
      receiver, parsed with minmea (CONFIG_GNSSUTILS_MINMEA_LIB) and
      confirmed by two sentences before a step.
    * Optionally (-r) CLOCK_REALTIME follows the PTP clock.
    
    The RMS and maximum phase error and the frequency are printed every
    16 pulses.
    
    Assisted-by: Claude Code:claude-opus-5-5
    Signed-off-by: Alan Carvalho de Assis <[email protected]>
---
 system/gnsspps/CMakeLists.txt |  35 ++
 system/gnsspps/Kconfig        |  46 ++
 system/gnsspps/Make.defs      |  25 ++
 system/gnsspps/Makefile       |  32 ++
 system/gnsspps/gnsspps_main.c | 950 ++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 1088 insertions(+)

diff --git a/system/gnsspps/CMakeLists.txt b/system/gnsspps/CMakeLists.txt
new file mode 100644
index 000000000..f8550eaee
--- /dev/null
+++ b/system/gnsspps/CMakeLists.txt
@@ -0,0 +1,35 @@
+# 
##############################################################################
+# apps/system/gnsspps/CMakeLists.txt
+#
+# SPDX-License-Identifier: Apache-2.0
+#
+# Licensed to the Apache Software Foundation (ASF) under one or more 
contributor
+# license agreements.  See the NOTICE file distributed with this work for
+# additional information regarding copyright ownership.  The ASF licenses this
+# file to you under the Apache License, Version 2.0 (the "License"); you may 
not
+# use this file except in compliance with the License.  You may obtain a copy 
of
+# the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+# License for the specific language governing permissions and limitations under
+# the License.
+#
+# 
##############################################################################
+
+if(CONFIG_SYSTEM_GNSSPPS)
+  nuttx_add_application(
+    MODULE
+    ${CONFIG_SYSTEM_GNSSPPS}
+    NAME
+    ${CONFIG_SYSTEM_GNSSPPS_PROGNAME}
+    STACKSIZE
+    ${CONFIG_SYSTEM_GNSSPPS_STACKSIZE}
+    PRIORITY
+    ${CONFIG_SYSTEM_GNSSPPS_PRIORITY}
+    SRCS
+    gnsspps_main.c)
+endif()
diff --git a/system/gnsspps/Kconfig b/system/gnsspps/Kconfig
new file mode 100644
index 000000000..c4f78ef80
--- /dev/null
+++ b/system/gnsspps/Kconfig
@@ -0,0 +1,46 @@
+#
+# For a description of the syntax of this configuration file,
+# see the file kconfig-language.txt in the NuttX tools repository.
+#
+
+config SYSTEM_GNSSPPS
+       tristate "GNSS PPS clock discipline (gnsspps)"
+       default n
+       depends on PTP_CLOCK && CLOCK_ADJTIME && GNSSUTILS_MINMEA_LIB
+       ---help---
+               Discipline a PTP clock (/dev/ptpX) to the PPS output of a GNSS
+               receiver.  Each PPS edge is time stamped in hardware by an
+               external time stamp (EXTTS) input of the clock, for example
+               the PPS disciplined software clock PTP_PPS, and a PI servo
+               steers the clock frequency to align it with the pulses.  The
+               whole seconds are taken from the NMEA RMC/ZDA sentences of
+               the receiver, parsed with the minmea library
+               (GNSSUTILS_MINMEA_LIB).  Optionally CLOCK_REALTIME follows
+               the clock.
+
+               Boards that each run gnsspps with their own GNSS receiver (or
+               a shared PPS signal) end up on the same UTC time scale.
+
+if SYSTEM_GNSSPPS
+
+config SYSTEM_GNSSPPS_PROGNAME
+       string "Program name"
+       default "gnsspps"
+
+config SYSTEM_GNSSPPS_PRIORITY
+       int "gnsspps task priority"
+       default 200
+       ---help---
+               The time stamps are latched by hardware, so the priority does
+               not affect the accuracy.  A high priority makes the frequency
+               corrections happen right after each pulse.
+
+config SYSTEM_GNSSPPS_STACKSIZE
+       int "gnsspps stack size"
+       default DEFAULT_TASK_STACKSIZE
+
+config SYSTEM_GNSSPPS_DEVPATH
+       string "Default PTP clock device"
+       default "/dev/ptp0"
+
+endif # SYSTEM_GNSSPPS
diff --git a/system/gnsspps/Make.defs b/system/gnsspps/Make.defs
new file mode 100644
index 000000000..7825493a1
--- /dev/null
+++ b/system/gnsspps/Make.defs
@@ -0,0 +1,25 @@
+############################################################################
+# apps/system/gnsspps/Make.defs
+#
+# SPDX-License-Identifier: Apache-2.0
+#
+# Licensed to the Apache Software Foundation (ASF) under one or more
+# contributor license agreements.  See the NOTICE file distributed with
+# this work for additional information regarding copyright ownership.  The
+# ASF licenses this file to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance with the
+# License.  You may obtain a copy of the License at
+#
+#   http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+# License for the specific language governing permissions and limitations
+# under the License.
+#
+############################################################################
+
+ifneq ($(CONFIG_SYSTEM_GNSSPPS),)
+CONFIGURED_APPS += $(APPDIR)/system/gnsspps
+endif
diff --git a/system/gnsspps/Makefile b/system/gnsspps/Makefile
new file mode 100644
index 000000000..330c34f34
--- /dev/null
+++ b/system/gnsspps/Makefile
@@ -0,0 +1,32 @@
+############################################################################
+# apps/system/gnsspps/Makefile
+#
+# SPDX-License-Identifier: Apache-2.0
+#
+# Licensed to the Apache Software Foundation (ASF) under one or more
+# contributor license agreements.  See the NOTICE file distributed with
+# this work for additional information regarding copyright ownership.  The
+# ASF licenses this file to you under the Apache License, Version 2.0 (the
+# "License"); you may not use this file except in compliance with the
+# License.  You may obtain a copy of the License at
+#
+#   http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+# License for the specific language governing permissions and limitations
+# under the License.
+#
+############################################################################
+
+include $(APPDIR)/Make.defs
+
+PROGNAME = $(CONFIG_SYSTEM_GNSSPPS_PROGNAME)
+PRIORITY = $(CONFIG_SYSTEM_GNSSPPS_PRIORITY)
+STACKSIZE = $(CONFIG_SYSTEM_GNSSPPS_STACKSIZE)
+MODULE = $(CONFIG_SYSTEM_GNSSPPS)
+
+MAINSRC = gnsspps_main.c
+
+include $(APPDIR)/Application.mk
diff --git a/system/gnsspps/gnsspps_main.c b/system/gnsspps/gnsspps_main.c
new file mode 100644
index 000000000..20127fc09
--- /dev/null
+++ b/system/gnsspps/gnsspps_main.c
@@ -0,0 +1,950 @@
+/****************************************************************************
+ * apps/system/gnsspps/gnsspps_main.c
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/* gnsspps - discipline a PTP clock (/dev/ptpX) to the PPS output of a GNSS
+ * receiver.
+ *
+ * Every PPS edge is time stamped in hardware by the PTP clock driver and
+ * read from /dev/ptpX as a struct ptp_extts_event.  The fractional part of
+ * that time stamp is the phase error of the clock, and a PI servo steers
+ * the clock frequency with clock_adjtime(ADJ_FREQUENCY) to drive it to
+ * zero.  The whole seconds come from the NMEA sentences (RMC or ZDA) that
+ * the receiver sends after each pulse.  Boards that run gnsspps with their
+ * own GNSS receivers then share the same UTC time scale, to within the
+ * accuracy of the receivers' PPS (typically some tens of nanoseconds).
+ *
+ * Optionally, CLOCK_REALTIME is kept aligned with the disciplined clock.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/ioctl.h>
+#include <sys/time.h>
+#include <sys/timex.h>
+#include <sys/types.h>
+
+#include <errno.h>
+#include <fcntl.h>
+#include <inttypes.h>
+#include <poll.h>
+#include <signal.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <unistd.h>
+
+#ifdef CONFIG_SERIAL_TERMIOS
+#  include <termios.h>
+#endif
+
+#include <nuttx/clock.h>
+#include <nuttx/timers/ptp_clock.h>
+
+#include <minmea/minmea.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define STATE_UNLOCKED      0   /* Waiting for the first sample */
+#define STATE_ACQUIRE       1   /* Have one sample, estimating frequency */
+#define STATE_LOCKED        2   /* PI servo running */
+
+#define OUTLIER_LIMIT       4   /* Re-acquire after this many outliers */
+#define NMEA_CONFIRM        2   /* Consistent NMEA seconds before a step */
+#define PPS_TIMEOUT_MS      2500
+#define STATS_INTERVAL      16  /* Up to 73 (0.5 s)^2 fit in stat_sq */
+
+#define RT_SYS_SAMPLES      5
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+struct gnsspps_s
+{
+  /* Configuration */
+
+  FAR const char *ptpdev;     /* PTP clock device */
+  FAR const char *nmeadev;    /* NMEA serial device, or NULL */
+  long            baud;       /* NMEA baud rate, 0 to keep */
+  unsigned int    channel;    /* EXTTS channel wired to PPS */
+  int64_t         delay_ns;   /* PPS delay: antenna cable + receiver */
+  int64_t         step_ns;    /* Larger offsets are outliers when locked */
+  int64_t         rtstep_ns;  /* CLOCK_REALTIME step threshold */
+  double          kp;         /* Proportional gain */
+  double          ki;         /* Integral gain */
+  unsigned long   count;      /* Exit after this many pulses, 0: never */
+  bool            follow_rt;  /* Keep CLOCK_REALTIME in sync */
+  bool            verbose;    /* Print every sample */
+
+  /* Devices */
+
+  int             ptpfd;
+  int             nmeafd;
+  clockid_t       clkid;
+  long            max_adj;    /* Frequency limit of the clock (ppb) */
+
+  /* Servo */
+
+  int             state;
+  int             outliers;
+  int64_t         freq;       /* Applied frequency offset (ppb) */
+  double          integral;   /* Integral term (ppb) */
+  int64_t         acq_offset; /* First sample of the acquisition */
+  int64_t         acq_sec;
+  int64_t         last_sec;   /* Second of the last pulse */
+
+  /* Time of day from NMEA */
+
+  struct timespec pulse_mono; /* When the last pulse was processed */
+  bool            have_pulse;
+  int64_t         pulse_sec;  /* Second of the last pulse, PTP scale */
+  int64_t         nmea_last;  /* Last NMEA second, to ignore repeats */
+  int64_t         nmea_err;   /* Pending whole second error */
+  int             nmea_cnt;   /* Consecutive identical errors */
+  bool            tod_ok;     /* Seconds confirmed by NMEA */
+
+  /* The longest sentence that minmea_check() accepts, and the NUL */
+
+  char            line[MINMEA_MAX_SENTENCE_LENGTH + 4];
+  size_t          linelen;
+
+  /* Statistics */
+
+  unsigned long   pulses;
+  unsigned long   nstats;
+  int64_t         stat_max;
+  uint64_t        stat_sq;    /* Sum of the squared offsets (ns^2) */
+};
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static volatile bool g_gnsspps_exit;
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static void gnsspps_sighandler(int signo)
+{
+  g_gnsspps_exit = true;
+}
+
+static void gnsspps_usage(FAR const char *progname)
+{
+  fprintf(stderr,
+          "Usage: %s [options]\n"
+          "  -d <dev>   PTP clock with the PPS input (default %s)\n"
+          "  -c <n>     External time stamp channel of the PPS (default 0)\n"
+          "  -s <tty>   NMEA serial port of the GNSS receiver, for the\n"
+          "             time of day.  Without it only the phase is "
+          "locked\n"
+          "  -b <baud>  Baud rate of the NMEA serial port\n"
+          "  -D <ns>    PPS delay to compensate (cable, receiver), ns\n"
+          "  -P <kp>    Proportional gain (default 0.7)\n"
+          "  -I <ki>    Integral gain (default 0.3)\n"
+          "  -S <ns>    Outlier/step threshold when locked (default "
+          "20000)\n"
+          "  -r         Keep CLOCK_REALTIME synchronized to the clock\n"
+          "  -R <ns>    Step CLOCK_REALTIME above this error (default "
+          "1000000)\n"
+          "  -n <n>     Exit after n pulses\n"
+          "  -v         Print every sample\n",
+          progname, CONFIG_SYSTEM_GNSSPPS_DEVPATH);
+}
+
+static inline int64_t pct2ns(FAR const struct ptp_clock_time *t)
+{
+  return t->sec * NSEC_PER_SEC + t->nsec;
+}
+
+static int64_t mono_ms_since(FAR const struct timespec *then)
+{
+  struct timespec now;
+
+  clock_gettime(CLOCK_MONOTONIC, &now);
+  return NSEC2MSEC(clock_time2nsec(&now) - clock_time2nsec(then));
+}
+
+/****************************************************************************
+ * Clock control
+ ****************************************************************************/
+
+static int gnsspps_setfreq(FAR struct gnsspps_s *g, double ppb)
+{
+  struct timex tx;
+
+  if (ppb > g->max_adj)
+    {
+      ppb = g->max_adj;
+    }
+  else if (ppb < -g->max_adj)
+    {
+      ppb = -g->max_adj;
+    }
+
+  memset(&tx, 0, sizeof(tx));
+  tx.modes = ADJ_FREQUENCY;
+  tx.freq  = ppb * 65.536;  /* ppb to ppm with a 16 bit fraction */
+
+  if (clock_adjtime(g->clkid, &tx) < 0)
+    {
+      fprintf(stderr, "gnsspps: ADJ_FREQUENCY failed: %d\n", errno);
+      return -errno;
+    }
+
+  g->freq = ppb;
+  return 0;
+}
+
+static int gnsspps_step(FAR struct gnsspps_s *g, int64_t delta_ns)
+{
+  struct timex tx;
+  int64_t sec = delta_ns / NSEC_PER_SEC;
+  int64_t nsec = delta_ns - sec * NSEC_PER_SEC;
+
+  /* ADJ_SETOFFSET needs 0 <= nsec < 1 s, also for a negative delta */
+
+  if (nsec < 0)
+    {
+      nsec += NSEC_PER_SEC;
+      sec--;
+    }
+
+  memset(&tx, 0, sizeof(tx));
+  tx.modes        = ADJ_SETOFFSET | ADJ_NANO;
+  tx.time.tv_sec  = sec;
+  tx.time.tv_usec = nsec;
+
+  if (clock_adjtime(g->clkid, &tx) < 0)
+    {
+      fprintf(stderr, "gnsspps: ADJ_SETOFFSET failed: %d\n", errno);
+      return -errno;
+    }
+
+  return 0;
+}
+
+/* Discard the time stamps that were taken before a step */
+
+static void gnsspps_drain(FAR struct gnsspps_s *g)
+{
+  struct ptp_extts_event ev[4];
+  int flags = fcntl(g->ptpfd, F_GETFL);
+
+  fcntl(g->ptpfd, F_SETFL, flags | O_NONBLOCK);
+  while (read(g->ptpfd, ev, sizeof(ev)) > 0)
+    {
+    }
+
+  fcntl(g->ptpfd, F_SETFL, flags);
+}
+
+/****************************************************************************
+ * CLOCK_REALTIME following
+ ****************************************************************************/
+
+/* Offset of the PTP clock from CLOCK_REALTIME (ns), measured with the
+ * smallest possible uncertainty.
+ */
+
+static int gnsspps_rtoffset(FAR struct gnsspps_s *g, FAR int64_t *offset)
+{
+  struct ptp_sys_offset_precise prec;
+  struct ptp_sys_offset sys;
+  int64_t best = INT64_MAX;
+  int i;
+
+  if (ioctl(g->ptpfd, PTP_SYS_OFFSET_PRECISE, &prec) == 0)
+    {
+      *offset = pct2ns(&prec.device) - pct2ns(&prec.sys_realtime);
+      return 0;
+    }
+
+  memset(&sys, 0, sizeof(sys));
+  sys.n_samples = RT_SYS_SAMPLES;
+  if (ioctl(g->ptpfd, PTP_SYS_OFFSET, &sys) < 0)
+    {
+      return -errno;
+    }
+
+  for (i = 0; i < RT_SYS_SAMPLES; i++)
+    {
+      int64_t t1 = pct2ns(&sys.ts[2 * i]);
+      int64_t tp = pct2ns(&sys.ts[2 * i + 1]);
+      int64_t t2 = pct2ns(&sys.ts[2 * i + 2]);
+
+      if (t2 - t1 < best)
+        {
+          best    = t2 - t1;
+          *offset = tp - (t1 + (t2 - t1) / 2);
+        }
+    }
+
+  return 0;
+}
+
+static void gnsspps_follow_rt(FAR struct gnsspps_s *g)
+{
+  struct timeval delta;
+  struct timespec now;
+  int64_t offset = 0;
+
+  if (gnsspps_rtoffset(g, &offset) < 0)
+    {
+      return;
+    }
+
+  if (g->verbose)
+    {
+      printf("gnsspps: CLOCK_REALTIME offset %+" PRId64 " ns\n", -offset);
+    }
+
+  if (llabs(offset) > g->rtstep_ns)
+    {
+      clock_gettime(CLOCK_REALTIME, &now);
+      clock_nsec2time(&now, clock_time2nsec(&now) + offset);
+      clock_settime(CLOCK_REALTIME, &now);
+
+      if (g->verbose)
+        {
+          printf("gnsspps: CLOCK_REALTIME stepped by %+" PRId64 " ns\n",
+                 offset);
+        }
+    }
+  else
+    {
+      delta.tv_sec  = offset / NSEC_PER_SEC;
+      delta.tv_usec = offset % NSEC_PER_SEC / NSEC_PER_USEC;
+      adjtime(&delta, NULL);
+    }
+}
+
+/****************************************************************************
+ * Servo
+ ****************************************************************************/
+
+static void gnsspps_stats(FAR struct gnsspps_s *g, int64_t offset)
+{
+  int64_t absoff = llabs(offset);
+
+  if (absoff > g->stat_max)
+    {
+      g->stat_max = absoff;
+    }
+
+  g->stat_sq += offset * offset;
+  if (++g->nstats >= STATS_INTERVAL)
+    {
+      uint64_t sq = g->stat_sq / g->nstats;
+      uint64_t rms = sq;
+      uint64_t x = (sq + 1) / 2;
+
+      /* Integer square root by Newton iterations, avoids libm */
+
+      while (x < rms)
+        {
+          rms = x;
+          x   = (rms + sq / rms) / 2;
+        }
+
+      printf("gnsspps: rms %" PRIu64 " ns max %" PRId64
+             " ns freq %+" PRId64 " ppb%s\n",
+             rms, g->stat_max, g->freq,
+             g->nmeafd >= 0 && !g->tod_ok ? " (time of day unknown)" : "");
+
+      g->nstats   = 0;
+      g->stat_max = 0;
+      g->stat_sq  = 0;
+    }
+}
+
+/* Process one PPS time stamp.  offset > 0 means that the clock is ahead */
+
+static void gnsspps_sample(FAR struct gnsspps_s *g, int64_t pulse_ns)
+{
+  int64_t sec = pulse_ns / NSEC_PER_SEC;
+  int64_t frac = pulse_ns - sec * NSEC_PER_SEC;
+  int64_t offset;
+  int64_t absoff;
+  int64_t interval;
+  double ppb;
+
+  /* The pulse marks a whole second: the phase error is the distance to
+   * the nearest second, in (-0.5 s, +0.5 s].
+   */
+
+  if (frac < 0)
+    {
+      frac += NSEC_PER_SEC;
+      sec--;
+    }
+
+  if (frac > NSEC_PER_HSEC)
+    {
+      offset = frac - NSEC_PER_SEC;
+      sec++;
+    }
+  else
+    {
+      offset = frac;
+    }
+
+  absoff = llabs(offset);
+  interval = sec - g->last_sec;
+  g->last_sec = sec;
+
+  clock_gettime(CLOCK_MONOTONIC, &g->pulse_mono);
+  g->pulse_sec  = sec;
+  g->have_pulse = true;
+  g->pulses++;
+
+  switch (g->state)
+    {
+      case STATE_UNLOCKED:
+        g->acq_offset = offset;
+        g->acq_sec    = sec;
+        g->state      = STATE_ACQUIRE;
+        if (g->verbose)
+          {
+            printf("gnsspps: offset %+" PRId64 " ns, acquiring\n", offset);
+          }
+        break;
+
+      case STATE_ACQUIRE:
+        interval = sec - g->acq_sec;
+        if (interval < 1 || interval > 4)
+          {
+            g->acq_offset = offset;
+            g->acq_sec    = sec;
+            break;
+          }
+
+        /* Frequency error of the clock: phase change per second */
+
+        ppb = g->freq - (offset - g->acq_offset) / interval;
+        gnsspps_setfreq(g, ppb);
+        g->integral = g->freq;
+
+        /* Remove the phase error with one step */
+
+        gnsspps_step(g, -offset);
+        gnsspps_drain(g);
+        g->state    = STATE_LOCKED;
+        g->outliers = 0;
+
+        printf("gnsspps: locked, offset %+" PRId64 " ns stepped, "
+               "freq %+" PRId64 " ppb\n", offset, g->freq);
+        break;
+
+      case STATE_LOCKED:
+        if (absoff > g->step_ns)
+          {
+            if (++g->outliers >= OUTLIER_LIMIT)
+              {
+                printf("gnsspps: offset %+" PRId64 " ns, lost lock\n",
+                       offset);
+                g->state = STATE_UNLOCKED;
+              }
+            else if (g->verbose)
+              {
+                printf("gnsspps: outlier %+" PRId64 " ns ignored\n",
+                       offset);
+              }
+
+            break;
+          }
+
+        if (interval < 1 || interval > 16)
+          {
+            interval = 1;
+          }
+
+        g->outliers  = 0;
+        ppb          = g->integral - (g->kp + g->ki * interval) * offset;
+        g->integral -= g->ki * interval * offset;
+
+        if (g->integral > g->max_adj)
+          {
+            g->integral = g->max_adj;
+          }
+        else if (g->integral < -g->max_adj)
+          {
+            g->integral = -g->max_adj;
+          }
+
+        gnsspps_setfreq(g, ppb);
+
+        if (g->verbose)
+          {
+            printf("gnsspps: offset %+6" PRId64 " ns freq %+" PRId64
+                   " ppb\n", offset, g->freq);
+          }
+
+        gnsspps_stats(g, offset);
+
+        if (g->follow_rt)
+          {
+            gnsspps_follow_rt(g);
+          }
+        break;
+    }
+}
+
+/****************************************************************************
+ * NMEA time of day
+ ****************************************************************************/
+
+/* Parse an RMC or ZDA sentence into UTC seconds.  Returns false if the
+ * sentence carries no valid time or no date.
+ */
+
+static bool gnsspps_nmea_time(FAR const char *line, FAR time_t *utc)
+{
+  struct minmea_sentence_rmc rmc;
+  struct minmea_sentence_zda zda;
+  FAR struct minmea_date *date;
+  FAR struct minmea_time *hms;
+  struct timespec ts;
+
+  /* Strict: the sentence must have a correct checksum */
+
+  switch (minmea_sentence_id(line, true))
+    {
+      case MINMEA_SENTENCE_RMC:
+        if (!minmea_parse_rmc(&rmc, line) || !rmc.valid)
+          {
+            return false;
+          }
+
+        date = &rmc.date;
+        hms  = &rmc.time;
+        break;
+
+      case MINMEA_SENTENCE_ZDA:
+        if (!minmea_parse_zda(&zda, line))
+          {
+            return false;
+          }
+
+        date = &zda.date;
+        hms  = &zda.time;
+        break;
+
+      default:
+        return false;
+    }
+
+  /* Empty date fields are -1 (RMC) or 0 (ZDA) */
+
+  if (date->day < 1 || date->month < 1 ||
+      minmea_gettime(&ts, date, hms) < 0)
+    {
+      return false;
+    }
+
+  *utc = ts.tv_sec;
+  return true;
+}
+
+/* The receiver sends the time of a pulse shortly after the pulse.  Compare
+ * it with the second the clock gave to that pulse and fix whole seconds.
+ */
+
+static void gnsspps_nmea_line(FAR struct gnsspps_s *g)
+{
+  time_t utc;
+  int64_t err;
+
+  if (!gnsspps_nmea_time(g->line, &utc) || utc == g->nmea_last)
+    {
+      return;
+    }
+
+  g->nmea_last = utc;
+
+  if (!g->have_pulse || mono_ms_since(&g->pulse_mono) >= 1000)
+    {
+      return;
+    }
+
+  err = utc - g->pulse_sec;
+  if (err == 0)
+    {
+      if (!g->tod_ok)
+        {
+          printf("gnsspps: time of day confirmed by NMEA\n");
+        }
+
+      g->tod_ok   = true;
+      g->nmea_cnt = 0;
+      return;
+    }
+
+  if (err == g->nmea_err)
+    {
+      g->nmea_cnt++;
+    }
+  else
+    {
+      g->nmea_err = err;
+      g->nmea_cnt = 1;
+    }
+
+  if (g->nmea_cnt >= NMEA_CONFIRM)
+    {
+      printf("gnsspps: stepping %+" PRId64 " s to NMEA time\n", err);
+      if (gnsspps_step(g, err * NSEC_PER_SEC) == 0)
+        {
+          gnsspps_drain(g);
+          g->pulse_sec += err;
+          g->last_sec  += err;
+          g->acq_sec   += err;
+          g->tod_ok     = true;
+        }
+
+      g->nmea_cnt = 0;
+    }
+}
+
+static void gnsspps_nmea_read(FAR struct gnsspps_s *g)
+{
+  char buf[64];
+  ssize_t n;
+  ssize_t i;
+
+  n = read(g->nmeafd, buf, sizeof(buf));
+  for (i = 0; i < n; i++)
+    {
+      char c = buf[i];
+
+      if (c == '$')
+        {
+          g->linelen = 0;
+        }
+
+      if (c == '\r' || c == '\n')
+        {
+          if (g->linelen > 0)
+            {
+              g->line[g->linelen] = '\0';
+              gnsspps_nmea_line(g);
+              g->linelen = 0;
+            }
+        }
+      else if (g->linelen < sizeof(g->line) - 1)
+        {
+          g->line[g->linelen++] = c;
+        }
+      else
+        {
+          g->linelen = 0;
+        }
+    }
+}
+
+static int gnsspps_nmea_open(FAR struct gnsspps_s *g)
+{
+  g->nmeafd = open(g->nmeadev, O_RDONLY | O_NONBLOCK | O_NOCTTY |
+                   O_CLOEXEC);
+  if (g->nmeafd < 0)
+    {
+      fprintf(stderr, "gnsspps: cannot open %s: %d\n", g->nmeadev, errno);
+      return -errno;
+    }
+
+#ifdef CONFIG_SERIAL_TERMIOS
+  if (g->baud > 0)
+    {
+      struct termios tio;
+
+      if (tcgetattr(g->nmeafd, &tio) == 0)
+        {
+          cfmakeraw(&tio);
+          cfsetspeed(&tio, g->baud);
+          tcsetattr(g->nmeafd, TCSANOW, &tio);
+        }
+    }
+#endif
+
+  return 0;
+}
+
+/****************************************************************************
+ * Setup
+ ****************************************************************************/
+
+static int gnsspps_open(FAR struct gnsspps_s *g)
+{
+  struct ptp_extts_request req;
+  struct ptp_clock_caps caps;
+  struct timespec ts;
+
+  g->ptpfd = open(g->ptpdev, O_RDWR | O_CLOEXEC);
+  if (g->ptpfd < 0)
+    {
+      fprintf(stderr, "gnsspps: cannot open %s: %d\n", g->ptpdev, errno);
+      return -errno;
+    }
+
+  g->clkid = (g->ptpfd << CLOCK_SHIFT) | CLOCK_FD;
+
+  memset(&caps, 0, sizeof(caps));
+  if (ioctl(g->ptpfd, PTP_CLOCK_GETCAPS, &caps) < 0)
+    {
+      fprintf(stderr, "gnsspps: PTP_CLOCK_GETCAPS failed: %d\n", errno);
+      return -errno;
+    }
+
+  if (g->channel >= caps.n_ext_ts)
+    {
+      fprintf(stderr, "gnsspps: %s has %d external time stamp inputs\n",
+              g->ptpdev, caps.n_ext_ts);
+      return -EINVAL;
+    }
+
+  g->max_adj = caps.max_adj > 0 ? caps.max_adj : 500000;
+
+  /* A clock that was never set starts near zero.  Start from the system
+   * time; NMEA will fix the seconds later if it is available.
+   */
+
+  if (clock_gettime(g->clkid, &ts) == 0 && ts.tv_sec < 365 * 86400)
+    {
+      clock_gettime(CLOCK_REALTIME, &ts);
+      clock_settime(g->clkid, &ts);
+    }
+
+  gnsspps_setfreq(g, 0);
+
+  memset(&req, 0, sizeof(req));
+  req.index = g->channel;
+  req.flags = PTP_ENABLE_FEATURE | PTP_RISING_EDGE;
+  if (ioctl(g->ptpfd, PTP_EXTTS_REQUEST, &req) < 0)
+    {
+      fprintf(stderr, "gnsspps: PTP_EXTTS_REQUEST failed: %d\n", errno);
+      return -errno;
+    }
+
+  return 0;
+}
+
+static void gnsspps_close(FAR struct gnsspps_s *g)
+{
+  if (g->ptpfd >= 0)
+    {
+      struct ptp_extts_request req;
+
+      memset(&req, 0, sizeof(req));
+      req.index = g->channel;
+      ioctl(g->ptpfd, PTP_EXTTS_REQUEST, &req);
+      close(g->ptpfd);
+    }
+
+  if (g->nmeafd >= 0)
+    {
+      close(g->nmeafd);
+    }
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+int main(int argc, FAR char *argv[])
+{
+  struct gnsspps_s g;
+  struct pollfd pfd[2];
+  struct timespec last_pps;
+  int nfds;
+  int opt;
+  int ret;
+
+  memset(&g, 0, sizeof(g));
+  g.ptpdev    = CONFIG_SYSTEM_GNSSPPS_DEVPATH;
+  g.ptpfd     = -1;
+  g.nmeafd    = -1;
+  g.kp        = 0.7;
+  g.ki        = 0.3;
+  g.step_ns   = 20000;
+  g.rtstep_ns = NSEC_PER_MSEC;
+  g.nmea_last = -1;
+
+  while ((opt = getopt(argc, argv, "d:c:s:b:D:P:I:S:rR:n:vh")) != -1)
+    {
+      switch (opt)
+        {
+          case 'd':
+            g.ptpdev = optarg;
+            break;
+
+          case 'c':
+            g.channel = strtoul(optarg, NULL, 0);
+            break;
+
+          case 's':
+            g.nmeadev = optarg;
+            break;
+
+          case 'b':
+            g.baud = strtol(optarg, NULL, 0);
+            break;
+
+          case 'D':
+            g.delay_ns = strtoll(optarg, NULL, 0);
+            break;
+
+          case 'P':
+            g.kp = strtod(optarg, NULL);
+            break;
+
+          case 'I':
+            g.ki = strtod(optarg, NULL);
+            break;
+
+          case 'S':
+            g.step_ns = strtoll(optarg, NULL, 0);
+            break;
+
+          case 'r':
+            g.follow_rt = true;
+            break;
+
+          case 'R':
+            g.rtstep_ns = strtoll(optarg, NULL, 0);
+            break;
+
+          case 'n':
+            g.count = strtoul(optarg, NULL, 0);
+            break;
+
+          case 'v':
+            g.verbose = true;
+            break;
+
+          default:
+            gnsspps_usage(argv[0]);
+            return opt == 'h' ? EXIT_SUCCESS : EXIT_FAILURE;
+        }
+    }
+
+  g_gnsspps_exit = false;
+  signal(SIGINT, gnsspps_sighandler);
+  signal(SIGTERM, gnsspps_sighandler);
+
+  ret = gnsspps_open(&g);
+  if (ret == 0 && g.nmeadev != NULL)
+    {
+      ret = gnsspps_nmea_open(&g);
+    }
+
+  if (ret < 0)
+    {
+      gnsspps_close(&g);
+      return EXIT_FAILURE;
+    }
+
+  printf("gnsspps: %s channel %u%s%s\n", g.ptpdev, g.channel,
+         g.nmeadev ? ", NMEA from " : ", no time of day source",
+         g.nmeadev ? g.nmeadev : "");
+
+  clock_gettime(CLOCK_MONOTONIC, &last_pps);
+
+  while (!g_gnsspps_exit && (g.count == 0 || g.pulses < g.count))
+    {
+      pfd[0].fd      = g.ptpfd;
+      pfd[0].events  = POLLIN;
+      pfd[0].revents = 0;
+      nfds           = 1;
+
+      if (g.nmeafd >= 0)
+        {
+          pfd[1].fd      = g.nmeafd;
+          pfd[1].events  = POLLIN;
+          pfd[1].revents = 0;
+          nfds           = 2;
+        }
+
+      ret = poll(pfd, nfds, PPS_TIMEOUT_MS);
+      if (ret < 0)
+        {
+          if (errno == EINTR)
+            {
+              continue;
+            }
+
+          fprintf(stderr, "gnsspps: poll failed: %d\n", errno);
+          break;
+        }
+
+      if (pfd[0].revents & POLLIN)
+        {
+          struct ptp_extts_event ev[4];
+          ssize_t n;
+          size_t i;
+
+          n = read(g.ptpfd, ev, sizeof(ev));
+          for (i = 0; n > 0 && i < n / sizeof(ev[0]); i++)
+            {
+              if (ev[i].index == g.channel)
+                {
+                  /* A delayed pulse is captured late: remove the delay */
+
+                  gnsspps_sample(&g, pct2ns(&ev[i].t) - g.delay_ns);
+                  clock_gettime(CLOCK_MONOTONIC, &last_pps);
+                }
+            }
+        }
+
+      if (nfds > 1 && (pfd[1].revents & POLLIN))
+        {
+          gnsspps_nmea_read(&g);
+        }
+
+      if (mono_ms_since(&last_pps) > PPS_TIMEOUT_MS)
+        {
+          /* Holdover: keep the last frequency until the PPS returns */
+
+          if (g.state != STATE_UNLOCKED)
+            {
+              printf("gnsspps: no PPS, holding frequency %+" PRId64
+                     " ppb\n", g.freq);
+            }
+
+          g.state = g.state == STATE_LOCKED ? STATE_UNLOCKED : g.state;
+          clock_gettime(CLOCK_MONOTONIC, &last_pps);
+        }
+    }
+
+  gnsspps_close(&g);
+  return EXIT_SUCCESS;
+}

Reply via email to