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

Generic improvements in blocks

parent d92ca07d
......@@ -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;
}
......@@ -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;
iir->private->transfer[iir->private->ind][2] =
*iir->private->input0;
iir->private->ind++;
iir->private->ind = iir->private->ind % iir->private->len;
ind = iir->private->ind;
len = iir->private->len;
ind = priv->ind;
transfer[ind][2] = input0;
len = priv->len;
ind++;
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;
}
......@@ -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;
}
......@@ -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;
}
......@@ -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;
}
......@@ -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)