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

Cherry pick new blocks originally made for corso

parent 25b2c5c2
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2011
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2011, 2013
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
......@@ -50,7 +50,9 @@ struct controller_block_private {
int divider;
int continuous;
bool pwm_prev;
int cnt;
float remainder;
};
static void calculate(struct controller_block *bridge)
......@@ -73,13 +75,23 @@ static void calculate(struct controller_block *bridge)
b = false;
}
cnt++;
cnt = cnt % divider;
cnt++;
priv->cnt = cnt;
value = (fabsf(in) * divider) + 0.5;
value = (fminf(fabsf(in), 1.0) * divider) + priv->remainder;
// printf("+ %d %f %f %f\n", cnt, in, value, priv->remainder);
if (cnt > value) {
float diff;
pwm = false;
diff = (float)cnt - value;
if (priv->pwm_prev) {
priv->remainder = 1.0 - diff;
}
// printf("- %d %f %f %f\n", cnt, value, (float)cnt, priv->remainder);
} else {
if (!priv->continuous && cnt == divider-1) {
pwm = false;
......@@ -88,6 +100,7 @@ static void calculate(struct controller_block *bridge)
}
}
priv->pwm_prev = pwm;
priv->a = a & pwm;
priv->b = b & pwm;
}
......@@ -129,54 +142,42 @@ static void param_set(struct controller_block *bridge, int param, va_list val)
}
}
static struct controller_block_interm_list interms[] = {
{ "in", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, in) },
{ NULL },
};
static struct controller_block_outterm_list outterms[] = {
{ "a", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, a) },
{ "b", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, b) },
{ NULL },
};
struct controller_block * block_bridge_pwm_create(char *name)
{
struct controller_block *bridge;
bridge = malloc(sizeof(struct controller_block));
bridge = controller_block_alloc("bridge_pwm", name,
sizeof(struct controller_block_private));
if (!bridge)
return NULL;
bridge->type = "bridge_pwm";
bridge->name = malloc(strlen(name)+1);
if (!bridge->name)
goto err_bridge;
strcpy(bridge->name, name);
bridge->private = malloc(sizeof(struct controller_block_private));
if (!bridge->private)
goto err_name;
bridge->private->a = false;
bridge->private->b = false;
bridge->private->divider = 1;
bridge->private->continuous = true;
bridge->private->remainder = 0.0;
if (controller_block_interm_list_init(bridge, interms))
goto err_inputs;
bridge->inputs = 1;
bridge->input = malloc(sizeof(struct controller_block_interm));
if (!bridge->input)
goto err_private;
bridge->input[0].name = "in";
bridge->input[0].type = CONTROLLER_BLOCK_TERM_FLOAT;
bridge->input[0].value.f = &bridge->private->in;
bridge->input[0].ghostof = NULL;
bridge->outputs = 2;
bridge->output = malloc(sizeof(struct controller_block_outterm) * 2);
if (!bridge->output)
goto err_input;
bridge->output[0].name = "a";
bridge->output[0].type = CONTROLLER_BLOCK_TERM_BOOL;
bridge->output[0].value.b = &bridge->private->a;
bridge->output[0].source = bridge;
bridge->output[1].name = "b";
bridge->output[1].type = CONTROLLER_BLOCK_TERM_BOOL;
bridge->output[1].value.b = &bridge->private->b;
bridge->output[1].source = bridge;
if (controller_block_outterm_list_init(bridge, outterms))
goto err_outputs;
bridge->calculate = calculate;
if (controller_block_param_list_init(bridge, params))
goto err_output;
goto err_params;
bridge->param_get = param_get;
bridge->param_set = param_set;
......@@ -184,15 +185,13 @@ struct controller_block * block_bridge_pwm_create(char *name)
controller_block_add(bridge);
return bridge;
err_output:
err_params:
free(bridge->output);
err_input:
err_outputs:
free(bridge->input);
err_private:
err_inputs:
free(bridge->private);
err_name:
free(bridge->name);
err_bridge:
free(bridge);
return NULL;
}
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2013
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <controller/controller_block.h>
/*
inputs outputs
nr name nr name
----------------------
| |
---| 0 in 0 out |----
| |
----------------------
out = c0 + c1 / (in + c2)
*/
struct controller_block_private {
float *in;
float out;
float c0;
float c1;
float c2;
};
static void calculate(struct controller_block *gain)
{
struct controller_block_private *priv = gain->private;
priv->out = priv->c0 + priv->c1 / (*priv->in + priv->c2);
}
static struct controller_block_param_list params[] = {
{ "c0", false },
{ "c1", false },
{ "c2", false },
{ NULL },
};
static void param_get(struct controller_block *iprop, int param, void *val)
{
switch (param) {
case 0:
*(float*)val = iprop->private->c0;
break;
case 1:
*(float*)val = iprop->private->c1;
break;
case 2:
*(float*)val = iprop->private->c2;
break;
}
}
static void param_set(struct controller_block *iprop, int param, va_list val)
{
switch (param) {
case 0:
iprop->private->c0 = va_arg(val, double);
break;
case 1:
iprop->private->c1 = va_arg(val, double);
break;
case 2:
iprop->private->c2 = va_arg(val, double);
break;
}
}
static struct controller_block_interm_list interms[] = {
{ "in", CONTROLLER_BLOCK_TERM_FLOAT,
offsetof(struct controller_block_private, in) },
{ NULL },
};
static struct controller_block_outterm_list outterms[] = {
{ "out", CONTROLLER_BLOCK_TERM_FLOAT,
offsetof(struct controller_block_private, out) },
{ NULL },
};
struct controller_block * block_inverse_proportional_create(char *name)
{
struct controller_block *iprop;
iprop = controller_block_alloc("inverse_proportional", name,
sizeof(struct controller_block_private));
if (!iprop)
return NULL;
iprop->private->out = 0.0;
iprop->private->c0 = 0.0;
iprop->private->c1 = 1.0;
iprop->private->c2 = 0.0;
if (controller_block_interm_list_init(iprop, interms))
goto err_inputs;
if (controller_block_outterm_list_init(iprop, outterms))
goto err_outputs;
iprop->calculate = calculate;
if (controller_block_param_list_init(iprop, params))
goto err_params;
iprop->param_get = param_get;
iprop->param_set = param_set;
controller_block_add(iprop);
return iprop;
err_params:
free(iprop->output);
err_outputs:
free(iprop->input);
err_inputs:
free(iprop->private);
free(iprop->name);
free(iprop);
return NULL;
}
......@@ -16,6 +16,7 @@ BLOCK_SRCS = \
$(DIR)/block_friction.c \
$(DIR)/block_gain.c \
$(DIR)/block_gain_var.c \
$(DIR)/block_inverse_proportional.c \
$(DIR)/block_limit.c \
$(DIR)/block_limit_dyn.c \
$(DIR)/block_limit_2nd.c \
......
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