Browse Source

Add multifrequency support

The frequency keyword has been removed.
The blocks keyword now needs two arguments: frequency (Hz) and delay (seconds)
beaglebone
Jeroen Vreeken 7 years ago
parent
commit
9634b339c9
50 changed files with 444 additions and 140 deletions
  1. +2
    -2
      Makefile
  2. +5
    -5
      build.mk
  3. +2
    -0
      common/log/log.h
  4. +1
    -2
      controller/block/block_command_bool.test.ctrl
  5. +1
    -3
      controller/block/block_filter_iir.test.ctrl
  6. +5
    -3
      controller/block/block_filter_lp.c
  7. +1
    -3
      controller/block/block_gain.test.ctrl
  8. +1
    -2
      controller/block/block_limit.test.ctrl
  9. +5
    -3
      controller/block/block_limit_2nd.c
  10. +1
    -2
      controller/block/block_matrix_2x2.test.ctrl
  11. +1
    -2
      controller/block/block_not.test.ctrl
  12. +3
    -3
      controller/block/block_pid.c
  13. +3
    -3
      controller/block/block_pid_aw.c
  14. +1
    -2
      controller/block/block_pid_aw.test.ctrl
  15. +1
    -3
      controller/block/block_quantize.test.ctrl
  16. +1
    -2
      controller/block/block_rangecheck.test.ctrl
  17. +2
    -2
      controller/block/block_servo_state.c
  18. +1
    -2
      controller/block/block_servo_state.test.ctrl
  19. +5
    -5
      controller/block/block_setpoint_generator_1d.c
  20. +1
    -2
      controller/block/block_setpoint_generator_1d.test.ctrl
  21. +4
    -4
      controller/block/block_setpoint_generator_3d.c
  22. +1
    -2
      controller/block/block_setpoint_generator_3d.test.ctrl
  23. +2
    -2
      controller/block/block_sine.c
  24. +2
    -2
      controller/block/block_trajectplayer.c
  25. +1
    -0
      controller/controller/build.mk
  26. +47
    -29
      controller/controller/controller_block.c
  27. +3
    -0
      controller/controller/controller_block.h
  28. +0
    -5
      controller/controller/controller_load.c
  29. +0
    -1
      controller/controller/controller_load_parser.l
  30. +6
    -9
      controller/controller/controller_load_parser.y
  31. +6
    -0
      controller/controller/controller_load_variable.c
  32. +11
    -11
      controller/controller/controller_sample.c
  33. +1
    -3
      controller/controller/controller_sample.h
  34. +164
    -0
      controller/controller/controller_time.c
  35. +36
    -0
      controller/controller/controller_time.h
  36. +3
    -3
      controller/controller/controller_trace.c
  37. +4
    -0
      controller/dt_ctrl.c
  38. +3
    -2
      controller/dt_ctrl.ctrl
  39. +1
    -1
      controller/dt_ctrl_az.ctrl
  40. +1
    -1
      controller/dt_ctrl_az_sim.ctrl
  41. +1
    -1
      controller/dt_ctrl_ec.ctrl
  42. +1
    -1
      controller/dt_ctrl_ec_sim.ctrl
  43. +1
    -1
      controller/dt_ctrl_el.ctrl
  44. +1
    -1
      controller/dt_ctrl_el_sim.ctrl
  45. +4
    -2
      controller/ec/ec.c
  46. +2
    -2
      controller/ec/esc_esi.c
  47. +2
    -2
      controller/ec/esc_watchdog.c
  48. +3
    -1
      controller/test/build.mk
  49. +6
    -3
      controller/test/test.ctrl
  50. +84
    -0
      controller/test/test_multifrequency.ctrl

+ 2
- 2
Makefile View File

@ -18,8 +18,8 @@ help:
echo " release Build both controller and console release"; \
echo " clean Clean for both controller and console"; \
echo ""; \
echo " HOST=<prefix> Cross compile for <prefix>.";\
echo " e.g.: HOST=arm-linux-gnueabihf"; \
echo " HOSTSYS=<prefix> Cross compile for <prefix>.";\
echo " e.g.: HOSTSYS=arm-linux-gnueabihf"; \
echo ""; \
echo "Building a release"; \
echo "------------------"; \


+ 5
- 5
build.mk View File

@ -17,11 +17,11 @@ else
FLEX=/usr/local/bin/flex
endif
ifdef HOST
CC=${HOST}-gcc
LIBTOOL=${HOST}-libtool
CONF_HOST=--host=${HOST}
HW=$(HOST)
ifdef HOSTSYS
CC=${HOSTSYS}-gcc
LIBTOOL=${HOSTSYS}-libtool
CONF_HOST=--host=${HOSTSYS}
HW=$(HOSTSYS)
endif


+ 2
- 0
common/log/log.h View File

@ -20,6 +20,8 @@
#ifndef _INCLUDE_LOG_H_
#define _INCLUDE_LOG_H_
#include <stdlib.h>
enum log_type {
LOG_T_ERROR,
LOG_T_WARNING,


+ 1
- 2
controller/block/block_command_bool.test.ctrl View File

@ -1,10 +1,9 @@
frequency 10
trigger {
{ "immediate" }
}
blocks {
blocks (10.0, 0.0) {
{ "command_bool", "command_bool" }
{ "test_output_bool", "value" }


+ 1
- 3
controller/block/block_filter_iir.test.ctrl View File

@ -1,11 +1,9 @@
frequency 250
trigger {
{ "immediate" }
}
blocks {
blocks (250.0, 0.0) {
{ "random", "noise" }
{ "filter_iir", "iir" }
{ "filter_iir", "iir2" }


+ 5
- 3
controller/block/block_filter_lp.c View File

@ -22,7 +22,7 @@
#include <string.h>
#include <controller/controller_block.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
/*
inputs outputs
@ -58,10 +58,12 @@ static void filter_lp_calculate(struct controller_block *lp)
static int param_set(struct controller_block *lp, va_list val)
{
double period = controller_time_period_get(lp->time);
lp->private->tau = va_arg(val, double);
lp->private->alpha =
controller_sample_period() / (controller_sample_period() + lp->private->tau);
lp->private->alpha = period / (period + lp->private->tau);
return 0;
}


+ 1
- 3
controller/block/block_gain.test.ctrl View File

@ -1,10 +1,8 @@
frequency 100
trigger {
{ "immediate" }
}
blocks {
blocks (100.0, 0.0) {
{ "gain", "gain" }
{ "test_input_float", "test_input" }


+ 1
- 2
controller/block/block_limit.test.ctrl View File

@ -1,10 +1,9 @@
frequency 10
trigger {
{ "immediate" }
}
blocks {
blocks (10.0, 0.0) {
{ "limit", "limit" }
{ "test_input_float", "in" }


+ 5
- 3
controller/block/block_limit_2nd.c View File

@ -23,7 +23,7 @@
#include <math.h>
#include <controller/controller_block.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
/*
inputs outputs
@ -119,10 +119,12 @@ static void limit_2nd_calculate(struct controller_block *limit)
static void param_prep(struct controller_block *limit)
{
double period = controller_time_period_get(limit->time);
limit->private->min = limit->private->mint
* controller_sample_period() * controller_sample_period();
* period * period;
limit->private->max = limit->private->maxt
* controller_sample_period() * controller_sample_period();
* period * period;
limit->private->imin = 1.0 / limit->private->min;
limit->private->imax = 1.0 / limit->private->max;
}


+ 1
- 2
controller/block/block_matrix_2x2.test.ctrl View File

@ -1,10 +1,9 @@
frequency 10
trigger {
{ "immediate" }
}
blocks {
blocks (10.0, 0.0) {
{ "matrix_2x2", "matrix" }
{ "test_input_float", "in0" }


+ 1
- 2
controller/block/block_not.test.ctrl View File

@ -1,10 +1,9 @@
frequency 100
trigger {
{ "immediate" }
}
blocks {
blocks (100.0, 0.0) {
{ "not", "not" }
{ "test_input_bool", "test_input" }


+ 3
- 3
controller/block/block_pid.c View File

@ -22,7 +22,7 @@
#include <string.h>
#include <controller/controller_block.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
/*
inputs outputs
@ -106,10 +106,10 @@ static void param_prep(struct controller_block *pid)
{
pid->private->ci =
pid->private->ki *
controller_sample_period();
controller_time_period_get(pid->time);
pid->private->cd =
pid->private->kd /
controller_sample_period();
controller_time_period_get(pid->time);
}
static int param_set_kp(struct controller_block *pid, va_list val)


+ 3
- 3
controller/block/block_pid_aw.c View File

@ -22,7 +22,7 @@
#include <string.h>
#include <controller/controller_block.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
/*
inputs outputs
@ -113,8 +113,8 @@ static void pid_aw_calculate(struct controller_block *pid)
static void param_prep(struct controller_block *pid)
{
pid->private->ci = pid->private->ki * controller_sample_period();
pid->private->cd = pid->private->kd / controller_sample_period();
pid->private->ci = pid->private->ki * controller_time_period_get(pid->time);
pid->private->cd = pid->private->kd / controller_time_period_get(pid->time);
}
static int param_set_kp(struct controller_block *pid, va_list val)


+ 1
- 2
controller/block/block_pid_aw.test.ctrl View File

@ -1,10 +1,9 @@
frequency 100
trigger {
{ "immediate" }
}
blocks {
blocks (100.0, 0.0) {
{ "pid_aw", "pid" }
{ "test_input_float", "in" }


+ 1
- 3
controller/block/block_quantize.test.ctrl View File

@ -1,10 +1,8 @@
frequency 10
trigger {
{ "immediate" }
}
blocks {
blocks (10.0, 0.0) {
{ "quantize", "quantize" }
{ "test_input_float", "in" }


+ 1
- 2
controller/block/block_rangecheck.test.ctrl View File

@ -1,10 +1,9 @@
frequency 10
trigger {
{ "immediate" }
}
blocks {
blocks (10.0, 0.0) {
{ "rangecheck", "rangecheck" }
{ "test_input_float", "in" }


+ 2
- 2
controller/block/block_servo_state.c View File

@ -24,7 +24,7 @@
#include <log/log.h>
#include <controller/controller_block.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
#include <controller/controller_command.h>
/*
@ -142,7 +142,7 @@ static void servo_state_calculate(struct controller_block *servo_state)
float out_x = priv->out_x;
float out_v = priv->out_v;
float out_a = 0.0;
float t = controller_sample_period();
float t = controller_time_period_get(servo_state->time);
if (out_v > 0.0) {
out_a = -priv->max_a;


+ 1
- 2
controller/block/block_servo_state.test.ctrl View File

@ -1,10 +1,9 @@
frequency 10
trigger {
{ "immediate" }
}
blocks {
blocks (10.0, 0.0) {
{ "servo_state", "servo_state" }
{ "test_input_float", "spg_x" }


+ 5
- 5
controller/block/block_setpoint_generator_1d.c View File

@ -26,7 +26,7 @@
#include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
#include <log/log.h>
/*
@ -184,7 +184,7 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
}
if (priv->cur_command.type == COMMAND_PTYPE_SPEED) {
cur_v = priv->cmd_v * controller_sample_period();
cur_v = priv->cmd_v * controller_time_period_get(spg->time);
priv->cmd_x = cur_x + cur_v;
if (priv->cmd_x > priv->max_x)
priv->cmd_x = priv->max_x;
@ -253,10 +253,10 @@ static int block_setpoint_generator_command_filter(struct controller_command *co
static void scale(struct controller_block *spg)
{
/* Scale all settings to sample time unit */
spg->private->max_v = spg->private->max_v_sec * controller_sample_period();
spg->private->max_v = spg->private->max_v_sec * controller_time_period_get(spg->time);
spg->private->freq = 1.0 / controller_sample_period();
spg->private->period_nsec = controller_sample_period() * 1000000000;
spg->private->freq = 1.0 / controller_time_period_get(spg->time);
spg->private->period_nsec = controller_time_period_get(spg->time) * 1000000000;
}
static int param_set_setpoint(struct controller_block *spg, va_list val)


+ 1
- 2
controller/block/block_setpoint_generator_1d.test.ctrl View File

@ -1,10 +1,9 @@
frequency 10
trigger {
{ "immediate" }
}
blocks {
blocks (10.0, 0.0) {
{ "setpoint_generator_1d", "spg", "spg", "na" }
{ "test_input_bool", "reset" }


+ 4
- 4
controller/block/block_setpoint_generator_3d.c View File

@ -26,7 +26,7 @@
#include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
#include <log/log.h>
/*
@ -280,7 +280,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->cur_done = true;
break;
case COMMAND_PTYPE_SPEED:
priv->cmd_v = priv->cur_command.value.f * controller_sample_period();
priv->cmd_v = priv->cur_command.value.f * controller_time_period_get(spg->time);
priv->cur_done = true;
break;
case COMMAND_PTYPE_SETPOINT_TIME:
@ -812,7 +812,7 @@ static int block_setpoint_generator_command_filter(struct controller_command *co
static void scale(struct controller_block *spg)
{
double t_max_a;
double tick = controller_sample_period();
double tick = controller_time_period_get(spg->time);
/* Scale all settings to sample time unit */
spg->private->max_v = spg->private->max_v_sec * tick;
@ -827,7 +827,7 @@ static void scale(struct controller_block *spg)
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;
spg->private->period_nsec = controller_sample_period() * 1000000000;
spg->private->period_nsec = controller_time_period_get(spg->time) * 1000000000;
/* Calculate delta v when comming from max_a */
t_max_a = ticks_to_a(spg->private, 0, spg->private->max_a);


+ 1
- 2
controller/block/block_setpoint_generator_3d.test.ctrl View File

@ -1,10 +1,9 @@
frequency 10
trigger {
{ "immediate" }
}
blocks {
blocks (10.0, 0.0) {
{ "setpoint_generator_3d", "spg", "spg", "na" }
{ "test_input_bool", "reset" }


+ 2
- 2
controller/block/block_sine.c View File

@ -23,7 +23,7 @@
#include <math.h>
#include <controller/controller_block.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
/*
outputs
@ -64,7 +64,7 @@ static int param_set(struct controller_block *sine, va_list val)
sine->private->f = va_arg(val, double);
sine->private->step =
2 * M_PI * sine->private->f * controller_sample_period();
2 * M_PI * sine->private->f * controller_time_period_get(sine->time);
return 0;
}


+ 2
- 2
controller/block/block_trajectplayer.c View File

@ -22,7 +22,7 @@
#include <string.h>
#include <controller/controller_block.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
#include <block/block_trajectplayer.h>
#include <log/log.h>
@ -88,7 +88,7 @@ static void calculate(struct controller_block *player)
}
} else if (priv->playing) {
traject = priv->trajectories[priv->cur];
playtime += controller_sample_period();
playtime += controller_time_period_get(player->time);
if (priv->playtime >= traject->time[traject->len - 1]) {
priv->playing = false;
priv->done = true;


+ 1
- 0
controller/controller/build.mk View File

@ -7,6 +7,7 @@ CONTROLLER_SRCS= \
$(DIR)/controller_block_trace.c \
$(DIR)/controller_bus.c \
$(DIR)/controller_command.c \
$(DIR)/controller_time.c \
$(DIR)/controller_trace.c \
$(DIR)/controller_sample.c \
$(DIR)/controller_dumpdot.c \


+ 47
- 29
controller/controller/controller_block.c View File

@ -27,6 +27,7 @@
#include <controller/controller_block.h>
#include <controller/controller_sample.h>
#include <controller/controller_bus.h>
#include <controller/controller_time.h>
#include <log/log.h>
int nr_blocks = 0;
@ -37,8 +38,9 @@ struct calculate_func {
struct controller_block *block;
};
static int nr_calculates = 0;
static struct calculate_func *calculates = NULL;
static int calculates_sub = 1;
static int calculates_sub_cur = 0;
static struct calculate_func **calculates = NULL;
static char *controller_block_context = "unknown";
@ -290,32 +292,42 @@ int controller_block_sample_init(void)
}
}
calculates = calloc(sizeof(struct calculate_func), 1);
for (i = 0; i < nr_blocks; i++) {
if (blocks[i]->calculate) {
log_send(LOG_T_DEBUG,
"Block %s has a calculate function",
blocks[i]->name);
tmp = realloc(calculates,
sizeof(struct calculate_func) * (nr_calculates+2));
if (tmp) {
calculates = tmp;
calculates[nr_calculates].func =
blocks[i]->calculate;
calculates[nr_calculates].block = blocks[i];
nr_calculates++;
calculates[nr_calculates].block = NULL;
int sub;
calculates_sub = controller_time_subsample_get();
calculates = calloc(sizeof(struct calculate_func *), calculates_sub);
for (sub = 0; sub < calculates_sub; sub++) {
int nr_calculates = 0;
calculates[sub] = calloc(sizeof(struct calculate_func), 1);
for (i = 0; i < nr_blocks; i++) {
if (blocks[i]->calculate && controller_time_subsample_member(blocks[i]->time, sub)) {
// log_send(LOG_T_DEBUG,
// "Block %s has a calculate function",
// blocks[i]->name);
tmp = realloc(calculates[sub],
sizeof(struct calculate_func) * (nr_calculates+2));
if (tmp) {
calculates[sub] = tmp;
calculates[sub][nr_calculates].func =
blocks[i]->calculate;
calculates[sub][nr_calculates].block = blocks[i];
nr_calculates++;
calculates[sub][nr_calculates].block = NULL;
} else {
log_send(LOG_T_ERROR,
"Out of memory allocating memory");
}
} else {
log_send(LOG_T_ERROR,
"Out of memory allocating memory");
// log_send(LOG_T_DEBUG,
// "Block %s has no calculate function",
// blocks[i]->name);
}
} else
log_send(LOG_T_DEBUG,
"Block %s has no calculate function",
blocks[i]->name);
}
log_send(LOG_T_DEBUG, "Found %d calculate functions in %d blocks for subsample %d",
nr_calculates, nr_blocks, sub);
}
log_send(LOG_T_DEBUG, "Found %d calculate functions in %d blocks",
nr_calculates, nr_blocks);
return 0;
}
@ -468,14 +480,14 @@ void controller_block_calculate(void)
struct calculate_func *calculate;
struct controller_block *block;
calculate = calculates;
for (calculate = calculates; true; calculate++) {
for (calculate = calculates[calculates_sub_cur]; true; calculate++) {
block = calculate->block;
if (!block)
return;
break;
calculate->func(block);
}
calculates_sub_cur++;
calculates_sub_cur %= calculates_sub;
}
@ -548,9 +560,15 @@ struct controller_block *controller_block_alloc(char *type, char *name,
if (!blk->private)
goto err_private;
}
blk->time = controller_time_get();
if (!blk->time)
goto err_time;
return blk;
err_time:
free(blk->private);
err_private:
free(blk->name);
err_name:


+ 3
- 0
controller/controller/controller_block.h View File

@ -140,6 +140,9 @@ struct controller_block {
void (*calculate)(struct controller_block *);
/* time domain this block belongs to */
struct controller_time *time;
/* Incomplete type for private data */
struct controller_block_private *private;
};


+ 0
- 5
controller/controller/controller_load.c View File

@ -238,11 +238,6 @@ void controller_load_var_clear(yyscan_t scanner)
}
}
void controller_load_frequency(double frequency)
{
controller_sample_frequency_set(frequency);
}
int controller_load_trigger(char *name, yyscan_t scanner)
{
struct controller_load_extra *extra = yyget_extra(scanner);


+ 0
- 1
controller/controller/controller_load_parser.l View File

@ -80,7 +80,6 @@ boolcast "("[ \t]*"bool"[ \t]*")"
"-" { return MINSYM; }
"alias" { return ALIASSYM; }
"frequency" { return FREQUENCYSYM; }
"trigger" { return TRIGGERSYM; }
"blocks" { return BLOCKSSYM; }
"links" { return LINKSSYM; }


+ 6
- 9
controller/controller/controller_load_parser.y View File

@ -29,6 +29,7 @@
#include <controller/controller_load_int.h>
#include <controller/controller_trace.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
#define YYERROR_VERBOSE
@ -60,7 +61,6 @@ void yyerror(yyscan_t *scanner, char const *s);
%token <integer> INTSYM
%token <dbl> DOUBLESYM
%token <ul> UNSIGNEDLONGSYM
%token FREQUENCYSYM
%token TRIGGERSYM
%token BLOCKSSYM
%token LINKSSYM
@ -111,7 +111,6 @@ ctrllist: ctrl
;
ctrl : aliases
| frequency
| triggers
| links
| blocks
@ -122,12 +121,6 @@ ctrl : aliases
| set
;
frequency: FREQUENCYSYM DOUBLESYM
{ controller_load_frequency($2); }
| FREQUENCYSYM INTSYM
{ controller_load_frequency($2); }
;
include: INCLUDESYM stringvar
{
if (controller_load_include($2)) {
@ -181,7 +174,11 @@ trigger : BRACEOPENSYM
}
blocks : BLOCKSSYM BRACEOPENSYM blocklist BRACECLOSESYM
blocks : BLOCKSSYM PARENTHESESOPENSYM doublevar COMMASYM doublevar
{
controller_time_param_set($3, $5);
}
PARENTHESESCLOSESYM BRACEOPENSYM blocklist BRACECLOSESYM
blocklist: block
| block blocklist


+ 6
- 0
controller/controller/controller_load_variable.c View File

@ -107,6 +107,9 @@ int controller_load_variable_int_get(char *varname)
struct variable *entry;
for (entry = variables; entry; entry = entry->next) {
if (!strcmp(entry->name, varname) &&
entry->type == VARIABLE_DOUBLE)
return entry->doubleval;
if (!strcmp(entry->name, varname) &&
entry->type == VARIABLE_INT)
return entry->intval;
@ -125,6 +128,9 @@ double controller_load_variable_double_get(char *varname)
if (!strcmp(entry->name, varname) &&
entry->type == VARIABLE_DOUBLE)
return entry->doubleval;
if (!strcmp(entry->name, varname) &&
entry->type == VARIABLE_INT)
return entry->intval;
}
log_send(LOG_T_ERROR, "Double variable %s does not exist", varname);


+ 11
- 11
controller/controller/controller_sample.c View File

@ -38,13 +38,13 @@
#include <controller/controller_block.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
#include <log/log.h>
#include <shell/shell.h>
static int nsec_interval;
static int controller_sampleoverruns = 0;
static double controller_frequency = 1.0;
static pthread_t controller_sample_thread;
static bool controller_sample_thread_running = false;
@ -496,15 +496,15 @@ int controller_sample_shell_add(void)
}
double controller_sample_period(void)
//TODO deprecated
double controller_sample_period(struct controller_block *block)
{
return 1.0 / controller_frequency;
}
void controller_sample_frequency_set(double frequency)
{
controller_frequency = frequency;
struct controller_time *t = NULL;
if (block)
t = block->time;
log_send(LOG_T_WARNING, "controller_sample_period() is deprecated!");
return controller_time_period_get(t);
}
@ -525,12 +525,12 @@ int controller_sample_start(void)
pthread_attr_t attr;
int i;
nsec_interval = 1000000000.0 / controller_frequency;
nsec_interval = controller_time_sample_period_get() * 1000000000.0;
if (-prestart > nsec_interval / 10)
prestart = -nsec_interval / 10;
log_send(LOG_T_DEBUG, "nsec_interval: %d %f",
nsec_interval, controller_frequency);
nsec_interval, controller_time_sample_frequency_get());
sample_timing_init(&st_start, "start");
sample_timing_init(&st_end, "end");


+ 1
- 3
controller/controller/controller_sample.h View File

@ -22,14 +22,12 @@
#include <stdio.h>
#include <unistd.h>
#include <stdbool.h>
#include <controller/controller_block.h>
int controller_sample_start(void);
void controller_sample_start_hook(void (*func)(void *arg), void *arg);
double controller_sample_period(void);
void controller_sample_frequency_set(double frequency);
bool controller_sample_context(void);
bool controller_sample_running(void);


+ 164
- 0
controller/controller/controller_time.c View File

@ -0,0 +1,164 @@
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 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/>.
*/
#include <controller/controller_time.h>
#include <log/log.h>
#include <stdlib.h>
#include <math.h>
struct controller_time {
double frequency;
double delay;
double period;
int sub;
int off;
struct controller_time *next;
};
static struct controller_time *time_list = NULL;
static double param_frequency = 1.0;
static double param_period = 1.0;
static double param_delay = 0.0;
void controller_time_param_set(double frequency, double delay)
{
param_frequency = frequency;
param_period = 1.0 / frequency;
param_delay = delay;
}
struct controller_time *controller_time_get(void)
{
struct controller_time *entry;
for (entry = time_list; entry; entry = entry->next) {
if (entry->frequency == param_frequency &&
entry->delay == param_delay) {
return entry;
}
}
if (param_delay >= param_period) {
log_send(LOG_T_ERROR,
"delay (%g) must be smaller than period (%g)",
param_delay, param_period);
return NULL;
}
entry = calloc(sizeof(struct controller_time), 1);
if (!entry)
return NULL;
entry->frequency = param_frequency;
entry->delay = param_delay;
entry->period = param_period;
entry->next = time_list;
time_list = entry;
return entry;
}
double controller_time_period_get(struct controller_time *t)
{
struct controller_time *entry;
double period = param_period;
if (t)
return t->period;
for (entry = time_list; entry; entry = entry->next) {
period = fmax(period, entry->period);
}
return period;
}
static double processed_frequency = 1.0;
static double processed_period = 0.0;
static int processed_sub = 1;
int controller_time_process(void)
{
struct controller_time *entry;
/* search for top frequency */
for (entry = time_list; entry; entry = entry->next) {
if (entry->frequency > processed_frequency)
processed_frequency = entry->frequency;
}
processed_period = 1.0 / processed_frequency;
log_send(LOG_T_DEBUG, "Time domains:");
for (entry = time_list; entry; entry = entry->next) {
double sub = processed_frequency / entry->frequency;
double off;
off = entry->delay / processed_period;
if (fmod(processed_frequency, entry->frequency) != 0.0) {
log_send(LOG_T_DEBUG, "Frequencies not divisable");
}
if (fmod(entry->delay, processed_period) != 0.0) {
log_send(LOG_T_DEBUG,
"Period (%g) and delay (%g) not divisable",
processed_period, entry->delay);
}
log_send(LOG_T_DEBUG,
"\tfrequency: %g Hz, period: %g sec, delay: %g sec, subsample: %g, offset: %g",
entry->frequency, entry->period, entry->delay, sub, off);
if (fmod(fmax(sub, processed_sub), fmin(sub, processed_sub)) != 0.0) {
processed_sub = sub * processed_sub;
} else if (sub > processed_sub) {
processed_sub = sub;
}
entry->sub = sub;
entry->off = off;
}
log_send(LOG_T_DEBUG,
"Sample frequency: %g Hz, period: %g sec, subsample: %d",
processed_frequency, processed_period, processed_sub);
return 0;
}
double controller_time_sample_frequency_get(void)
{
return processed_frequency;
}
double controller_time_sample_period_get(void)
{
return processed_period;
}
int controller_time_subsample_get(void)
{
return processed_sub;
}
bool controller_time_subsample_member(struct controller_time *time, int sub)
{
return !((sub - time->off) % time->sub);
}

+ 36
- 0
controller/controller/controller_time.h View File

@ -0,0 +1,36 @@
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 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/>.
*/
#ifndef _INCLUDE_CONTROLLER_TIME_H_
#define _INCLUDE_CONTROLLER_TIME_H_
#include <stdbool.h>
void controller_time_param_set(double frequency, double delay);
struct controller_time *controller_time_get();
double controller_time_period_get(struct controller_time *t);
int controller_time_process(void);
double controller_time_sample_frequency_get(void);
double controller_time_sample_period_get(void);
int controller_time_subsample_get(void);
bool controller_time_subsample_member(struct controller_time *time, int sub);
#endif /* _INCLUDE_CONTROLLER_TIME_H_ */

+ 3
- 3
controller/controller/controller_trace.c View File

@ -34,7 +34,7 @@
#include <controller/controller_block.h>
#include <controller/controller_trace.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
#include <log/log.h>
#include <trace/trace.h>
@ -138,7 +138,7 @@ static void *controller_trace_handle(void *arg)
fd_set fdrx;
int high;
struct timeval tv;
long usec = controller_sample_period() * 1000000;
long usec = controller_time_sample_period_get() * 1000000;
while (1) {
FD_ZERO(&fdrx);
@ -475,7 +475,7 @@ void controller_trace_server_start(int portnr, int max)
}
controller_trace_pkt_interval = trace_packet_new();
period = controller_sample_period();
period = controller_time_sample_period_get();
interval.tv_sec = period;
period -= interval.tv_sec;
period *= 1000000000.0;


+ 4
- 0
controller/dt_ctrl.c View File

@ -97,6 +97,10 @@ int main(int argc, char **argv)
free(dot_filename);
/* Start 'sample' */
if (controller_time_process()) {
goto err_init;
}
if (controller_block_sample_init()) {
goto err_init;


+ 3
- 2
controller/dt_ctrl.ctrl View File

@ -8,7 +8,8 @@
#
# Frequency is limited by the stoeber drives.
frequency 250
set frequency 250
set delay 0.0
trigger {
{ "prestart", 0.000150 }
@ -31,7 +32,7 @@ import "dt_ctrl_az_sim.ctrl"
import "dt_ctrl_ec_sim.ctrl"
blocks {
blocks ($(frequency), $(delay)) {
{ "setpoint_generator_3d", "azimuth_spg", "Azimuth_Setpoint", "rad" }
{ "servo_state", "azimuth_servo_state" }
{ "and2", "azimuth_safe_and" }


+ 1
- 1
controller/dt_ctrl_az.ctrl View File

@ -1,6 +1,6 @@
import "dt_ctrl_ec.ctrl"
blocks {
blocks ($(frequency), $(delay)) {
{ "stoeber", "dt_az",
"ethercat", # bus name
2, # ethercat position


+ 1
- 1
controller/dt_ctrl_az_sim.ctrl View File

@ -1,4 +1,4 @@
blocks {
blocks ($(frequency), $(delay)) {
{ "limit", "dt_az_speed_limit" }
{ "quantize", "dt_az_speed_quantize" }
{ "subtract", "dt_az_speed_fb" }


+ 1
- 1
controller/dt_ctrl_ec.ctrl View File

@ -1,4 +1,4 @@
blocks {
blocks ($(frequency), $(delay)) {
{ "ec", "ethercat",
"eth0", # interface
3, # number of slaves


+ 1
- 1
controller/dt_ctrl_ec_sim.ctrl View File

@ -1,3 +1,3 @@
blocks {
blocks ($(frequency), $(delay)) {
{ "ec_sim", "ethercat" }
}

+ 1
- 1
controller/dt_ctrl_el.ctrl View File

@ -1,6 +1,6 @@
import "dt_ctrl_ec.ctrl"
blocks {
blocks ($(frequency), $(delay)) {
{ "stoeber", "dt_el_r",
"ethercat", # bus name
0, # ethercat position


+ 1
- 1
controller/dt_ctrl_el_sim.ctrl View File

@ -1,4 +1,4 @@
blocks {
blocks ($(frequency), $(delay)) {
{ "limit", "dt_el_speed_limit_r" }
{ "quantize", "dt_el_speed_quantize_r" }
{ "subtract", "dt_el_speed_fb_r" }


+ 4
- 2
controller/ec/ec.c View File

@ -34,6 +34,7 @@
#include <pthread.h>
#include <semaphore.h>
#include <controller/controller_time.h>
#include <controller/controller_sample.h>
#include <controller/controller_block.h>
#include <ec/ec.h>
@ -623,13 +624,14 @@ static void ec_datagram_sample_init(void *arg)
}
}
timeout_sec = controller_sample_period() * 200;
timeout_sec = controller_time_period_get(ec_bus->owner->time) * 200;
timeout.tv_sec = (int)(timeout_sec);
timeout.tv_usec = (int)(timeout_sec * 1000000) % 1000000;
log_send(LOG_T_INFO,
"Setting ethercat rx/tx timeout to %d.%06d sec based on %f sec period",
(int)timeout.tv_sec, (int)timeout.tv_usec, controller_sample_period());
(int)timeout.tv_sec, (int)timeout.tv_usec,
controller_time_period_get(ec_bus->owner->time));
eth_timeout_set(ec_sock, &timeout, &timeout);


+ 2
- 2
controller/ec/esc_esi.c View File

@ -37,7 +37,7 @@
#include <ec/esc_mailbox.h>
#include <ec/esc_watchdog.h>
#include <log/log.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
#define MAX_RETRIES 1000
@ -394,7 +394,7 @@ static int esc_esi_device_fill_dc(struct esc_device *dev)
double period;
uint32_t period_cycle;
period = controller_sample_period();
period = controller_time_period_get(dev->block_tx->time);
period_cycle = (uint32_t)(period * 1000 * 1000 * 1000);
for (cat = dev->categories;


+ 2
- 2
controller/ec/esc_watchdog.c View File

@ -27,7 +27,7 @@
#include <ec/esc_watchdog.h>
#include <ec/esc_registers.h>
#include <controller/controller_sample.h>
#include <controller/controller_time.h>
#include <log/log.h>
int esc_watchdog_enable(struct esc_device *dev, struct timespec *timeout)
@ -92,7 +92,7 @@ int esc_watchdog_init(struct esc_device *dev)
int r = 0;
double timeout_sec;
timeout_sec = controller_sample_period() * 2;
timeout_sec = controller_time_period_get(dev->block_tx->time) * 2;
addr.addr.position.off = ESC_ADDR_MAP_WATCHDOG_COUNTER_PD;
val8 = 0;


+ 3
- 1
controller/test/build.mk View File

@ -31,5 +31,7 @@ SRCS += $(TEST_BLOCK_SRCS)
CTRL_TESTS += \
$(DIR)/test.ctrl
$(DIR)/test.ctrl \
$(DIR)/test_multifrequency.ctrl \

+ 6
- 3
controller/test/test.ctrl View File

@ -1,10 +1,9 @@
frequency 100
trigger {
{ "immediate" }
}
blocks {
blocks (100.0, 0.0) {
{ "test_input_bool", "input_bool" }
{ "test_input_uint32", "input_uint32" }
{ "test_input_float", "input_float" }
@ -14,8 +13,12 @@ blocks {
{ "test_output_float", "output_float" }
}
alias {
{ "input_bool", "input_bool", "value" }
}
links {
{ "input_bool", "value", "output_bool", "value", true }
{ $<input_bool>, "output_bool", "value", true }
{ "input_uint32", "value", "output_uint32", "value", true }
{ "input_float", "value", "output_float", "value", true }
}


+ 84
- 0
controller/test/test_multifrequency.ctrl View File

@ -0,0 +1,84 @@
# Test multiple frequencies and multiple phases
trigger {
{ "immediate" }
}
blocks (100.0, 0.0) {
{ "test_input_uint32", "input100" }
{ "test_output_uint32", "output100" }
{ "test_output_uint32", "output10_100" }
}
blocks (10.0, 0.01) {
{ "test_input_uint32", "input10" }
{ "test_output_uint32", "output10" }
{ "test_output_uint32", "output100_10" }
}
blocks (25.0, 0.02) {
{ "test_input_uint32", "input25" }
{ "test_output_uint32", "output25" }
{ "test_output_uint32", "output100_25" }
}
blocks (10.0, 0.03) {
{ "test_output_uint32", "output100_10_3" }
}
links {
{ "input100", "value", "output100", "value", true }
{ "input100", "value", "output100_10", "value", true }
{ "input100", "value", "output100_10_3", "value", true }
{ "input100", "value", "output100_25", "value", true }
{ "input10", "value", "output10", "value", true }
{ "input10", "value", "output10_100", "value", true }
{ "input25", "value", "output25", "value", true }
}
params {
{ "input100", "value", 40, (int) {
0, 1, 2, 3, 4, 5, 6, 7, 8, 9,
10, 11, 12, 13, 14, 15, 16, 17, 18, 19,
20, 21, 22, 23, 24, 25, 26, 27, 28, 29,
30, 31, 32, 33, 34, 35, 36, 37, 38, 39 }
}
{ "input10", "value", 20, (int) {
0, 1, 2, 3, 4, 5, 6, 7, 8, 9,
10, 11, 12, 13, 14, 15, 16, 17, 18, 19 }
}
{ "input25", "value", 20, (int) {
0, 1, 2, 3, 4, 5, 6, 7, 8, 9,
10, 11, 12, 13, 14, 15, 16, 17, 18, 19 }
}
{ "output100", "value", 20, (int) {
0, 1, 2, 3, 4, 5, 6, 7, 8, 9,
10, 11, 12, 13, 14, 15, 16, 17, 18, 19 }
}
{ "output10", "value", 20, (int) {
0, 1, 2, 3, 4, 5, 6, 7, 8, 9,
10, 11, 12, 13, 14, 15, 16, 17, 18, 19 }
}
{ "output100_10", "value", 5, (int) {
1, 11, 21, 31, 39 }
}
{ "output10_100", "value", 20, (int) {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 1, 1, 1, 1, 1, 1, 1, 1 }
}
{ "output100_25", "value", 10, (int) {
2, 6, 10, 14, 18, 22, 26, 30, 34, 38 }
}
{ "output25", "value", 20, (int) {
0, 1, 2, 3, 4, 5, 6, 7, 8, 9,
10, 11, 12, 13, 14, 15, 16, 17, 18, 19 }
}
{ "output100_10_3", "value", 5, (int) {
3, 13, 23, 33, 39 }
}
}

Loading…
Cancel
Save