Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Tammo Jan Dijkema
dt_ctrl
Commits
eefd8984
Commit
eefd8984
authored
Dec 03, 2014
by
Jeroen Vreeken
Browse files
Add trigger section to control file.
Use immediate trigger in test.ctrl files. Fix some bugs
parent
966b1736
Changes
30
Hide whitespace changes
Inline
Side-by-side
controller/controller/controller_load.c
View file @
eefd8984
...
...
@@ -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
View file @
eefd8984
...
...
@@ -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
View file @
eefd8984
...
...
@@ -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
View file @
eefd8984
...
...
@@ -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
View file @
eefd8984
...
...
@@ -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
View file @
eefd8984
...
...
@@ -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
View file @
eefd8984
...
...
@@ -26,8 +26,6 @@
work is done.
*/
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
...
...
controller/dt_ctrl.ctrl
View file @
eefd8984
...
...
@@ -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
View file @
eefd8984
...
...
@@ -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
View file @
eefd8984
frequency 100
trigger {
{ "immediate" }
}
blocks {
{ "test_input_bool", "input_bool" }
{ "test_input_uint32", "input_uint32" }
...
...
Prev
1
2
Next
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment