Merge pull request machinekit#1373 from zultron/task-pll-correction

Port sittner's add-task-pll-functions.patch to Machinekit

src/rtapi/rt-preempt.c

@@ -328,7 +328,7 @@ static void *realtime_thread(void *arg) {
 
     clock_gettime(CLOCK_MONOTONIC, &extra_task_data[task_id(task)].next_time);
     _rtapi_advance_time(&extra_task_data[task_id(task)].next_time,
-		       task->period, 0);
+		       task->period + task->pll_correction, 0);
 
     _rtapi_task_update_stats_hook(); // inital stats update
 
@@ -423,7 +423,7 @@ int _rtapi_wait_hook(const int flags) {
     clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME,
 		    &extra_task_data[task_id(task)].next_time, NULL);
     _rtapi_advance_time(&extra_task_data[task_id(task)].next_time,
-		       task->period, 0);
+		       task->period + task->pll_correction, 0);
     clock_gettime(CLOCK_MONOTONIC, &ts);
     if (ts.tv_sec > extra_task_data[task_id(task)].next_time.tv_sec
 	|| (ts.tv_sec == extra_task_data[task_id(task)].next_time.tv_sec
@@ -478,4 +478,26 @@ int _rtapi_task_self_hook(void) {
     return -EINVAL;
 }
 
+long long _rtapi_task_pll_get_reference_hook(void) {
+    int task_id = _rtapi_task_self_hook();
+    if (task_id < 0) return 0;
+    return extra_task_data[task_id].next_time.tv_sec * 1000000000LL
+        + extra_task_data[task_id].next_time.tv_nsec;
+}
+
+int _rtapi_task_pll_set_correction_hook(long value) {
+    int task_id = _rtapi_task_self_hook();
+    task_data *task = &task_array[task_id];
+    if (task <= 0) return -EINVAL;
+    if (value > task->pll_correction_limit)
+        value = task->pll_correction_limit;
+    if (value < -(task->pll_correction_limit))
+        value = -(task->pll_correction_limit);
+    task->pll_correction = value;
+    rtapi_print_msg(RTAPI_MSG_DBG,
+		    "Task %d pll correction set to %ld\n",
+                    task_id, value);
+    return 0;
+}
+
 #endif /* RTAPI */

src/rtapi/rt-preempt.h

@@ -50,6 +50,8 @@
 #define HAVE_RTAPI_WAIT_HOOK
 #define HAVE_RTAPI_TASK_SELF_HOOK
 #define HAVE_RTAPI_TASK_UPDATE_STATS_HOOK
+#define HAVE_RTAPI_TASK_PLL_GET_REFERENCE_HOOK
+#define HAVE_RTAPI_TASK_PLL_SET_CORRECTION_HOOK
 
 #if !defined(__i386__) && !defined(__x86_64__)
 #define HAVE_RTAPI_GET_CLOCKS_HOOK // needed for e.g. ARM, see rtapi_time.c

src/rtapi/rtapi.h

@@ -655,6 +655,30 @@ typedef long int (*rtapi_delay_max_t)(void);
     rtapi_switch->rtapi_delay_max()
 extern long int _rtapi_delay_max(void);
 
+/** Support external clock tracking for linuxcnc-ethercat */
+#define RTAPI_TASK_PLL_SUPPORT
+
+/** 'rtapi_task_pll_get_reference()' gets the reference timestamp
+    for the start of the current cycle.
+    Returns 0 if not called from within task context or on
+    platforms that do not support this.
+*/
+typedef long long (*rtapi_task_pll_get_reference_t)(void);
+#define rtapi_task_pll_get_reference()			\
+    rtapi_switch->rtapi_task_pll_get_reference()
+extern long long _rtapi_task_pll_get_reference(void);
+
+/** 'rtapi_task_pll_set_correction()' sets the correction value for
+    the next scheduling cycle of the current task. This could be
+    used to synchronize the task cycle to external sources.
+    Returns -EINVAL if not called from within task context or on
+    platforms that do not support this.
+*/
+typedef int (*rtapi_task_pll_set_correction_t)(long);
+#define rtapi_task_pll_set_correction(value)			\
+    rtapi_switch->rtapi_task_pll_set_correction(value)
+extern int _rtapi_task_pll_set_correction(long value);
+
 #endif /* RTAPI */
 
 /** rtapi_get_time returns the current time in nanoseconds.  Depending
@@ -1080,10 +1104,14 @@ typedef struct {
     rtapi_clock_set_period_t rtapi_clock_set_period;
     rtapi_delay_t rtapi_delay;
     rtapi_delay_max_t rtapi_delay_max;
+    rtapi_task_pll_get_reference_t rtapi_task_pll_get_reference;
+    rtapi_task_pll_set_correction_t rtapi_task_pll_set_correction;
 #else
     rtapi_dummy_t rtapi_clock_set_period;
     rtapi_dummy_t rtapi_delay;
     rtapi_dummy_t rtapi_delay_max;
+    rtapi_dummy_t rtapi_task_pll_get_reference;
+    rtapi_dummy_t rtapi_task_pll_set_correction;
 #endif
     rtapi_get_time_t rtapi_get_time;
     rtapi_get_clocks_t rtapi_get_clocks;
@@ -1140,7 +1168,6 @@ typedef struct {
     rtapi_heap_status_t  rtapi_heap_status;
     rtapi_heap_setflags_t rtapi_heap_setflags;
     rtapi_heap_walk_freelist_t rtapi_heap_walk_freelist;
-
 } rtapi_switch_t;
 
 // using code is responsible to define this:

src/rtapi/rtapi_common.c

@@ -93,10 +93,14 @@ static rtapi_switch_t rtapi_switch_struct = {
     .rtapi_clock_set_period = &_rtapi_clock_set_period,
     .rtapi_delay = &_rtapi_delay,
     .rtapi_delay_max = &_rtapi_delay_max,
+    .rtapi_task_pll_get_reference = &_rtapi_task_pll_get_reference,
+    .rtapi_task_pll_set_correction = &_rtapi_task_pll_set_correction,
 #else
     .rtapi_clock_set_period = &_rtapi_dummy,
     .rtapi_delay = &_rtapi_dummy,
     .rtapi_delay_max = &_rtapi_dummy,
+    .rtapi_task_pll_get_reference = &_rtapi_dummy,
+    .rtapi_task_pll_set_correction = &_rtapi_dummy,
 #endif
     .rtapi_get_time = &_rtapi_get_time,
     .rtapi_get_clocks = &_rtapi_get_clocks,

src/rtapi/rtapi_common.h

@@ -150,6 +150,8 @@ typedef struct {
     size_t stacksize;
     int period;
     int ratio;
+    long pll_correction;
+    long pll_correction_limit;
     task_state_t state;		/* task state */
     int prio;			/* priority */
     int owner;			/* owning module */

src/rtapi/rtapi_task.c

@@ -346,13 +346,18 @@ int _rtapi_task_start(int task_id, unsigned long int period_nsec) {
     task->period = period_nsec;
     task->ratio = period_nsec / period;
 
+    // limit PLL correction values to +/-1% of cycle time
+    task->pll_correction_limit = period_nsec / 100;
+    task->pll_correction = 0;
+
     rtapi_print_msg(RTAPI_MSG_DBG,
 		    "rtapi_task_start:  starting task %d '%s'\n",
 		    task_id, task->name);
     rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI: period_nsec: %ld\n", period_nsec);
 
     return _rtapi_task_start_hook(task,task_id,0);
 }
+
 #else  /* kernel RTAPI */
 int _rtapi_task_start(int task_id, unsigned long int period_nsec) {
     int retval;
@@ -520,5 +525,30 @@ int _rtapi_task_self(void) {
 #endif
 }
 
+#ifdef HAVE_RTAPI_TASK_PLL_GET_REFERENCE_HOOK
+long long _rtapi_task_pll_get_reference_hook(void);
+#endif
+
+long long _rtapi_task_pll_get_reference(void) {
+#ifdef HAVE_RTAPI_TASK_PLL_GET_REFERENCE_HOOK
+    return _rtapi_task_pll_get_reference_hook();
+#else
+    return 0;
+#endif
+}
+
+#ifdef HAVE_RTAPI_TASK_PLL_SET_CORRECTION_HOOK
+int _rtapi_task_pll_set_correction_hook(long);
+#endif
+
+int _rtapi_task_pll_set_correction(long value) {
+#ifdef HAVE_RTAPI_TASK_PLL_SET_CORRECTION_HOOK
+    return _rtapi_task_pll_set_correction_hook(value);
+#else
+    return 0;
+#endif
+}
+
+
 #endif  /* RTAPI */
 

src/rtapi/ulapi_autoload.c

@@ -116,6 +116,8 @@ static rtapi_switch_t dummy_ulapi_switch_struct = {
     .rtapi_clock_set_period = &_ulapi_dummy,
     .rtapi_delay = &_ulapi_dummy,
     .rtapi_delay_max = &_ulapi_dummy,
+    .rtapi_task_pll_get_reference = &_ulapi_dummy,
+    .rtapi_task_pll_set_correction = &_ulapi_dummy,
 
     .rtapi_get_time = (rtapi_get_time_t) &_ulapi_dummy,
     .rtapi_get_clocks = (rtapi_get_clocks_t) &_ulapi_dummy,

tests/pll_correction/checkresult

@@ -0,0 +1,37 @@
+#!/usr/bin/python
+
+import sys
+import os
+
+# Params from test.hal
+period = 1000000
+# Params from pll_correction.comp
+numsamps = 1000
+
+os.chdir(os.path.dirname(os.path.realpath(__file__)))
+
+with open("result", 'r') as f:
+    with open("stderr", 'a') as log:
+        for line in f:
+            line = line.rstrip('[ \n]')
+            (cycle_count, period_actual, pll_err, samp_avg, phase_diff) = (
+                [int(s) for s in line.split(' ')])
+
+            if cycle_count == 2*numsamps:
+                log.write("%s\n" % line)
+                if abs(samp_avg) > period/100:
+                    log.write("0 PLL didn't converge:  abs(%d) > %d/100\n" % (
+                        samp_avg, period))
+                    sys.exit(-1)
+                else:
+                    log.write("0 PLL converged:  abs(%d) <= %d/100\n" % (
+                        samp_avg, period))
+            if cycle_count == 4*numsamps:
+                log.write("%s\n" % line)
+                if abs(samp_avg) > period/100:
+                    log.write("10 PLL didn't converge:  abs(%d) > %d/100\n" % (
+                        samp_avg, period))
+                    sys.exit(-1)
+                else:
+                    log.write("10 PLL converged:  abs(%d) <= %d/100\n" % (
+                        samp_avg, period))

tests/pll_correction/pll_correction.comp

@@ -0,0 +1,88 @@
+component pll_correction;
+
+// Cumulative difference between expected and actual period
+pin out s32 pll_err = 0;
+// Actual period of this cycle
+pin out s32 period_actual;
+// expected cycle start time
+pin out u32 time_base_hi = 0;
+pin out u32 time_base_lo = 0;
+// cycle start time
+pin out u32 now_hi;
+pin out u32 now_lo;
+// previous cycle start time
+pin out u32 prev_hi;
+pin out u32 prev_lo;
+// PLL reference
+pin out u32 ref_hi;
+pin out u32 ref_lo;
+// Number of cycles
+pin out u32 cycle_count = 0;
+
+// Buffer for average
+variable int numsamps = 1000;
+variable int samps[1000];
+variable int samp_last;
+pin out s32 samp_avg;
+
+// PLL settings
+pin out bit pll_on;
+variable int pll_periods = 10; // Lock on to this many phases in the future
+// Monitor phase difference
+pin out s32 phase_diff;
+
+function _;
+license "GPL";
+;;
+
+// time_set(foo, bar) splits ull bar into foo_hi and foo_lo
+#define time_set(dst, src) do {                 \
+    dst ## _hi = (uint32_t) (src>>32);          \
+    dst ## _lo = (uint32_t) src;                \
+  } while (0)
+
+// time(foo) returns ull of joined foo_hi and foo_lo
+#define time(src)                                               \
+  ((unsigned long long)                                         \
+   ((((unsigned long long) src ## _hi) << 32) + src ## _lo))    \
+
+#define min(x,y) (x<y ? x : y)
+
+FUNCTION(_) {
+  // Get current time
+  time_set(now, rtapi_get_time());
+  if (cycle_count == 0) {
+    // at first cycle, set time_base and prev to now
+    time_set(time_base, time(now));
+    time_set(prev, time(now) - period);
+  } else
+    // increment time base by period
+    time_set(time_base, time(time_base) + period);
+
+  // Difference between expected and actual period start
+  period_actual = time(now) - time(prev);
+
+  // Phase difference from base
+  phase_diff = time(now) - time(time_base);
+
+  // Incremental phase error
+  pll_err = phase_diff - (
+    // Maintain 0 err in beginning, period*pll_periods later
+    !(pll_on = (cycle_count > numsamps*2)) ? 0 : period*pll_periods);
+
+  // Adjust phase
+  rtapi_task_pll_set_correction(-pll_err);
+
+  // Get reference (unused)
+  time_set(ref, rtapi_task_pll_get_reference());
+
+  // Averages
+  samps[(samp_last++)%numsamps] = pll_err;
+  samp_avg = 0;
+  for (int i=0; i<min(numsamps,samp_last); i++) samp_avg += samps[i];
+  samp_avg /= min(numsamps,samp_last);
+
+  // Updates for next cycle
+  time_set(prev, time(now));
+  cycle_count++;
+}

tests/pll_correction/pll_correction.hal

@@ -0,0 +1,27 @@
+
+loadrt threads name1=t period1=1000000
+
+# Load pc comp
+loadrt pll_correction names=pc
+addf pc t
+
+# Load sampler comp
+loadrt sampler cfg=ussss depth=2000
+addf sampler.0 t
+
+# Net pc signals to sampler
+net cycle-count pc.cycle-count => sampler.0.pin.0
+net period-actual pc.period-actual => sampler.0.pin.1
+net pll-err pc.pll-err => sampler.0.pin.2
+net samp-avg pc.samp-avg => sampler.0.pin.3
+net phase-diff pc.phase-diff => sampler.0.pin.4
+
+# Start threads and wait for userland sampler to exit
+start
+
+# Load userland sampler comp
+# - LinuxCNC
+#loadusr -Wn halsampler halsampler -N halsampler -n 100
+# - Machinekit
+loadusr -w halsampler -n 4000
+

tests/pll_correction/pll_time.h

@@ -0,0 +1 @@
+typedef long long int TIME;

tests/pll_correction/test.sh

@@ -0,0 +1,5 @@
+#!/bin/bash -e
+
+COMP="$(which halcompile || which comp)"
+$COMP --install pll_correction.comp 1>&2
+halrun -f pll_correction.hal