Commit 2b2fee09 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Use controller_trigger_time for timing during sample as much as possible.

Remove now unneeded nsec base time.
Command handling in the sample now uses trigger time.
Trace handling still has to be done, then all nsec timers can be removed
from the sample.
parent f39c9668
......@@ -36,7 +36,7 @@ struct controller_block_private {
static void command_calculate(struct controller_block *cmd)
{
struct controller_block_private *priv = cmd->private;
struct command_entry c_entry;
struct controller_command_entry c_entry;
if (!controller_command_queue_read(priv->command, &c_entry)) {
priv->value = c_entry.value.b;
......
......@@ -36,7 +36,7 @@ struct controller_block_private {
static void command_calculate(struct controller_block *cmd)
{
struct controller_block_private *priv = cmd->private;
struct command_entry c_entry;
struct controller_command_entry c_entry;
if (!controller_command_queue_read(priv->command, &c_entry)) {
priv->value = c_entry.value.f;
......
......@@ -78,7 +78,7 @@ static void servo_state_calculate(struct controller_block *servo_state)
struct controller_block_private *priv = servo_state->private;
bool safe = *priv->safe;
bool unsafe_disable = false;
struct command_entry c_entry;
struct controller_command_entry c_entry;
if (!controller_command_queue_read(priv->command, &c_entry)) {
priv->enable_param = c_entry.value.b;
......
......@@ -27,6 +27,7 @@
#include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <controller/controller_time.h>
#include <controller/controller_sample.h>
#include <log/log.h>
/*
......@@ -72,7 +73,7 @@ struct controller_block_private {
/* conversion factor for seconds/tick and its inverse */
float freq; /* ticks per second */
uint64_t period_nsec;
controller_trigger_time period;
/* parameters in real world format (time unit: second) */
float max_v_sec;
......@@ -83,7 +84,7 @@ struct controller_block_private {
uint32_t id;
struct command_entry cur_command;
struct controller_command_entry cur_command;
bool cur_done;
bool cur_start;
uint64_t command_t;
......@@ -101,7 +102,7 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
cur_v = priv->cur_v;
if (*priv->reset) {
struct command_entry entry;
struct controller_command_entry entry;
priv->cmd_x = *priv->reset_x;
priv->cur_x = priv->cmd_x;
......@@ -146,20 +147,18 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
break;
case COMMAND_PTYPE_SETPOINT_TIME:
if (!priv->cur_start) {
uint64_t command_t;
controller_trigger_time command_t;
int64_t t;
command_t = (uint64_t)priv->cur_command.t.tv_nsec +
(uint64_t)priv->cur_command.t.tv_sec * 1000000000;
priv->command_t = command_t;
command_t = priv->cur_command.t;
t = (command_t - controller_time_nseconds) / priv->period_nsec;
t = (command_t - controller_sample_timestamp) / priv->period;
if (t < 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 < %lld)",
spg->name, (long long)priv->cur_command.t.tv_sec, (long long)controller_time_seconds);
spg->name, (long long)command_t, (long long)controller_sample_timestamp);
priv->cmd_x = priv->cur_command.value.f;
priv->cmd_v = 0.0;
priv->cur_done = true;
......@@ -171,7 +170,7 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
}
}
if (priv->command_t <= controller_time_nseconds) {
if (priv->cur_command.t <= controller_sample_timestamp) {
priv->cur_done = true;
priv->cmd_x =
priv->cur_command.value.f -
......@@ -252,11 +251,16 @@ static int block_setpoint_generator_command_filter(struct controller_command *co
static void scale(struct controller_block *spg)
{
double tick = controller_time_period_get(spg->time);
struct timespec ts;
/* Scale all settings to sample time unit */
spg->private->max_v = spg->private->max_v_sec * controller_time_period_get(spg->time);
spg->private->max_v = spg->private->max_v_sec * tick;
spg->private->freq = 1.0 / controller_time_period_get(spg->time);
spg->private->period_nsec = controller_time_period_get(spg->time) * 1000000000;
spg->private->freq = 1.0 / tick;
ts.tv_sec = tick;
ts.tv_nsec = (uint64_t)(tick * 1000000000) % 1000000000;
spg->private->period = controller_sample_trigger->timespec2timestamp(&ts);
}
static int param_set_setpoint(struct controller_block *spg, char *param,
......
......@@ -27,6 +27,7 @@
#include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <controller/controller_time.h>
#include <controller/controller_sample.h>
#include <log/log.h>
/*
......@@ -101,7 +102,7 @@ struct controller_block_private {
float freq3;
double t_max_a;
double v_delta_from_max_a;
uint64_t period_nsec;
controller_trigger_time period;
/* parameters in real world format (time unit: second) */
float max_v_sec;
......@@ -131,7 +132,7 @@ struct controller_block_private {
uint32_t id;
struct command_entry cur_command;
struct controller_command_entry cur_command;
bool cur_done;
bool cur_start;
uint64_t command_t;
......@@ -233,7 +234,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
t_max_a = priv->t_max_a;
if (*priv->reset) {
struct command_entry entry;
struct controller_command_entry entry;
priv->cmd_x = *priv->reset_x;
cur_x = priv->cmd_x;
......@@ -285,20 +286,18 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
break;
case COMMAND_PTYPE_SETPOINT_TIME:
if (!priv->cur_start) {
uint64_t command_t;
controller_trigger_time command_t;
int64_t t;
command_t = (uint64_t)priv->cur_command.t.tv_nsec +
(uint64_t)priv->cur_command.t.tv_sec * 1000000000;
priv->command_t = command_t;
command_t = priv->cur_command.t;
t = (command_t - controller_time_nseconds) / priv->period_nsec;
t = (command_t - controller_sample_timestamp) / priv->period;
if (t < 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 < %lld)",
spg->name, (long long)priv->cur_command.t.tv_sec, (long long)controller_time_seconds);
spg->name, (long long)priv->cur_command.t, (long long)controller_sample_timestamp);
priv->cmd_x = priv->cur_command.value.f;
priv->cmd_v = 0.0;
priv->cur_done = true;
......@@ -310,7 +309,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
}
}
if (priv->command_t <= controller_time_nseconds) {
if (priv->cur_command.t <= controller_sample_timestamp) {
priv->cur_done = true;
priv->cmd_x =
priv->cur_command.value.f -
......@@ -813,6 +812,7 @@ static void scale(struct controller_block *spg)
{
double t_max_a;
double tick = controller_time_period_get(spg->time);
struct timespec ts;
/* Scale all settings to sample time unit */
spg->private->max_v = spg->private->max_v_sec * tick;
......@@ -827,7 +827,9 @@ 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_time_period_get(spg->time) * 1000000000;
ts.tv_sec = tick;
ts.tv_nsec = (uint64_t)(tick * 1000000000) % 1000000000;
spg->private->period = controller_sample_trigger->timespec2timestamp(&ts);
/* Calculate delta v when comming from max_a */
t_max_a = ticks_to_a(spg->private, 0, spg->private->max_a);
......
......@@ -61,7 +61,7 @@ void il2c_program(char *program)
fprintf(il2c_cout, "#include <string.h>\n");
fprintf(il2c_cout, "#include <math.h>\n");
fprintf(il2c_cout, "#include <controller/controller_block.h>\n");
fprintf(il2c_cout, "#include <controller/controller_time.h>\n");
fprintf(il2c_cout, "#include <controller/controller_sample.h>\n");
fprintf(il2c_cout, "#include <log/log.h>\n");
fprintf(il2c_cout, "\n");
}
......@@ -656,7 +656,7 @@ void il2c_not(void)
char *il2c_system_time(void)
{
return "(uint32_t)(controller_time_nseconds/1000)";
return "(uint32_t)(controller_sample_timestamp)";
}
void il2c_minmax(bool max, char *val)
......
......@@ -154,6 +154,7 @@ static void handler_name(struct command *command, char *name)
static void handler_entry(struct command *command, struct command_entry *entry)
{
struct command_hdl *hdl = command->private;
struct controller_command_entry centry;
if (hdl->ccommand->filter) {
if (hdl->ccommand->filter(hdl->ccommand, entry)) {
......@@ -163,8 +164,13 @@ static void handler_entry(struct command *command, struct command_entry *entry)
return;
}
}
centry.type = entry->type;
centry.id = entry->id;
centry.value.u32 = entry->value.u32;
centry.t = controller_sample_trigger->timespec2timestamp(&entry->t);
if (controller_command_queue_write(hdl->ccommand, entry)) {
if (controller_command_queue_write(hdl->ccommand, &centry)) {
log_send(LOG_T_WARNING,
"%s: Command queue full, dropping command",
hdl->ccommand->name);
......
......@@ -20,11 +20,29 @@
#define _INCLUDE_CONTROLLER_COMMAND_H_
#include <controller/controller_block.h>
#include <controller/controller_trigger.h>
#include <command/command.h>
#define CONTROLLER_COMMAND_QUEUE_DEPTH 128
/* command entry with a controller 'perspective' */
struct controller_command_entry {
enum command_ptype type;
uint32_t id;
union {
float f;
bool b;
uint8_t u8;
uint16_t u16;
uint32_t u32;
int8_t s8;
int16_t s16;
int32_t s32;
} value;
controller_trigger_time t;
};
struct controller_command {
struct controller_block *block; /* owner */
char *name; /* outside name */
......@@ -36,7 +54,7 @@ struct controller_command {
/* returns zero if command ok. */
int (*filter)(struct controller_command *command, struct command_entry *entry);
struct command_entry queue[CONTROLLER_COMMAND_QUEUE_DEPTH];
struct controller_command_entry queue[CONTROLLER_COMMAND_QUEUE_DEPTH];
int queue_w;
int queue_r;
};
......@@ -51,7 +69,7 @@ struct controller_command *controller_command_find_by_name(char *name);
void controller_command_server_start(int portnr, int max);
static inline int controller_command_queue_read(struct controller_command *command, struct command_entry *entry)
static inline int controller_command_queue_read(struct controller_command *command, struct controller_command_entry *entry)
{
int qr = command->queue_r;
int qw = command->queue_w;
......@@ -59,7 +77,7 @@ static inline int controller_command_queue_read(struct controller_command *comma
if (qr != qw) {
__sync_synchronize();
memcpy(entry, &command->queue[qr], sizeof(struct command_entry));
memcpy(entry, &command->queue[qr], sizeof(struct controller_command_entry));
qr++;
qr %= CONTROLLER_COMMAND_QUEUE_DEPTH;
......@@ -73,7 +91,7 @@ static inline int controller_command_queue_read(struct controller_command *comma
static inline int controller_command_queue_write(
struct controller_command *command,
struct command_entry *entry)
struct controller_command_entry *entry)
{
int qw = command->queue_w;
int qwn = (qw + 1) % CONTROLLER_COMMAND_QUEUE_DEPTH;
......@@ -85,7 +103,7 @@ static inline int controller_command_queue_write(
if (qwn == qr) {
return -1;
} else {
memcpy(&command->queue[qw], entry, sizeof(struct command_entry));
memcpy(&command->queue[qw], entry, sizeof(struct controller_command_entry));
__sync_synchronize();
command->queue_w = qwn;
......
......@@ -277,7 +277,7 @@ int controller_load_trigger(char *name, yyscan_t scanner)
{
struct controller_load_extra *extra = yyget_extra(scanner);
return controller_sample_trigger(name, extra->va_list_argc,
return controller_sample_trigger_create(name, extra->va_list_argc,
extra->va_list, extra->va_type_list);
}
......
......@@ -49,6 +49,8 @@
static int nsec_interval;
static uint32_t controller_samplenr = 0;
controller_trigger_time controller_sample_timestamp;
static pthread_t controller_sample_thread;
static bool controller_sample_thread_running = false;
......@@ -191,7 +193,7 @@ static void cpu_bind(void)
}
static struct controller_trigger *trigger = NULL;
struct controller_trigger *controller_sample_trigger = NULL;
static controller_trigger_time (*wait_next)(struct controller_trigger *) = NULL;
static controller_trigger_time (*timestamp)(void) = NULL;
......@@ -235,8 +237,9 @@ static void *sample_thread(void *arg)
t.tv_sec = 0;
t.tv_nsec = nsec_interval;
trigger->init(trigger, &t);
log_time_source_set(trigger->timestamp, trigger->timestamp2timespec);
controller_sample_trigger->init(controller_sample_trigger, &t);
log_time_source_set(controller_sample_trigger->timestamp,
controller_sample_trigger->timestamp2timespec);
while (1) {
/* Pre sample stuff: */
......@@ -244,10 +247,10 @@ static void *sample_thread(void *arg)
/* Wait for the right moment...
(Internal timer or external interrupt) */
t_trig = wait_next(trigger);
t_trig = wait_next(controller_sample_trigger);
trigger->timestamp2timespec(&t, t_trig);
controller_time_nseconds = t_trig;
controller_sample_trigger->timestamp2timespec(&t, t_trig);
controller_sample_timestamp = t_trig;
controller_time_seconds = t.tv_sec;
controller_time_nsec = t.tv_nsec;
......@@ -291,7 +294,8 @@ static void *sample_monitor(void *arg)
linebuf[200] = 0;
while(1) {
log_send(LOG_T_DEBUG, "Samplenr: %d\tOverruns: %d",
controller_samplenr, trigger->overruns(trigger));
controller_samplenr,
controller_sample_trigger->overruns(controller_sample_trigger));
sample_timing_snprintf(&st_start, linebuf, 200);
log_send(LOG_T_DEBUG, "%s", linebuf);
sample_timing_snprintf(&st_end, linebuf, 200);
......@@ -377,7 +381,7 @@ int controller_sample_start(void)
pthread_attr_t attr;
int i;
if (!trigger) {
if (!controller_sample_trigger) {
log_send(LOG_T_DEBUG, "No trigger defined");
return -1;
}
......@@ -410,7 +414,7 @@ int controller_sample_start(void)
#define PREFIX "trigger_"
#define POSTFIX "_create_struct"
int controller_sample_trigger(char *name, int argc, va_list ap,
int controller_sample_trigger_create(char *name, int argc, va_list ap,
char **arg_types)
{
void *handle;
......@@ -445,14 +449,14 @@ int controller_sample_trigger(char *name, int argc, va_list ap,
}
// Try to create the actual trigger.
trigger = create->create(argc, ap);
if (!trigger) {
controller_sample_trigger = create->create(argc, ap);
if (!controller_sample_trigger) {
log_send(LOG_T_ERROR, "%s creation failed", name);
ret = -1;
}
wait_next = trigger->next;
timestamp = trigger->timestamp;
wait_next = controller_sample_trigger->next;
timestamp = controller_sample_trigger->timestamp;
errout:
dlclose(handle);
......
......@@ -23,6 +23,7 @@
#include <unistd.h>
#include <stdbool.h>
#include <controller/controller_block.h>
#include <controller/controller_trigger.h>
int controller_sample_start(void);
......@@ -33,8 +34,11 @@ bool controller_sample_running(void);
int controller_sample_shell_add(void);
int controller_sample_trigger(char *name, int argc, va_list ap,
int controller_sample_trigger_create(char *name, int argc, va_list ap,
char **arg_types);
extern controller_trigger_time controller_sample_timestamp;
extern struct controller_trigger *controller_sample_trigger;
#endif /* _INCLUDE_CONTROLLER_SAMPLE_ */
......@@ -21,7 +21,6 @@
#include <math.h>
uint64_t controller_time_nseconds = 0;
uint64_t controller_time_seconds = 0;
uint64_t controller_time_nsec = 0;
......
......@@ -21,9 +21,6 @@
#include <stdbool.h>
#include <stdint.h>
/* 64bit continuous counter */
extern uint64_t controller_time_nseconds;
extern uint64_t controller_time_seconds;
extern uint64_t controller_time_nsec;
......
......@@ -18,8 +18,6 @@
#ifndef _INCLUDE_CONTROLLER_TRIGGER_H_
#define _INCLUDE_CONTROLLER_TRIGGER_H_
#include <controller/controller_time.h>
#include <stddef.h>
#include <inttypes.h>
#include <stdarg.h>
......
......@@ -23,7 +23,7 @@
#include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <controller/controller_time.h>
#include <controller/controller_sample.h>
#include <test/test_block.h>
#include <log/log.h>
......@@ -42,8 +42,12 @@ static void calculate_test_command(struct controller_block *block)
struct controller_block_private *priv = block->private;
if (!priv->start_sec) {
priv->start_sec = controller_time_seconds;
priv->start_nsec = controller_time_nseconds % 1000000000;
struct timespec t;
controller_sample_trigger->timestamp2timespec(
&t, controller_sample_timestamp);
priv->start_sec = t.tv_sec;
priv->start_nsec = t.tv_nsec;
log_send(LOG_T_DEBUG, "%s: Start time: %" PRIu64 ".%09" PRIu32,
block->name, priv->start_sec, priv->start_nsec);
}
......@@ -51,6 +55,7 @@ static void calculate_test_command(struct controller_block *block)
if (priv->cur < priv->entries_nr) {
struct command_entry *entry;
struct controller_command *command;
struct controller_command_entry centry;
int r = 0;
entry = &priv->entries[priv->cur];
......@@ -64,8 +69,13 @@ static void calculate_test_command(struct controller_block *block)
command->name, priv->cur);
if (command->filter)
r = command->filter(command, entry);
if (r == 0)
controller_command_queue_write(command, entry);
if (r == 0) {
centry.type = entry->type;
centry.id = entry->id;
centry.value.u32 = entry->value.u32;
centry.t = controller_sample_trigger->timespec2timestamp(&entry->t);
controller_command_queue_write(command, &centry);
}
priv->cur++;
if (priv->cur == priv->entries_nr)
......
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