Add trigger section to control file.

Use immediate trigger in test.ctrl files.
Fix some bugs

controller/controller/controller_load.c

@@ -243,6 +243,13 @@ void controller_load_frequency(double frequency)
 	controller_sample_frequency_set(frequency);
 }
 
+int controller_load_trigger(char *name, yyscan_t scanner)
+{
+	struct controller_load_extra *extra = yyget_extra(scanner);
+
+	return controller_sample_trigger(name, extra->va_list);
+}
+
 int controller_load_block_create(char *type, char *name, yyscan_t scanner)
 {
 	struct controller_load_extra *extra = yyget_extra(scanner);
@@ -312,7 +319,7 @@ int controller_load_import(char *file_name)
 	return r;
 }
 
-int controller_load_yy_input(char *buf, yy_size_t *readbytes, yy_size_t sizebytes, yyscan_t scanner)
+int controller_load_yy_input(char *buf, size_t *readbytes, size_t sizebytes, yyscan_t scanner)
 {
 	struct controller_load_extra *extra = yyget_extra(scanner);
 

controller/controller/controller_load_int.h

@@ -52,6 +52,7 @@ void controller_load_variable_string_set(char *varname, char *val);
 int controller_load_variable_int_get(char *varname);
 void controller_load_variable_int_set(char *varname, int i);
 
+int controller_load_trigger(char *name, yyscan_t scanner);
 int controller_load_block_create(char *type, char *name, yyscan_t scanner);
 int controller_load_block_param_set(char *block, char *param, yyscan_t scanner);
 

controller/controller/controller_load_parser.l

@@ -79,6 +79,7 @@ boolcast	"("[ \t]*"bool"[ \t]*")"
 "-"		{ return MINSYM; }
 
 "frequency"	{ return FREQUENCYSYM; }
+"trigger"	{ return TRIGGERSYM; }
 "blocks"	{ return BLOCKSSYM; }
 "links"		{ return LINKSSYM; }
 "traces"	{ return TRACESSYM; }

controller/controller/controller_load_parser.y

@@ -17,13 +17,14 @@
 
  */
 
-%code requires {
-typedef void*                 yyscan_t;
-}
+//%code requires {
+//typedef void*                 yyscan_t;
+//}
 
 %{
 
 #include <stdio.h>
+#include <math.h>
 #include <controller/controller_block.h>
 #include <controller/controller_load_int.h>
 #include <controller/controller_trace.h>
@@ -58,6 +59,7 @@ void yyerror(yyscan_t *scanner, char const *s);
 %token <dbl> DOUBLESYM
 %token <ul> UNSIGNEDLONGSYM
 %token FREQUENCYSYM
+%token TRIGGERSYM
 %token BLOCKSSYM
 %token LINKSSYM
 %token TRACESSYM
@@ -107,6 +109,7 @@ ctrllist: ctrl
 	;
 
 ctrl	: frequency
+	| triggers
 	| links
 	| blocks
 	| traces
@@ -140,6 +143,25 @@ import: IMPORTSYM STRINGSYM
 		}
 	;
 
+triggers: TRIGGERSYM BRACEOPENSYM triggerlist BRACECLOSESYM
+
+triggerlist: trigger
+	| trigger triggerlist
+	;
+
+trigger	: BRACEOPENSYM
+		STRINGSYM varlist
+		BRACECLOSESYM
+		{
+			if(controller_load_trigger($2, scanner)) {
+				yyerror(scanner, "Error in trigger");
+				YYERROR;
+			}
+			free($2);
+			controller_load_var_clear(scanner);
+		}
+
+
 blocks	: BLOCKSSYM BRACEOPENSYM blocklist BRACECLOSESYM
 
 blocklist: block

controller/controller/controller_sample.c

@@ -20,8 +20,7 @@
 	It also provides access to sample timing information.
 	
  */
-#define _GNU_SOURCE
- 
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
@@ -110,16 +109,6 @@ static void sample_timing_add(struct sample_timing *st, uint64_t t1, uint64_t t2
 	return;
 }
 
-static long sample_timing_diff_nsec(struct timespec *t1, struct timespec *t2)
-{
-	int diff;
-	
-	diff = t2->tv_nsec - t1->tv_nsec;
-	diff += (t2->tv_sec - t1->tv_sec) * 1000000000;
-	
-	return diff;
-}
-
 
 static ssize_t sample_timing_snprintf(struct sample_timing *st,
     char *dest, size_t n)
@@ -142,7 +131,10 @@ static int sample_timing_sane(uint64_t t_exp, uint64_t t_act)
 		n_diff = t_act - t_exp;
 	
 	if (n_diff > nsec_interval * 10) {
-		return 1;
+		log_send(LOG_T_ERROR, 
+		    "Time sanity check triggered: %" PRIu64 " nsec difference",
+		    t_act - t_exp);
+		return -1;
 	}
 
 	return 0;
@@ -228,6 +220,11 @@ static void cpu_bind(void)
 	    nr_set, nr_failed);
 }
 
+static uint64_t timestamp_from_timespec(struct timespec *t)
+{
+	return (uint64_t)t->tv_sec * 1000000000 + (uint64_t)t->tv_nsec;
+}
+
 static clockid_t sample_clock;
 #ifdef __FreeBSD__
 static pthread_condattr_t attr;
@@ -235,8 +232,25 @@ static pthread_mutex_t lock;
 static pthread_cond_t cv;
 #endif
 
-static void wait_init(void)
+static uint64_t timestamp(void)
+{
+	struct timespec t;
+	
+	clock_gettime(sample_clock, &t);
+
+	return timestamp_from_timespec(&t);
+}
+
+
+
+static void wait_init_default(struct timespec *t)
 {
+	clock_gettime(sample_clock, t);
+	
+	t->tv_nsec += nsec_interval - 1;
+	t->tv_nsec -= t->tv_nsec % nsec_interval;
+	tsnorm(t);
+
 #ifndef __FreeBSD__
 	sample_clock = CLOCK_REALTIME;
 #else
@@ -251,46 +265,66 @@ static void wait_init(void)
 #endif
 }
 
-static long prestart = 0;
-static long prestart_max;
+static void wait_next_default(struct timespec *t)
+{
+	uint64_t tnow = timestamp();
+
+	if (sample_timing_sane(controller_time_nseconds, tnow)) {
+		clock_gettime(sample_clock, t);
+		t->tv_nsec -= (t->tv_nsec % nsec_interval);
+		log_send(LOG_T_WARNING,
+		    "Using current time for next sampe: %lld.%09ld",
+		    (long long)t->tv_sec, t->tv_nsec);
+	}
+
+	t->tv_nsec += nsec_interval;
+	tsnorm(t);
+	
+#ifndef __FreeBSD__
+	clock_nanosleep(sample_clock, TIMER_ABSTIME, t, NULL);
+#else
+	pthread_cond_timedwait(&cv, &lock, t);
+#endif
+}
+
+
+static uint64_t prestart = 0;
 
-static void wait_next(struct timespec *t)
+static void wait_next_prestart(struct timespec *t)
 {
 #ifndef __FreeBSD__
 	struct timespec tn;
-	long diff;
+	uint64_t tnow = timestamp();
+
+	if (sample_timing_sane(controller_time_nseconds, tnow)) {
+		clock_gettime(sample_clock, t);
+		t->tv_nsec -= (t->tv_nsec % nsec_interval);
+		log_send(LOG_T_WARNING,
+		    "Using current time for next sampe: %lld.%09ld",
+		    (long long)t->tv_sec, t->tv_nsec);
+	}
+	
+	t->tv_nsec += nsec_interval;
 	
 	tn.tv_sec = t->tv_sec;
 	tn.tv_nsec = t->tv_nsec;
-	tssubtract(&tn, prestart);
+	tn.tv_nsec += prestart;
+
+	tsnorm(t);
+	tsnorm(&tn);
 	
 	clock_nanosleep(sample_clock, TIMER_ABSTIME, &tn, NULL);
 
 	clock_gettime(sample_clock, &tn);
-	diff = sample_timing_diff_nsec(t, &tn);
-	if (diff > 0)
-		prestart += diff;
-	else 
-		prestart += diff / 100;
-	if (prestart > prestart_max)
-		prestart = prestart_max;
 
 	while (tn.tv_sec < t->tv_sec || tn.tv_nsec < t->tv_nsec) {
 		clock_gettime(sample_clock, &tn);
 	}
-#else
-	pthread_cond_timedwait(&cv, &lock, t);
 #endif
 }
 
-static uint64_t timestamp(void)
-{
-	struct timespec t;
-	
-	clock_gettime(sample_clock, &t);
-
-	return (uint64_t)t.tv_sec * 1000000000 + (uint64_t)t.tv_nsec;
-}
+static void (*wait_init)(struct timespec *) = wait_init_default;
+static void (*wait_next)(struct timespec *) = wait_next_default;
 
 /*
 	This thread should be realtime....
@@ -330,31 +364,21 @@ static void *sample_thread(void *arg)
 		    strerror(errno));
 	}
 
-	wait_init();
-
-	clock_gettime(sample_clock, &t);
-	
-	t.tv_nsec += nsec_interval - 1;
-	t.tv_nsec -= t.tv_nsec % nsec_interval;
-	tsnorm(&t);
+	wait_init(&t);
 
 	while (1) {
 		/* Pre sample stuff: */
 		controller_samplenr++;
 		
-		t.tv_nsec += nsec_interval;
-		tsnorm(&t);
-	
-		t_trig = (uint64_t)t.tv_sec * 1000000000;
-		t_trig += t.tv_nsec;
-		controller_time_nseconds = t_trig;
-		controller_time_seconds = t.tv_sec;
-		controller_time_samplenr = t.tv_nsec / nsec_interval;
-
 		/* Wait for the right moment... 
 		   (Internal timer or external interrupt) */
 		wait_next(&t);
 
+		t_trig = timestamp_from_timespec(&t);
+		controller_time_nseconds = t_trig;
+		controller_time_seconds = t.tv_sec;
+		controller_time_samplenr = t.tv_nsec / nsec_interval;
+
 		t_start = timestamp();
 
 		/* Do sample stuff */
@@ -370,21 +394,12 @@ static void *sample_thread(void *arg)
 		if (t_end - t_trig > nsec_interval) {
 			controller_sampleoverruns++;
 		}
-		if (sample_timing_sane(t_trig, t_start)) {
-			log_send(LOG_T_ERROR, 
-			    "Time sanity check triggered: %" PRIu64 " nsec difference",
-			    t_start - t_trig);
-			clock_gettime(sample_clock, &t);
-			t.tv_nsec -= (t.tv_nsec % nsec_interval);
-			log_send(LOG_T_WARNING,
-			    "Using current time for next sampe: %lld.%09ld",
-			    (long long)t.tv_sec, t.tv_nsec);
-		} else {
-			sample_timing_add(&st_start, t_trig, t_start);
-			sample_timing_add(&st_io, t_start, t_io);
-			sample_timing_add(&st_end, t_trig, t_end);
-			sample_timing_add(&st_sample, t_start, t_end);
-		}
+
+		sample_timing_add(&st_start, t_trig, t_start);
+		sample_timing_add(&st_io, t_start, t_io);
+		sample_timing_add(&st_end, t_trig, t_end);
+		sample_timing_add(&st_sample, t_start, t_end);
+
 		alarm(1);
 	}
 	return NULL;
@@ -417,7 +432,6 @@ static void *sample_monitor(void *arg)
 		log_send(LOG_T_DEBUG, linebuf);
 		sample_timing_snprintf(&st_sample, linebuf, 200);
 		log_send(LOG_T_DEBUG, linebuf);
-		log_send(LOG_T_DEBUG, "prestart: %ld", prestart);
 		sleep(1);
 	}
 	return NULL;
@@ -509,7 +523,8 @@ int controller_sample_start(void)
 	int i;
 
 	nsec_interval = 1000000000.0 / controller_frequency;
-	prestart_max = nsec_interval / 10;
+	if (-prestart > nsec_interval / 10)
+		prestart = -nsec_interval / 10;
 
 	log_send(LOG_T_DEBUG, "nsec_interval: %d %f", 
 	    nsec_interval, controller_frequency);
@@ -531,3 +546,71 @@ int controller_sample_start(void)
 
 	return 0;
 }
+
+
+void wait_next_immediate(struct timespec *t)
+{
+	t->tv_nsec += nsec_interval;
+	tsnorm(t);
+}
+
+
+static int trigger_default(va_list ap)
+{
+	log_send(LOG_T_DEBUG, "Default trigger");
+
+	wait_init = wait_init_default;
+	wait_next = wait_next_default;
+
+	return 0;
+}
+
+static int trigger_prestart(va_list ap)
+{
+	double prestart_d;
+	
+	prestart_d = va_arg(ap, double);
+	prestart = -((1000000000.0 * prestart_d) + 0.5);
+	
+	log_send(LOG_T_DEBUG, "Prestart trigger: %g sec (%" PRId64 " nsec)",
+	    prestart_d, -prestart);
+
+	wait_init = wait_init_default;
+	wait_next = wait_next_prestart;
+
+	return 0;
+}
+
+static int trigger_immediate(va_list ap)
+{
+	log_send(LOG_T_DEBUG, "Immediate trigger");
+	
+	wait_init = wait_init_default;
+	wait_next = wait_next_immediate;
+	
+	return 0;
+}
+
+
+static struct {
+	char *name;
+	int (*func)(va_list ap);
+} triggers[] = {
+	{ "default", trigger_default },
+	{ "prestart", trigger_prestart },
+	{ "immediate", trigger_immediate },
+};
+
+int controller_sample_trigger(char *name, va_list ap)
+{
+	int i;
+	
+	for (i = 0; i < sizeof(triggers)/sizeof(triggers[0]); i++) {
+		if (!strcmp(triggers[i].name, name))
+			return triggers[i].func(ap);
+	}
+
+	log_send(LOG_T_ERROR, "No trigger '%s' available", name);
+
+	return -1;
+}

controller/controller/controller_sample.h

@@ -35,5 +35,7 @@ bool controller_sample_running(void);
 
 int controller_sample_shell_add(void);
 
+int controller_sample_trigger(char *name, va_list ap);
+
 
 #endif /* _INCLUDE_CONTROLLER_SAMPLE_ */

controller/dt_ctrl.c

@@ -26,8 +26,6 @@
 	work is done.
  */
 
-#define _GNU_SOURCE
-
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>

controller/dt_ctrl.ctrl

@@ -10,6 +10,10 @@
 # Frequency is limited by the stoeber drives.
 frequency 250
 
+trigger {
+	{ "prestart", 0.000150 }
+}
+
 # dt_ctrl_el.ctrl	Elevation with real servo drives
 # dt_ctrl_el_sim.ctrl	Elevation with simulated network
 # dt_ctrl_az.ctrl	Azimuth with real servo drives

controller/ec/esc_coe.c

@@ -22,7 +22,6 @@
 	These functions implement a canopen device (which can be used with
 	the canopen functions) based on a ethercat mailbox.
  */
-#define _GNU_SOURCE
 
 #include <string.h>
 #include <limits.h>

controller/test/test.ctrl

 frequency 100
 
+trigger {
+	{ "immediate" }
+}
+
 blocks {
 	{ "test_input_bool",    "input_bool"    }
 	{ "test_input_uint32",  "input_uint32"  }