Commit 87baa8ef authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Rename 3d order SPG and add 1st order

parent e4612fe1
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2009
Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2009
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/>.
*/
#ifndef _INCLUDE_BLOCK_SETPOINT_GENERATOR_
#define _INCLUDE_BLOCK_SETPOINT_GENERATOR_
#include "controller_block.h"
struct controller_block * block_setpoint_generator_create(char *name, va_list ap);
#endif /* _INCLUDE_BLOCK_SETPOINT_GENERATOR_ */
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2014
Copyright Stichting C.A. Muller Radioastronomiestation, 2014
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/>.
*/
#define _ISOC99_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <float.h>
#include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <controller/controller_sample.h>
#include <log/log.h>
/*
inputs outputs
nr name nr name
--------------------------
| |
----| 0 reset_x 0 x |----
| |
----| 1 reset 1 v |----
| |
| 2 setpoint |----
| |
--------------------------
'setpoint' is the setpoint as the user requested it.
'x' is the output of the setpoint generator.
'v', is the first order derivative of 'x'.
*/
struct controller_block_private {
/*
In the setpoint generator blocks the following physics notations
will be used.
x = position
v = velocity (1st derivative of x)
*/
float *reset_x;
bool *reset;
/* beware: 'samples' is the time unit, not 'seconds',
all parameters and commands are scaled on block entry/exit */
float cmd_x;
float cmd_v;
float max_x;
float min_x;
float max_v;
/* conversion factor for seconds/tick and its inverse */
float freq; /* ticks per second */
/* parameters in real world format (time unit: second) */
float max_v_sec;
/* current internal state */
float cur_x;
float cur_v;
struct command_entry cur_command;
bool cur_done;
bool cur_start;
struct controller_command *command;
};
static void setpoint_generator_1d_calculate(struct controller_block *spg)
{
struct controller_block_private *priv = spg->private;
float cur_x, cur_v;
cur_x = priv->cur_x;
cur_v = priv->cur_v;
if (*priv->reset) {
priv->cmd_x = *priv->reset_x;
cur_x = priv->cmd_x;
priv->cur_done = true;
priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
priv->cmd_v = 0.0;
cur_v = 0.0;
}
if (priv->cur_done) {
int r;
r = controller_command_queue_read(priv->command, &priv->cur_command);
if (r == 0) {
priv->cur_done = false;
priv->cur_start = false;
} else {
if (priv->cur_command.type ==
COMMAND_PTYPE_SETPOINT_TIME) {
priv->cmd_v = 0.0;
priv->cmd_x = priv->cur_command.value.f;
}
}
}
if (!priv->cur_done) {
double t;
switch (priv->cur_command.type) {
case COMMAND_PTYPE_SETPOINT:
priv->cmd_x = priv->cur_command.value.f;
priv->cmd_v = 0.0;
priv->cur_done = true;
break;
case COMMAND_PTYPE_SPEED:
priv->cmd_v = priv->cur_command.value.f;
priv->cur_done = true;
break;
case COMMAND_PTYPE_SETPOINT_TIME:
if (!priv->cur_start) {
double t_samplenr;
t_samplenr = floor(
((double)priv->cur_command.t.tv_nsec * priv->freq) /
1000000.0);
t = (double)(priv->cur_command.t.tv_sec
- controller_time_seconds) * priv->freq +
t_samplenr -
(double)controller_time_samplenr;
if (t <= 0.0) {
/* Command is to old, pretend it
is a setpoint without time */
log_send(LOG_T_WARNING, "%s: setpoint's time is in the past, clock skew? (%lld < %d)",
spg->name, (long long)priv->cur_command.t.tv_sec, controller_time_seconds);
priv->cmd_x = priv->cur_command.value.f;
priv->cmd_v = 0.0;
priv->cur_done = true;
} else {
priv->cmd_v =
(priv->cur_command.value.f -
priv->cmd_x) / (t+1);
priv->cur_start = true;
}
}
if (priv->cur_command.t.tv_sec <=
controller_time_seconds) {
priv->cur_done = true;
priv->cmd_x =
priv->cur_command.value.f -
priv->cmd_v;
}
break;
default:
break;
}
}
if (fabs(priv->cmd_v) >= priv->max_v) {
priv->cmd_v = copysign(priv->max_v, priv->cmd_v);
}
if (priv->cur_command.type == COMMAND_PTYPE_SPEED) {
priv->cmd_x = cur_x + priv->cmd_v * controller_sample_period();
if (priv->cmd_x > priv->max_x)
priv->cmd_x = priv->max_x;
if (priv->cmd_x < priv->min_x)
priv->cmd_x = priv->min_x;
cur_x = priv->cmd_x;
cur_v = priv->cmd_v;
} else {
float x_diff = priv->cmd_x - cur_x;
if (fabs(x_diff) <= priv->max_v) {
cur_x = priv->cmd_x;
cur_v = 0;
} else {
float v = copysign(priv->max_v, x_diff);
cur_x += v;
cur_v = v;
}
}
priv->cur_x = cur_x;
priv->cur_v = cur_v * priv->freq;
}
static int block_setpoint_generator_command_filter(struct controller_command *command,
struct command_entry *entry)
{
struct controller_block_private *priv = command->block->private;
int r = 0;
switch (entry->type) {
case COMMAND_PTYPE_SETPOINT_TIME:
case COMMAND_PTYPE_SETPOINT: {
float value = entry->value.f;
if (value > priv->max_x)
value = priv->max_x;
if (value < priv->min_x)
value = priv->min_x;
entry->value.f = value;
break;
}
case COMMAND_PTYPE_SPEED: {
float value = entry->value.f;
if (fabs(value) > priv->max_v_sec)
value = copysign(priv->max_v_sec, value);
entry->value.f = value;
break;
}
default:
r = -1;
break;
}
return r;
}
static struct controller_block_param_list params[] = {
{ "setpoint", true },
{ "max_x", true },
{ "min_x", true },
{ "max_v", true },
{ NULL },
};
static void param_get(struct controller_block *spg, int param, void *val)
{
switch (param) {
case 0:
*(float*)val =
spg->private->cmd_x;
break;
case 1:
*(float*)val =
spg->private->max_x;
break;
case 2:
*(float*)val =
spg->private->min_x;
break;
case 3:
*(float*)val =
spg->private->max_v_sec;
break;
}
}
static void param_set(struct controller_block *spg, int param, va_list val)
{
switch (param) {
case 0:
spg->private->cmd_x = va_arg(val, double);
spg->private->cur_x = spg->private->cmd_x;
spg->private->cur_done = true;
spg->private->cmd_v = 0.0;
spg->private->cur_v = 0.0;
break;
case 1:
spg->private->max_x = va_arg(val, double);
break;
case 2:
spg->private->min_x = va_arg(val, double);
break;
case 3:
spg->private->max_v_sec = va_arg(val, double);
break;
}
/* Scale all settings to sample time unit */
spg->private->max_v = spg->private->max_v_sec * controller_sample_period();
spg->private->freq = 1.0 / controller_sample_period();
}
static struct controller_block_interm_list interms[] = {
{ "reset_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, reset_x) },
{ "reset", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, reset) },
{ NULL }
};
static struct controller_block_outterm_list outterms[] = {
{ "x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_x) },
{ "v", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_v) },
{ "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x) },
{ NULL }
};
struct controller_block * block_setpoint_generator_1d_create(char *name, va_list ap)
{
struct controller_block *spg;
char *server_name;
char *spg_unit;
server_name = va_arg(ap, char *);
spg_unit = va_arg(ap, char *);
spg = malloc(sizeof(struct controller_block));
if (!spg)
return NULL;
spg->type = "setpoint_generator_1d";
spg->name = malloc(strlen(name)+1);
if (!spg->name)
goto err_spg;
strcpy(spg->name, name);
spg->private = malloc(sizeof(struct controller_block_private));
if (!spg->private)
goto err_name;
spg->private->cmd_x = 0.0;
spg->private->cmd_v = 0.0;
spg->private->max_x = 0.0;
spg->private->min_x = 0.0;
spg->private->max_v = 0.0;
spg->private->max_v_sec = 0.0;
spg->private->freq = 1.0;
spg->private->cur_x = 0.0;
spg->private->cur_v = 0.0;
if (controller_block_interm_list_init(spg, interms))
goto err_private;
if (controller_block_outterm_list_init(spg, outterms))
goto err_input;
spg->calculate = setpoint_generator_1d_calculate;
if (controller_block_param_list_init(spg, params))
goto err_output;
spg->param_get = param_get;
spg->param_set = param_set;
spg->private->cur_done = true;
spg->private->cur_start = false;
controller_block_add(spg);
spg->private->command = controller_command_create(spg,
server_name, spg_unit);
spg->private->command->value_type = COMMAND_VALUE_TYPE_FLOAT;
spg->private->command->command_types[0] = COMMAND_PTYPE_SETPOINT;
spg->private->command->command_types[1] = COMMAND_PTYPE_SPEED;
spg->private->command->command_types[2] = COMMAND_PTYPE_SETPOINT_TIME;
spg->private->command->filter =
block_setpoint_generator_command_filter;
return spg;
err_output:
free(spg->output);
err_input:
free(spg->input);
err_private:
free(spg->private);
err_name:
free(spg->name);
err_spg:
free(spg);
return NULL;
}
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013, 2014
Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008, 2013
This program is free software: you can redistribute it and/or modify
......@@ -215,7 +215,7 @@ static double x_after_ticks(struct controller_block_private *priv,
return x_at_t;
}
static void setpoint_generator_calculate(struct controller_block *spg)
static void setpoint_generator_3d_calculate(struct controller_block *spg)
{
struct controller_block_private *priv = spg->private;
double cur_x, cur_v;
......@@ -943,7 +943,7 @@ static struct controller_block_outterm_list outterms[] = {
};
struct controller_block * block_setpoint_generator_create(char *name, va_list ap)
struct controller_block * block_setpoint_generator_3d_create(char *name, va_list ap)
{
struct controller_block *spg;
char *server_name;
......@@ -956,7 +956,7 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
if (!spg)
return NULL;
spg->type = "setpoint_generator";
spg->type = "setpoint_generator_3d";
spg->name = malloc(strlen(name)+1);
if (!spg->name)
goto err_spg;
......@@ -1009,7 +1009,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 = setpoint_generator_calculate;
spg->calculate = setpoint_generator_3d_calculate;
if (controller_block_param_list_init(spg, params))
goto err_output;
......
......@@ -27,7 +27,8 @@ BLOCKS := \
quantize \
random \
rangecheck \
setpoint_generator \
setpoint_generator_1d \
setpoint_generator_3d \
servo_state \
sine \
state_machine \
......
......@@ -28,7 +28,7 @@ import "dt_ctrl_az_sim.ctrl"
import "dt_ctrl_ec_sim.ctrl"
blocks {
{ "setpoint_generator", "azimuth_spg", "Azimuth_Setpoint", "rad" }
{ "setpoint_generator_3d", "azimuth_spg", "Azimuth_Setpoint", "rad" }
{ "servo_state", "azimuth_servo_state" }
{ "and2", "azimuth_safe_and" }
{ "subtract", "azimuth_setpoint_error" }
......@@ -47,10 +47,10 @@ blocks {
{ "rangecheck", "azimuth_speed_warning" }
{ "matrix_2x2", "elevation_input_matrix" }
{ "setpoint_generator", "elevation_spg", "Elevation_Setpoint", "rad" }
{ "setpoint_generator_3d", "elevation_spg", "Elevation_Setpoint", "rad" }
{ "servo_state", "elevation_servo_state" }
{ "and2", "elevation_safe_and" }
{ "setpoint_generator", "elevation_torsion_spg", "Elevation_Torsion_Setpoint", "rad" }
{ "setpoint_generator_1d", "elevation_torsion_spg", "Elevation_Torsion_Setpoint", "rad" }
{ "subtract", "elevation_setpoint_error" }
{ "subtract", "elevation_error" }
{ "subtract", "elevation_torsion_error" }
......@@ -325,15 +325,9 @@ params {
{ "elevation_servo_state", "max_v", (float) rpm2rads(2100.0)/$(elevation_gear) }
{ "elevation_servo_state", "max_a", (float) 0.0003 }
{ "elevation_torsion_spg", "setpoint", (float) 0.0 }
{ "elevation_torsion_spg", "t", (float) 0.004 }
{ "elevation_torsion_spg", "max_x", (float) 0.00 }
{ "elevation_torsion_spg", "min_x", (float)-0.00 }
{ "elevation_torsion_spg", "max_v", (float) rpm2rads(0.001) }
{ "elevation_torsion_spg", "max_a", (float) 0.00004 }
{ "elevation_torsion_spg", "max_j", (float) 0.00001 }
{ "elevation_torsion_spg", "precision_x", (float) 0.000001 }
{ "elevation_torsion_spg", "precision_a", (float) 0.000001 }
{ "elevation_torsion_spg", "precision_v", (float) 0.000001 }
{ "elevation_pid", "kp", (float) 0.80 }
{ "elevation_pid", "ki", (float) 0.001 }
{ "elevation_pid", "kd", (float) 0.001 }
......
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