Commit eefd8984 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add trigger section to control file.

Use immediate trigger in test.ctrl files.
Fix some bugs
parent 966b1736
......@@ -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);
......
......@@ -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);
......
......@@ -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; }
......
......@@ -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
......
......@@ -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;
}
......@@ -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_ */
......@@ -26,8 +26,6 @@
work is done.
*/
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
......
......@@ -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
......
......@@ -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>
......
frequency 100
trigger {
{ "immediate" }
}
blocks {
{ "test_input_bool", "input_bool" }
{ "test_input_uint32", "input_uint32" }
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment