Skip to content
GitLab
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
d0273cc4
Commit
d0273cc4
authored
Nov 28, 2013
by
Jeroen Vreeken
Browse files
Generic improvements in blocks
parent
d92ca07d
Changes
26
Hide whitespace changes
Inline
Side-by-side
controller/block/block_add.c
View file @
d0273cc4
...
...
@@ -41,69 +41,50 @@ struct controller_block_private {
float
out
;
};
static
void
calculate
(
struct
controller_block
*
sub
)
static
void
add_
calculate
(
struct
controller_block
*
sub
)
{
sub
->
private
->
out
=
*
sub
->
private
->
input0
+
*
sub
->
private
->
input1
;
struct
controller_block_private
*
priv
=
sub
->
private
;
priv
->
out
=
*
priv
->
input0
+
*
priv
->
input1
;
}
static
struct
controller_block_interm_list
interms
[]
=
{
{
"in0"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
input0
)
},
{
"in1"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
input1
)
},
{
NULL
}
};
static
struct
controller_block_outterm_list
outterms
[]
=
{
{
"out"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
out
)
},
{
NULL
}
};
struct
controller_block
*
block_add_create
(
char
*
name
)
{
struct
controller_block
*
sub
;
struct
controller_block
*
add
;
sub
=
malloc
(
sizeof
(
struct
controller_block
));
if
(
!
sub
)
if
(
!
(
add
=
controller_block_alloc
(
"add"
,
name
,
sizeof
(
struct
controller_block_private
))))
return
NULL
;
sub
->
type
=
"add"
;
sub
->
name
=
malloc
(
strlen
(
name
)
+
1
);
if
(
!
sub
->
name
)
goto
err_sub
;
strcpy
(
sub
->
name
,
name
);
sub
->
private
=
malloc
(
sizeof
(
struct
controller_block_private
));
if
(
!
sub
->
private
)
goto
err_name
;
sub
->
private
->
out
=
0
.
0
;
add
->
private
->
out
=
0
.
0
;
sub
->
inputs
=
2
;
sub
->
input
=
malloc
(
sizeof
(
struct
controller_block_interm
)
*
2
);
if
(
!
sub
->
input
)
goto
err_private
;
sub
->
input
[
0
].
name
=
"in0"
;
sub
->
input
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
sub
->
input
[
0
].
value
.
f
=
&
sub
->
private
->
input0
;
sub
->
input
[
0
].
ghostof
=
NULL
;
sub
->
input
[
1
].
name
=
"in1"
;
sub
->
input
[
1
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
sub
->
input
[
1
].
value
.
f
=
&
sub
->
private
->
input1
;
sub
->
input
[
1
].
ghostof
=
NULL
;
sub
->
outputs
=
1
;
sub
->
output
=
malloc
(
sizeof
(
struct
controller_block_outterm
));
if
(
!
sub
->
output
)
goto
err_input
;
sub
->
output
[
0
].
name
=
"out"
;
sub
->
output
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
sub
->
output
[
0
].
value
.
f
=
&
sub
->
private
->
out
;
sub
->
output
[
0
].
source
=
sub
;
sub
->
calculate
=
calculate
;
sub
->
params
=
0
;
sub
->
param
=
NULL
;
sub
->
param_get
=
NULL
;
sub
->
param_set
=
NULL
;
controller_block_add
(
sub
);
return
sub
;
err_input:
free
(
sub
->
input
);
err_private:
free
(
sub
->
private
);
err_name:
free
(
sub
->
name
);
err_sub:
free
(
sub
);
if
(
controller_block_interm_list_init
(
add
,
interms
))
goto
err_block
;
if
(
controller_block_outterm_list_init
(
add
,
outterms
))
goto
err_block
;
add
->
calculate
=
add_calculate
;
add
->
params
=
0
;
add
->
param
=
NULL
;
add
->
param_get
=
NULL
;
add
->
param_set
=
NULL
;
controller_block_add
(
add
);
return
add
;
err_block:
controller_block_free
(
add
);
return
NULL
;
}
controller/block/block_filter_iir.c
View file @
d0273cc4
...
...
@@ -47,30 +47,33 @@ struct controller_block_private {
struct
block_filter_iir_param
param
;
};
static
void
calculate
(
struct
controller_block
*
iir
)
{
static
void
filter_iir_calculate
(
struct
controller_block
*
iir
)
{
struct
controller_block_private
*
priv
=
iir
->
private
;
float
input0
=
*
priv
->
input0
;
float
(
*
transfer
)[
4
]
=
priv
->
transfer
;
int
i
;
int
ind
,
len
;
float
out
;
i
ir
->
private
->
transfer
[
iir
->
priv
ate
->
ind
][
2
]
=
*
iir
->
private
->
input0
;
iir
->
private
->
ind
++
;
i
ir
->
private
->
ind
=
iir
->
private
->
ind
%
iir
->
private
->
len
;
ind
=
iir
->
private
->
ind
;
len
=
iir
->
private
->
len
;
i
nd
=
priv
->
ind
;
transfer
[
ind
][
2
]
=
input0
;
len
=
priv
->
len
;
i
nd
++
;
ind
=
ind
%
len
;
priv
->
ind
=
ind
;
iir
->
private
->
transfer
[
ind
][
3
]
=
0
;
out
=
0
;
for
(
i
=
0
;
i
<
len
;
i
++
)
{
iir
->
private
->
transfer
[
ind
][
3
]
+=
iir
->
private
->
transfer
[
i
][
1
]
*
iir
->
private
->
transfer
[(
i
+
ind
)
%
len
][
2
];
iir
->
private
->
transfer
[
ind
][
3
]
+=
iir
->
private
->
transfer
[
i
][
0
]
*
iir
->
private
->
transfer
[(
i
+
ind
)
%
len
][
3
];
}
iir
->
private
->
out
=
iir
->
private
->
transfer
[
ind
][
3
];
int
j
=
(
i
+
ind
)
%
len
;
out
+=
transfer
[
i
][
1
]
*
transfer
[
j
][
2
];
out
+=
transfer
[
i
][
0
]
*
transfer
[
j
][
3
];
}
transfer
[
ind
][
3
]
=
out
;
priv
->
out
=
out
;
}
static
struct
controller_block_param_list
params
[]
=
{
...
...
@@ -149,50 +152,38 @@ static void param_set(struct controller_block *iir, int param, va_list ap)
}
}
static
struct
controller_block_interm_list
interms
[]
=
{
{
"in"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
input0
)
},
{
NULL
}
};
static
struct
controller_block_outterm_list
outterms
[]
=
{
{
"out"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
out
)
},
{
NULL
}
};
struct
controller_block
*
block_filter_iir_create
(
char
*
name
)
{
struct
controller_block
*
iir
;
iir
=
malloc
(
sizeof
(
struct
controller_block
));
if
(
!
iir
)
if
(
!
(
iir
=
controller_block_alloc
(
"filter_iir"
,
name
,
sizeof
(
struct
controller_block_private
))))
return
NULL
;
iir
->
type
=
"filter_iir"
;
iir
->
name
=
malloc
(
strlen
(
name
)
+
1
);
if
(
!
iir
->
name
)
goto
err_iir
;
strcpy
(
iir
->
name
,
name
);
iir
->
private
=
malloc
(
sizeof
(
struct
controller_block_private
));
if
(
!
iir
->
private
)
goto
err_name
;
iir
->
private
->
out
=
0
.
0
;
iir
->
private
->
len
=
0
;
iir
->
private
->
transfer
=
NULL
;
iir
->
private
->
ind
=
0
;
iir
->
inputs
=
1
;
iir
->
input
=
malloc
(
sizeof
(
struct
controller_block_interm
));
if
(
!
iir
->
input
)
goto
err_private
;
iir
->
input
[
0
].
name
=
"in"
;
iir
->
input
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
iir
->
input
[
0
].
value
.
f
=
&
iir
->
private
->
input0
;
iir
->
input
[
0
].
ghostof
=
NULL
;
iir
->
outputs
=
1
;
iir
->
output
=
malloc
(
sizeof
(
struct
controller_block_outterm
)
*
1
);
if
(
!
iir
->
output
)
goto
err_input
;
iir
->
output
[
0
].
name
=
"out"
;
iir
->
output
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
iir
->
output
[
0
].
value
.
f
=
&
iir
->
private
->
out
;
iir
->
output
[
0
].
source
=
iir
;
iir
->
calculate
=
calculate
;
if
(
controller_block_interm_list_init
(
iir
,
interms
))
goto
err_block
;
if
(
controller_block_outterm_list_init
(
iir
,
outterms
))
goto
err_block
;
iir
->
calculate
=
filter_iir_calculate
;
if
(
controller_block_param_list_init
(
iir
,
params
))
goto
err_
output
;
goto
err_
block
;
iir
->
param_get
=
param_get
;
iir
->
param_set
=
param_set
;
...
...
@@ -200,15 +191,7 @@ struct controller_block * block_filter_iir_create(char *name)
controller_block_add
(
iir
);
return
iir
;
err_output:
free
(
iir
->
output
);
err_input:
free
(
iir
->
input
);
err_private:
free
(
iir
->
private
);
err_name:
free
(
iir
->
name
);
err_iir:
free
(
iir
);
err_block:
controller_block_free
(
iir
);
return
NULL
;
}
controller/block/block_filter_lp.c
View file @
d0273cc4
...
...
@@ -45,10 +45,15 @@ struct controller_block_private {
float
tau
;
};
static
void
calculate
(
struct
controller_block
*
lp
)
static
void
filter_lp_
calculate
(
struct
controller_block
*
lp
)
{
lp
->
private
->
out
*=
1
.
0
-
lp
->
private
->
alpha
;
lp
->
private
->
out
+=
*
lp
->
private
->
input0
*
lp
->
private
->
alpha
;
struct
controller_block_private
*
priv
=
lp
->
private
;
float
alpha
=
priv
->
alpha
;
float
out
=
priv
->
out
;
out
*=
1
.
0
-
alpha
;
out
+=
*
lp
->
private
->
input0
*
alpha
;
priv
->
out
=
out
;
}
static
struct
controller_block_param_list
params
[]
=
{
...
...
@@ -87,50 +92,38 @@ static void param_set(struct controller_block *lp, int param, va_list val)
lp
->
private
->
t
/
(
lp
->
private
->
t
+
lp
->
private
->
tau
);
}
static
struct
controller_block_interm_list
interms
[]
=
{
{
"in"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
input0
)
},
{
NULL
}
};
static
struct
controller_block_outterm_list
outterms
[]
=
{
{
"out"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
out
)
},
{
NULL
}
};
struct
controller_block
*
block_filter_lp_create
(
char
*
name
)
{
struct
controller_block
*
lp
;
lp
=
malloc
(
sizeof
(
struct
controller_block
));
if
(
!
lp
)
if
(
!
(
lp
=
controller_block_alloc
(
"filter_lp"
,
name
,
sizeof
(
struct
controller_block_private
))))
return
NULL
;
lp
->
type
=
"filter_lp"
;
lp
->
name
=
malloc
(
strlen
(
name
)
+
1
);
if
(
!
lp
->
name
)
goto
err_lp
;
strcpy
(
lp
->
name
,
name
);
lp
->
private
=
malloc
(
sizeof
(
struct
controller_block_private
));
if
(
!
lp
->
private
)
goto
err_name
;
lp
->
private
->
out
=
0
.
0
;
lp
->
private
->
t
=
1
.
0
;
lp
->
private
->
tau
=
1
.
0
;
lp
->
private
->
alpha
=
0
.
0
;
lp
->
inputs
=
1
;
lp
->
input
=
malloc
(
sizeof
(
struct
controller_block_interm
));
if
(
!
lp
->
input
)
goto
err_private
;
lp
->
input
[
0
].
name
=
"in"
;
lp
->
input
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
lp
->
input
[
0
].
value
.
f
=
&
lp
->
private
->
input0
;
lp
->
input
[
0
].
ghostof
=
NULL
;
lp
->
outputs
=
1
;
lp
->
output
=
malloc
(
sizeof
(
struct
controller_block_outterm
)
*
1
);
if
(
!
lp
->
output
)
goto
err_input
;
lp
->
output
[
0
].
name
=
"out"
;
lp
->
output
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
lp
->
output
[
0
].
value
.
f
=
&
lp
->
private
->
out
;
lp
->
output
[
0
].
source
=
lp
;
lp
->
calculate
=
calculate
;
if
(
controller_block_interm_list_init
(
lp
,
interms
))
goto
err_block
;
if
(
controller_block_outterm_list_init
(
lp
,
outterms
))
goto
err_block
;
lp
->
calculate
=
filter_lp_calculate
;
if
(
controller_block_param_list_init
(
lp
,
params
))
goto
err_
output
;
goto
err_
block
;
lp
->
param_get
=
param_get
;
lp
->
param_set
=
param_set
;
...
...
@@ -138,15 +131,7 @@ struct controller_block * block_filter_lp_create(char *name)
controller_block_add
(
lp
);
return
lp
;
err_output:
free
(
lp
->
output
);
err_input:
free
(
lp
->
input
);
err_private:
free
(
lp
->
private
);
err_name:
free
(
lp
->
name
);
err_lp:
free
(
lp
);
err_block:
controller_block_free
(
lp
);
return
NULL
;
}
controller/block/block_friction.c
View file @
d0273cc4
...
...
@@ -66,30 +66,32 @@ struct controller_block_private {
float
friction_dynamic
;
};
static
void
calculate
(
struct
controller_block
*
friction
)
static
void
friction_
calculate
(
struct
controller_block
*
friction
)
{
struct
controller_block_private
*
priv
=
friction
->
private
;
float
input0
=
*
priv
->
input0
;
float
input1
=
*
priv
->
input1
;
if
(
*
priv
->
input1
<=
priv
->
deadzone_max
&&
*
priv
->
input1
>=
priv
->
deadzone_min
)
{
if
(
abs
(
*
priv
->
input0
)
>
abs
(
priv
->
friction_static
))
{
if
(
*
priv
->
input1
>=
0
)
{
if
(
input1
<=
priv
->
deadzone_max
&&
input1
>=
priv
->
deadzone_min
)
{
if
(
abs
(
input0
)
>
abs
(
priv
->
friction_static
))
{
if
(
input1
>=
0
)
{
priv
->
out
=
*
priv
->
input0
-
priv
->
friction_static
;
input0
-
priv
->
friction_static
;
}
else
{
priv
->
out
=
*
priv
->
input0
+
priv
->
friction_static
;
input0
+
priv
->
friction_static
;
}
}
else
{
priv
->
out
=
0
.
0
;
}
}
else
{
if
(
*
priv
->
input1
>=
0
)
{
if
(
input1
>=
0
)
{
priv
->
out
=
*
priv
->
input0
-
priv
->
friction_dynamic
;
input0
-
priv
->
friction_dynamic
;
}
else
{
priv
->
out
=
*
priv
->
input0
+
priv
->
friction_dynamic
;
input0
+
priv
->
friction_dynamic
;
}
}
}
...
...
@@ -142,56 +144,40 @@ static void param_set(struct controller_block *friction, int param, va_list val)
}
}
static
struct
controller_block_interm_list
interms
[]
=
{
{
"in"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
input0
)
},
{
"speed"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
input1
)
},
{
NULL
}
};
static
struct
controller_block_outterm_list
outterms
[]
=
{
{
"out"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
out
)
},
{
NULL
}
};
struct
controller_block
*
block_friction_create
(
char
*
name
)
{
struct
controller_block
*
friction
;
friction
=
malloc
(
sizeof
(
struct
controller_block
));
if
(
!
friction
)
if
(
!
(
friction
=
controller_block_alloc
(
"friction"
,
name
,
sizeof
(
struct
controller_block_private
))))
return
NULL
;
friction
->
type
=
"friction"
;
friction
->
name
=
malloc
(
strlen
(
name
)
+
1
);
if
(
!
friction
->
name
)
goto
err_friction
;
strcpy
(
friction
->
name
,
name
);
friction
->
private
=
malloc
(
sizeof
(
struct
controller_block_private
));
if
(
!
friction
->
private
)
goto
err_name
;
friction
->
private
->
out
=
0
.
0
;
friction
->
private
->
friction_dynamic
=
0
.
0
;
friction
->
private
->
friction_static
=
0
.
0
;
friction
->
private
->
deadzone_min
=
0
.
0
;
friction
->
private
->
deadzone_max
=
0
.
0
;
friction
->
inputs
=
2
;
friction
->
input
=
malloc
(
sizeof
(
struct
controller_block_interm
)
*
2
);
if
(
!
friction
->
input
)
goto
err_private
;
friction
->
input
[
0
].
name
=
"in"
;
friction
->
input
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
friction
->
input
[
0
].
value
.
f
=
&
friction
->
private
->
input0
;
friction
->
input
[
0
].
ghostof
=
NULL
;
friction
->
input
[
1
].
name
=
"speed"
;
friction
->
input
[
1
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
friction
->
input
[
1
].
value
.
f
=
&
friction
->
private
->
input1
;
friction
->
input
[
1
].
ghostof
=
NULL
;
friction
->
outputs
=
1
;
friction
->
output
=
malloc
(
sizeof
(
struct
controller_block_outterm
));
if
(
!
friction
->
output
)
goto
err_input
;
friction
->
output
[
0
].
name
=
"out"
;
friction
->
output
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
friction
->
output
[
0
].
value
.
f
=
&
friction
->
private
->
out
;
friction
->
output
[
0
].
source
=
friction
;
friction
->
calculate
=
calculate
;
if
(
controller_block_interm_list_init
(
friction
,
interms
))
goto
err_block
;
if
(
controller_block_outterm_list_init
(
friction
,
outterms
))
goto
err_block
;
friction
->
calculate
=
friction_calculate
;
if
(
controller_block_param_list_init
(
friction
,
params
))
goto
err_
output
;
goto
err_
block
;
friction
->
param_get
=
param_get
;
friction
->
param_set
=
param_set
;
...
...
@@ -199,15 +185,7 @@ struct controller_block * block_friction_create(char *name)
controller_block_add
(
friction
);
return
friction
;
err_output:
free
(
friction
->
output
);
err_input:
free
(
friction
->
input
);
err_private:
free
(
friction
->
private
);
err_name:
free
(
friction
->
name
);
err_friction:
free
(
friction
);
err_block:
controller_block_free
(
friction
);
return
NULL
;
}
controller/block/block_gain.c
View file @
d0273cc4
...
...
@@ -41,10 +41,11 @@ struct controller_block_private {
float
gain
;
};
static
void
calculate
(
struct
controller_block
*
gain
)
static
void
gain_
calculate
(
struct
controller_block
*
gain
)
{
gain
->
private
->
out
=
*
gain
->
private
->
input0
*
gain
->
private
->
gain
;
struct
controller_block_private
*
priv
=
gain
->
private
;
priv
->
out
=
*
priv
->
input0
*
priv
->
gain
;
}
static
struct
controller_block_param_list
params
[]
=
{
...
...
@@ -70,48 +71,36 @@ static void param_set(struct controller_block *gain, int param, va_list val)
}
}
static
struct
controller_block_interm_list
interms
[]
=
{
{
"in"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
input0
)
},
{
NULL
}
};
static
struct
controller_block_outterm_list
outterms
[]
=
{
{
"out"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
out
)
},
{
NULL
}
};
struct
controller_block
*
block_gain_create
(
char
*
name
)
{
struct
controller_block
*
gain
;
gain
=
malloc
(
sizeof
(
struct
controller_block
));
if
(
!
gain
)
if
(
!
(
gain
=
controller_block_alloc
(
"gain"
,
name
,
sizeof
(
struct
controller_block_private
))))
return
NULL
;
gain
->
type
=
"gain"
;
gain
->
name
=
malloc
(
strlen
(
name
)
+
1
);
if
(
!
gain
->
name
)
goto
err_gain
;
strcpy
(
gain
->
name
,
name
);
gain
->
private
=
malloc
(
sizeof
(
struct
controller_block_private
));
if
(
!
gain
->
private
)
goto
err_name
;
gain
->
private
->
out
=
0
.
0
;
gain
->
private
->
gain
=
0
.
0
;
gain
->
inputs
=
1
;
gain
->
input
=
malloc
(
sizeof
(
struct
controller_block_interm
));
if
(
!
gain
->
input
)
goto
err_private
;
gain
->
input
[
0
].
name
=
"in"
;
gain
->
input
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
gain
->
input
[
0
].
value
.
f
=
&
gain
->
private
->
input0
;
gain
->
input
[
0
].
ghostof
=
NULL
;
gain
->
outputs
=
1
;
gain
->
output
=
malloc
(
sizeof
(
struct
controller_block_outterm
)
*
1
);
if
(
!
gain
->
output
)
goto
err_input
;
gain
->
output
[
0
].
name
=
"out"
;
gain
->
output
[
0
].
type
=
CONTROLLER_BLOCK_TERM_FLOAT
;
gain
->
output
[
0
].
value
.
f
=
&
gain
->
private
->
out
;
gain
->
output
[
0
].
source
=
gain
;
gain
->
calculate
=
calculate
;
if
(
controller_block_interm_list_init
(
gain
,
interms
))
goto
err_block
;
if
(
controller_block_outterm_list_init
(
gain
,
outterms
))
goto
err_block
;
gain
->
calculate
=
gain_calculate
;
if
(
controller_block_param_list_init
(
gain
,
params
))
goto
err_
output
;
goto
err_
block
;
gain
->
param_get
=
param_get
;
gain
->
param_set
=
param_set
;
...
...
@@ -119,15 +108,7 @@ struct controller_block * block_gain_create(char *name)
controller_block_add
(
gain
);
return
gain
;
err_output:
free
(
gain
->
output
);
err_input:
free
(
gain
->
input
);
err_private:
free
(
gain
->
private
);
err_name:
free
(
gain
->
name
);
err_gain:
free
(
gain
);
err_block:
controller_block_free
(
gain
);
return
NULL
;
}
controller/block/block_gain_var.c
View file @
d0273cc4
...
...
@@ -41,55 +41,40 @@ struct controller_block_private {
float
out
;
};
static
void
calculate
(
struct
controller_block
*
gain
)
static
void
gain_var_
calculate
(
struct
controller_block
*
gain
)