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
8d1a320c
Commit
8d1a320c
authored
May 14, 2014
by
Jeroen Vreeken
Browse files
Merge branch 'jeroen' into controller_bus
parents
e8a77b98
6606002f
Changes
8
Hide whitespace changes
Inline
Side-by-side
common/utils/Makefile
View file @
8d1a320c
...
...
@@ -11,7 +11,7 @@ LIBUTILOBJS= $(UTILSRCS:.c=.lo)
all
:
$(LIBUTILOBJS) weather_test libutils.la libutils.la_install
libutils.la_LDFLAGS
=
-lm
-rpath
${CURDIR}
/../lib
libutils.la_LDFLAGS
=
-lm
-rpath
${CURDIR}
/../lib
-lrt
libutils.la
:
$(LIBUTILOBJS)
${CURDIR}/../lib
:
...
...
console/console/Makefile
View file @
8d1a320c
...
...
@@ -8,7 +8,7 @@ CFLAGS= -Wall -O3 -I../../common/utils \
-I
../../common/log
\
-I
../../include
LDFLAGS
+=
-L
../../common/lib/
-L
../../lib/
-L
./lib
-lutils
-ltrace
-static
-llog
LDFLAGS
+=
-L
../../common/lib/
-L
../../lib/
-L
./lib
-lutils
-ltrace
-llog
LIBNOVA
=
-lnova
-L
../../lib/
...
...
controller/block/Makefile
View file @
8d1a320c
...
...
@@ -70,7 +70,7 @@ il2cdir:
$(BLOCKSRCS)
:
il2cdir
libblock.la_LDFLAGS
=
-rpath
${CURDIR}
/../lib
libblock.la_LDFLAGS
=
-rpath
${CURDIR}
/../lib
-lm
libblock.la
:
$(BLOCKS)
libblock.la_install
:
libblock.la
...
...
controller/block/block_setpoint_generator.c
View file @
8d1a320c
...
...
@@ -97,6 +97,10 @@ struct controller_block_private {
/* conversion factor for seconds/tick and its inverse */
float
tick
;
/* seconds per tick */
float
freq
;
/* ticks per second */
float
freq2
;
float
freq3
;
double
t_max_a
;
double
v_delta_from_max_a
;
/* parameters in real world format (time unit: second) */
float
max_v_sec
;
...
...
@@ -107,6 +111,13 @@ struct controller_block_private {
float
precision_v_sec
;
float
precision_a_sec
;
/* state at last change */
double
start_x
;
double
start_v
;
double
start_a
;
double
start_j
;
int
start_t
;
/* current internal state */
double
cur_x
;
double
cur_v
;
...
...
@@ -151,8 +162,8 @@ static double ticks_to_v(struct controller_block_private *priv,
double
t
;
/* From a constant speed to a constant speed is done in two halves:
First halve (untill half the speed difference is reached is
done
at max jerk, second half is done at -max jerk.
First halve (untill half the speed difference is reached
)
is
done
at max jerk, second half is done at -max jerk.
1/2 v = 1/2 j t^2 -> v = j t^2 -> t = sqrt(v/j)
...
...
@@ -203,22 +214,36 @@ static double x_after_ticks(struct controller_block_private *priv,
return
x_at_t
;
}
static
void
calculate
(
struct
controller_block
*
spg
)
static
void
setpoint_generator_
calculate
(
struct
controller_block
*
spg
)
{
struct
controller_block_private
*
priv
=
spg
->
private
;
double
cur_x
,
cur_v
;
double
t_max_a
;
bool
ignore_x
=
false
;
bool
must_brake
=
false
;
bool
good_x
;
bool
good_v
;
cur_x
=
priv
->
cur_x
;
cur_v
=
priv
->
cur_v
;
t_max_a
=
priv
->
t_max_a
;
if
(
*
priv
->
reset
)
{
priv
->
cmd_x
=
*
priv
->
reset_x
;
priv
->
cur_x
=
priv
->
cmd_x
;
cur_x
=
priv
->
cmd_x
;
priv
->
current_command
.
done
=
1
;
priv
->
current_command
.
type
=
BLOCK_SPG_SETPOINT
;
priv
->
next_command
.
done
=
1
;
priv
->
cmd_v
=
0
.
0
;
priv
->
cur_v
=
0
.
0
;
cur_v
=
0
.
0
;
priv
->
cur_a
=
0
.
0
;
priv
->
cur_j
=
0
.
0
;
priv
->
start_x
=
cur_x
;
priv
->
start_v
=
0
.
0
;
priv
->
start_a
=
0
.
0
;
priv
->
start_j
=
0
.
0
;
priv
->
start_t
=
0
;
}
if
(
priv
->
current_command
.
done
)
{
...
...
@@ -266,7 +291,7 @@ static void calculate(struct controller_block *spg)
}
else
{
priv
->
cmd_v
=
(
priv
->
current_command
.
setpoint
-
priv
->
cmd_x
)
/
t
;
priv
->
cmd_x
)
/
(
t
+
1
)
;
priv
->
current_command
.
start
=
1
;
}
}
...
...
@@ -291,9 +316,7 @@ static void calculate(struct controller_block *spg)
if
(
priv
->
current_command
.
type
==
BLOCK_SPG_SPEED
)
{
ignore_x
=
true
;
priv
->
cmd_x
=
priv
->
cur_x
;
}
else
{
priv
->
cmd_x
+=
priv
->
cmd_v
;
priv
->
cmd_x
=
cur_x
;
}
if
(
priv
->
cmd_x
>
priv
->
max_x
)
...
...
@@ -301,13 +324,20 @@ static void calculate(struct controller_block *spg)
if
(
priv
->
cmd_x
<
priv
->
min_x
)
priv
->
cmd_x
=
priv
->
min_x
;
if
(
!
almost_equal
(
priv
->
cur_v
,
priv
->
cmd_v
)
||
!
almost_equal
(
priv
->
cur_x
,
priv
->
cmd_x
))
{
double
error_x
=
priv
->
cmd_x
-
priv
->
cur_x
;
double
error_v
=
priv
->
cmd_v
-
priv
->
cur_v
;
good_x
=
almost_equal
(
cur_x
,
priv
->
cmd_x
);
good_v
=
almost_equal
(
cur_v
,
priv
->
cmd_v
);
if
(
!
good_v
||
!
good_x
)
{
double
error_x
;
double
req_x_1
;
double
error_x_jpos
;
double
error_x_j0
;
double
error_x_jneg
;
double
error_v
=
priv
->
cmd_v
-
cur_v
;
double
x
,
v
,
a
,
j
,
t
;
double
req_x
,
req_v
,
t_max_a
,
v_delta_from_max_a
;
double
req_x
,
req_v
,
v_delta_from_max_a
;
double
error_v_after_a
,
error_x_at_v
;
double
j_from_pos
;
bool
state_at_max_a
=
false
;
bool
state_to_max_a
=
false
;
...
...
@@ -315,19 +345,34 @@ static void calculate(struct controller_block *spg)
requested speed.
*/
x
=
priv
->
cur_x
;
v
=
priv
->
cur_v
;
x
=
cur_x
;
v
=
cur_v
;
a
=
priv
->
cur_a
;
req_x
=
priv
->
cmd_x
;
req_v
=
priv
->
cmd_v
;
/* Calculate delta v when comming from max_a */
t_max_a
=
ticks_to_a
(
priv
,
0
,
priv
->
max_a
);
v_delta_from_max_a
=
v_after_ticks
(
priv
,
0
,
0
,
priv
->
max_j
,
t_max_a
);
req_x_1
=
req_x
+
req_v
;
error_x_jpos
=
req_x_1
-
x_after_ticks
(
priv
,
x
,
v
,
a
,
priv
->
max_j
,
1
);
error_x_j0
=
req_x_1
-
x_after_ticks
(
priv
,
x
,
v
,
a
,
0
,
1
);
error_x_jneg
=
req_x_1
-
x_after_ticks
(
priv
,
x
,
v
,
a
,
-
priv
->
max_j
,
1
);
if
(
fabs
(
error_x_jpos
)
<
fabs
(
error_x_jneg
))
{
error_x
=
error_x_jpos
;
j_from_pos
=
priv
->
max_j
;
}
else
{
error_x
=
error_x_jneg
;
j_from_pos
=
-
priv
->
max_j
;
}
if
(
fabs
(
error_x_j0
)
<
fabs
(
error_x
))
{
error_x
=
error_x_j0
;
j_from_pos
=
0
;
}
v_delta_from_max_a
=
priv
->
v_delta_from_max_a
;
j
=
copysign
(
priv
->
max_j
,
error_v
);
if
(
fabs
(
a
)
>
priv
->
max_j
)
{
if
(
fabs
(
a
)
>
=
priv
->
max_j
)
{
/* Not at constant velocity */
if
(
signbit
(
a
)
!=
signbit
(
error_v
))
{
...
...
@@ -362,6 +407,7 @@ static void calculate(struct controller_block *spg)
t_a_to_0
=
ticks_to_a
(
priv
,
a
,
0
);
}
else
if
(
fabs
(
a_peak
)
>
priv
->
max_a
)
{
/* We are going to hit maximum acc */
a_peak
=
copysign
(
priv
->
max_a
,
a_peak
);
t
=
ticks_to_a
(
priv
,
0
,
a_peak
);
...
...
@@ -394,6 +440,7 @@ static void calculate(struct controller_block *spg)
if
(
t_to_max_a
>=
1
.
0
)
{
state_to_max_a
=
true
;
/* accelerate to max a */
x
=
x_after_ticks
(
priv
,
x
,
v
,
a
,
j
,
t_to_max_a
);
v
=
v_after_ticks
(
priv
,
v
,
a
,
j
,
t_to_max_a
);
a
=
a_after_ticks
(
priv
,
a
,
j
,
t_to_max_a
);
...
...
@@ -402,9 +449,9 @@ static void calculate(struct controller_block *spg)
state_at_max_a
=
true
;
}
}
/* decellerate to a==0 */
x
=
x_after_ticks
(
priv
,
x
,
v
,
a
,
-
j
,
t_a_to_0
);
v
=
v_after_ticks
(
priv
,
v
,
a
,
-
j
,
t_a_to_0
);
a
=
a_after_ticks
(
priv
,
a
,
-
j
,
t_a_to_0
);
req_x
=
req_x
+
req_v
*
t_a_to_0
;
a
=
0
;
}
...
...
@@ -427,7 +474,7 @@ static void calculate(struct controller_block *spg)
x
=
x_after_ticks
(
priv
,
x
,
v
,
0
,
j
,
t_max_a
);
v
=
v_after_ticks
(
priv
,
v
,
0
,
j
,
t_max_a
);
a
=
a_after_ticks
(
priv
,
0
,
j
,
t_
max_a
);
a
=
copysign
(
priv
->
max_a
,
j
);
req_x
=
req_x
+
req_v
*
t_max_a
;
t_at_max_a
=
(
fabs
(
error_v_after_a
)
-
v_delta_from_max_a
*
2
)
*
priv
->
inv_max_a
;
...
...
@@ -439,7 +486,6 @@ static void calculate(struct controller_block *spg)
x
=
x_after_ticks
(
priv
,
x
,
v
,
a
,
-
j
,
t_max_a
);
v
=
v_after_ticks
(
priv
,
v
,
a
,
-
j
,
t_max_a
);
req_x
=
req_x
+
req_v
*
t_max_a
;
a
=
a_after_ticks
(
priv
,
a
,
-
j
,
t_max_a
);
a
=
0
;
}
else
{
/* Simple profile: triangle
...
...
@@ -463,11 +509,13 @@ static void calculate(struct controller_block *spg)
error_x_at_v
=
req_x
-
x
;
if
(
fabs
(
error_x_at_v
)
<
fabs
(
error_x
)
||
ignore_x
)
{
if
(
fabs
(
error_x_at_v
)
>
req_v
&&
!
ignore_x
&&
if
(
fabs
(
error_x_at_v
)
<
fabs
(
error_x
)
||
(
signbit
(
error_x
)
!=
signbit
(
error_x_at_v
)
&&
!
state_to_max_a
&&
!
state_at_max_a
)
||
ignore_x
)
{
if
(
!
ignore_x
&&
signbit
(
error_x_at_v
)
==
signbit
(
error_x
)
)
{
/* position got better, but not yet enough */
priv
->
cur_j
=
copysign
(
priv
->
max_j
,
error_x
);
priv
->
cur_j
=
j_from_pos
;
}
else
{
if
(
state_to_max_a
)
{
priv
->
cur_j
=
j
;
...
...
@@ -481,17 +529,18 @@ static void calculate(struct controller_block *spg)
/* going to requested speed would make position error
bigger, first go to position.
*/
priv
->
cur_j
=
copysign
(
priv
->
max_j
,
error_x
)
;
priv
->
cur_j
=
j_from_pos
;
}
}
/* When moving can we brake before the position limits? */
if
(
fabs
(
priv
->
cur_v
)
>
0
.
0
)
{
if
(
fabs
(
cur_v
)
>
0
.
0
)
{
double
t
,
x
,
v
,
a
,
j
;
bool
done
=
false
;
x
=
priv
->
cur_x
;
v
=
priv
->
cur_v
;
x
=
cur_x
;
v
=
cur_v
;
a
=
priv
->
cur_a
;
/* add 1 tick, we want to know if we are still safe untill the next
...
...
@@ -541,7 +590,7 @@ static void calculate(struct controller_block *spg)
/* Assuming we have a constant v,
check if we have to start deceleration now. */
t
=
ticks_to_v
(
priv
,
priv
->
cur_v
,
0
);
t
=
ticks_to_v
(
priv
,
cur_v
,
0
);
/* will we hit max a? */
if
(
fabs
(
a_after_ticks
(
priv
,
a
,
j
,
t
/
2
))
>
priv
->
max_a
)
{
...
...
@@ -549,7 +598,7 @@ static void calculate(struct controller_block *spg)
double
v_start_3
,
t_2
;
t
=
t
icks_to_a
(
priv
,
0
,
priv
->
max_a
)
;
t
=
t
_
max_a
;
x
=
x_after_ticks
(
priv
,
x
,
v
,
a
,
j
,
t
);
v
=
v_after_ticks
(
priv
,
v
,
a
,
j
,
t
);
...
...
@@ -585,10 +634,11 @@ static void calculate(struct controller_block *spg)
}
}
/* If accelerating, can we decelerate before going beyond our max vel */
if
(
fabs
(
priv
->
cur_a
)
>
0
.
0
)
{
double
t
,
v
,
a
,
j
;
v
=
priv
->
cur_v
;
v
=
cur_v
;
a
=
priv
->
cur_a
;
/* add 1 tick, we want to know if we are still safe untill the next
...
...
@@ -615,58 +665,102 @@ static void calculate(struct controller_block *spg)
}
}
if
(
!
ignore_x
)
{
priv
->
cmd_x
+=
priv
->
cmd_v
;
}
/* Is the difference between spg and command small enough?
If so, make outputs equal to command */
if
(
!
must_brake
&&
fabs
(
priv
->
cmd_x
-
priv
->
cur_x
)
<
priv
->
precision_x
&&
fabs
(
priv
->
cmd_v
-
priv
->
cur_v
)
<
priv
->
precision_v
&&
fabs
(
priv
->
cmd_x
-
cur_x
)
<
priv
->
precision_x
&&
fabs
(
priv
->
cmd_v
-
cur_v
)
<
priv
->
precision_v
&&
fabs
(
priv
->
cur_a
)
<
priv
->
precision_a
)
{
priv
->
cur_j
=
0
.
0
;
priv
->
cur_a
=
0
.
0
;
priv
->
cur_v
=
priv
->
cmd_v
;
priv
->
cur_x
=
priv
->
cmd_x
;
cur_v
=
priv
->
cmd_v
;
cur_x
=
priv
->
cmd_x
;
}
priv
->
cur_a
+=
priv
->
cur_j
;
/* new jerk? */
if
(
!
almost_equal
(
priv
->
cur_j
,
priv
->
start_j
))
{
priv
->
start_j
=
priv
->
cur_j
;
priv
->
start_a
=
priv
->
cur_a
;
priv
->
start_v
=
cur_v
;
priv
->
start_x
=
cur_x
;
priv
->
start_t
=
0
;
}
priv
->
start_t
++
;
priv
->
cur_a
=
a_after_ticks
(
priv
,
priv
->
start_a
,
priv
->
start_j
,
priv
->
start_t
);
cur_v
=
v_after_ticks
(
priv
,
priv
->
start_v
,
priv
->
start_a
,
priv
->
start_j
,
priv
->
start_t
);
cur_x
=
x_after_ticks
(
priv
,
priv
->
start_x
,
priv
->
start_v
,
priv
->
start_a
,
priv
->
start_j
,
priv
->
start_t
);
if
(
fabs
(
priv
->
cur_a
)
>
priv
->
max_a
)
{
priv
->
cur_a
=
copysign
(
priv
->
max_a
,
priv
->
cur_a
);
priv
->
cur_j
=
0
;
}
priv
->
cur_v
+=
priv
->
cur_a
;
priv
->
start_j
=
priv
->
cur_j
;
priv
->
start_a
=
priv
->
cur_a
;
priv
->
start_v
=
cur_v
;
priv
->
start_x
=
cur_x
;
priv
->
start_t
=
0
;
}
if
(
fabs
(
priv
->
cur_v
)
>=
priv
->
max_v
)
{
if
(
fabs
(
cur_v
)
>=
priv
->
max_v
)
{
/* prevent further acceleration beyond max v */
if
(
signbit
(
priv
->
cur_v
)
==
signbit
(
priv
->
cur_a
))
{
if
(
signbit
(
cur_v
)
==
signbit
(
priv
->
cur_a
))
{
priv
->
cur_a
=
0
.
0
;
}
if
(
signbit
(
priv
->
cur_v
)
==
signbit
(
priv
->
cur_j
))
{
if
(
signbit
(
cur_v
)
==
signbit
(
priv
->
cur_j
))
{
priv
->
cur_j
=
0
.
0
;
}
priv
->
cur_v
=
copysign
(
priv
->
max_v
,
priv
->
cur_v
);
cur_v
=
copysign
(
priv
->
max_v
,
cur_v
);
priv
->
start_j
=
priv
->
cur_j
;
priv
->
start_a
=
priv
->
cur_a
;
priv
->
start_v
=
cur_v
;
priv
->
start_x
=
cur_x
;
priv
->
start_t
=
0
;
}
priv
->
cur_x
+=
priv
->
cur_v
;
if
(
priv
->
cur_x
>
priv
->
max_x
)
{
priv
->
cur_x
=
priv
->
max_x
;
priv
->
cur_v
=
0
;
if
(
cur_x
>
priv
->
max_x
)
{
cur_x
=
priv
->
max_x
;
cur_v
=
0
;
priv
->
cur_a
=
0
;
priv
->
cur_j
=
0
;
priv
->
start_j
=
priv
->
cur_j
;
priv
->
start_a
=
priv
->
cur_a
;
priv
->
start_v
=
cur_v
;
priv
->
start_x
=
cur_x
;
priv
->
start_t
=
0
;
}
if
(
priv
->
cur_x
<
priv
->
min_x
)
{
priv
->
cur_x
=
priv
->
min_x
;
priv
->
cur_v
=
0
;
if
(
cur_x
<
priv
->
min_x
)
{
cur_x
=
priv
->
min_x
;
cur_v
=
0
;
priv
->
cur_a
=
0
;
priv
->
cur_j
=
0
;
priv
->
start_j
=
priv
->
cur_j
;
priv
->
start_a
=
priv
->
cur_a
;
priv
->
start_v
=
cur_v
;
priv
->
start_x
=
cur_x
;
priv
->
start_t
=
0
;
}
priv
->
cur_x_out
=
priv
->
cur_x
;
priv
->
cur_v_out
=
priv
->
cur_v
*
priv
->
freq
;
priv
->
cur_a_out
=
priv
->
cur_a
*
priv
->
freq
*
priv
->
freq
;
priv
->
cur_j_out
=
priv
->
cur_j
*
priv
->
freq
*
priv
->
freq
*
priv
->
freq
;
priv
->
cur_x_out
=
cur_x
;
priv
->
cur_v_out
=
cur_v
*
priv
->
freq
;
priv
->
cur_a_out
=
priv
->
cur_a
*
priv
->
freq
2
;
priv
->
cur_j_out
=
priv
->
cur_j
*
priv
->
freq
3
;
priv
->
cmd_x_out
=
priv
->
cmd_x
;
priv
->
cur_x
=
cur_x
;
priv
->
cur_v
=
cur_v
;
}
static
bool
block_setpoint_generator_queue_space
(
struct
controller_block
*
spg
)
...
...
@@ -763,6 +857,8 @@ static void param_get(struct controller_block *spg, int param, void *val)
static
void
param_set
(
struct
controller_block
*
spg
,
int
param
,
va_list
val
)
{
double
t_max_a
;
switch
(
param
)
{
case
0
:
spg
->
private
->
cmd_x
=
va_arg
(
val
,
double
);
...
...
@@ -811,8 +907,16 @@ static void param_set(struct controller_block *spg, int param, va_list val)
spg
->
private
->
precision_a
=
spg
->
private
->
precision_a_sec
*
spg
->
private
->
tick
*
spg
->
private
->
tick
;
spg
->
private
->
freq
=
1
.
0
/
spg
->
private
->
tick
;
spg
->
private
->
freq2
=
spg
->
private
->
freq
*
spg
->
private
->
freq
;
spg
->
private
->
freq3
=
spg
->
private
->
freq2
*
spg
->
private
->
freq
;
spg
->
private
->
inv_max_j
=
1
.
0
/
spg
->
private
->
max_j
;
spg
->
private
->
inv_max_a
=
1
.
0
/
spg
->
private
->
max_a
;
/* Calculate delta v when comming from max_a */
t_max_a
=
ticks_to_a
(
spg
->
private
,
0
,
spg
->
private
->
max_a
);
spg
->
private
->
v_delta_from_max_a
=
v_after_ticks
(
spg
->
private
,
0
,
0
,
spg
->
private
->
max_j
,
t_max_a
);
spg
->
private
->
t_max_a
=
t_max_a
;
}
static
struct
controller_block_interm_list
interms
[]
=
{
...
...
@@ -876,6 +980,8 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
spg
->
private
->
max_a_sec
=
0
.
0
;
spg
->
private
->
max_j_sec
=
0
.
0
;
spg
->
private
->
freq
=
1
.
0
;
spg
->
private
->
freq2
=
1
.
0
;
spg
->
private
->
freq3
=
1
.
0
;
spg
->
private
->
precision_x
=
0
.
0
;
spg
->
private
->
precision_v
=
0
.
0
;
spg
->
private
->
precision_a
=
0
.
0
;
...
...
@@ -887,6 +993,11 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
spg
->
private
->
cur_v
=
0
.
0
;
spg
->
private
->
cur_a
=
0
.
0
;
spg
->
private
->
cur_j
=
0
.
0
;
spg
->
private
->
start_x
=
0
.
0
;
spg
->
private
->
start_v
=
0
.
0
;
spg
->
private
->
start_a
=
0
.
0
;
spg
->
private
->
start_j
=
0
.
0
;
spg
->
private
->
start_t
=
0
;
spg
->
private
->
cur_x_out
=
0
.
0
;
spg
->
private
->
cur_v_out
=
0
.
0
;
spg
->
private
->
cur_a_out
=
0
.
0
;
...
...
@@ -899,7 +1010,7 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
if
(
controller_block_outterm_list_init
(
spg
,
outterms
))
goto
err_input
;
spg
->
calculate
=
calculate
;
spg
->
calculate
=
setpoint_generator_
calculate
;
if
(
controller_block_param_list_init
(
spg
,
params
))
goto
err_output
;
...
...
controller/controller/Makefile
View file @
8d1a320c
...
...
@@ -28,7 +28,7 @@ controller_load_parser.tab.lo: controller_load_parser.yy.c
controller_load.lo
:
controller_load_parser.tab.h controller_load_parser.yy.h
libcontroller.la_LDFLAGS
=
-rpath
${CURDIR}
/../lib
-lshell
libcontroller.la_LDFLAGS
=
-rpath
${CURDIR}
/../lib
-lshell
-ldl
-lpthread
-lrt
libcontroller.la
:
$(CONTROLLER)
libcontroller.la_install
:
libcontroller.la
...
...
controller/controller/controller_sample.c
View file @
8d1a320c
...
...
@@ -29,6 +29,7 @@
#include <string.h>
#include <sys/mman.h>
#include <errno.h>
#include <signal.h>
#include "controller_block.h"
#include "controller_sample.h"
...
...
@@ -125,6 +126,34 @@ static ssize_t sample_timing_snprintf(struct sample_timing *st,
}
static
int
sample_timing_sane
(
struct
timespec
*
t_exp
,
struct
timespec
*
t_act
)
{
long
s_diff
;
long
n_diff
;
s_diff
=
t_act
->
tv_sec
-
t_exp
->
tv_sec
;
if
(
s_diff
<
0
)
{
return
-
1
;
}
if
(
s_diff
>
1
)
{
return
1
;
}
n_diff
=
s_diff
*
1000
*
1000
*
1000
;
n_diff
+=
t_act
->
tv_nsec
-
t_exp
->
tv_nsec
;
if
(
n_diff
>
nsec_interval
*
10
)
{
return
1
;
}
return
0
;
}
static
void
alarm_handler
(
int
d
)
{
log_send
(
LOG_T_ERROR
,
"Sample has been inactive to long!"
);
}
/*
This thread should be realtime....
*/
...
...
@@ -142,6 +171,16 @@ static void *sample_thread(void *arg)
pthread_mutex_t
lock
;
pthread_cond_t
cv
;
#endif
sigset_t
sigset
;
struct
sigaction
sigact
;
sigemptyset
(
&
sigset
);
sigaddset
(
&
sigset
,
SIGALRM
);
sigprocmask
(
SIG_UNBLOCK
,
&
sigset
,
NULL
);
sigact
.
sa_handler
=
alarm_handler
;
sigemptyset
(
&
sigact
.
sa_mask
);
sigact
.
sa_flags
=
0
;
sigaction
(
SIGALRM
,
&
sigact
,
NULL
);
log_send
(
LOG_T_DEBUG
,
"Starting sample thread"
);
controller_sample_thread_running
=
true
;
...
...
@@ -206,14 +245,26 @@ static void *sample_thread(void *arg)
clock_gettime
(
clock
,
&
t_end
);
sample_timing_add
(
&
st_start
,
&
t
,
&
t_start
);
sample_timing_add
(
&
st_io
,
&
t_start
,
&
t_io
);
sample_timing_add
(
&
st_end
,
&
t
,
&
t_end
);
sample_timing_add
(
&
st_sample
,
&
t_start
,
&
t_end
);
if
(
sample_timing_diff_nsec
(
&
t
,
&
t_end
)
>
nsec_interval
)
{
controller_sampleoverruns
++
;
}
if
(
sample_timing_sane
(
&
t
,
&
t_start
))
{
log_send
(
LOG_T_ERROR
,
"Time sanity check triggered: %lld.%09ld %lld.%09ld"
,
(
long
long
)
t
.
tv_sec
,
t
.
tv_nsec
,
(
long
long
)
t_start
.
tv_sec
,
t_start
.
tv_nsec
);
t
.
tv_sec
=
t_start
.
tv_sec
;
t
.
tv_nsec
=
(
t_start
.
tv_nsec
/
nsec_interval
)
*
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
,
&
t_start
);
sample_timing_add
(
&
st_io
,
&
t_start
,
&
t_io
);
sample_timing_add
(
&
st_end
,
&
t
,
&
t_end
);
sample_timing_add
(
&
st_sample
,
&
t_start
,
&
t_end
);
}
alarm
(
1
);
}
return
NULL
;
}
...
...
controller/dt_ctrl.c
View file @
8d1a320c
...
...
@@ -33,6 +33,7 @@
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <signal.h>
#include "dynarg.h"
...
...
@@ -53,7 +54,12 @@ int main(int argc, char **argv)
{
char
*
ctrl_filename
;
char
*
dot_filename
;
sigset_t
sigset
;
sigemptyset
(
&
sigset
);
sigaddset
(
&
sigset
,
SIGALRM
);
sigprocmask
(
SIG_BLOCK
,
&
sigset
,
NULL
);
log_server_start
(
CTRL_LOG_PORT
,
LOG_T_DEBUG
,
LOG_T_INFO
);