Commit 89022ffc authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Merge branch 'build' into beaglebone

parents e9e91045 98c41253
......@@ -120,6 +120,7 @@ TARGETS:=
include $(DIR)/$1/build.mk
targets_$$(DIR): $$(TARGETS)
$$(TARGETS): $(DIR)/$1/build.mk
TARGETS:= $$($(DIR)_TMPTARGETS) $$(TARGETS)
......@@ -127,6 +128,10 @@ DIR := $(DIR)
endef
CFLAGS := -pthread -Icommon -Icommon/include -Icontroller -Iconsole/console -Iinclude
LDFLAGS := -pthread -Lcommon/lib -Lcontroller/lib -Lconsole/console/lib -Llib -Wl,--as-needed
VPATH += common/lib controller/lib console/console/lib lib
TARGETS:=
$(eval $(call SUBDIR,common))
......@@ -140,9 +145,6 @@ TARGETS:=
$(eval $(call SUBDIR,console))
console: $(TARGETS)
CFLAGS := -Icommon -Icommon/include -Icontroller -Iconsole/console -Iinclude
LDFLAGS := -Lcommon/lib -Lcontroller/lib -Lconsole/console/lib -Llib -Wl,--as-needed
VPATH += common/lib controller/lib console/console/lib lib
SRCS_TMP:=$(SRCS:.il=.il2c.d)
......
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2008, 2013
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2008, 2013, 2014
Copyright Stichting C.A. Muller Radioastronomiestation, 2008, 2013
This program is free software: you can redistribute it and/or modify
......@@ -35,6 +35,7 @@
#include <limits.h>
#include <stdarg.h>
#include <stdbool.h>
#include <sched.h>
#include <utils/tcp_connect.h>
......@@ -57,7 +58,7 @@ struct log_client {
#define LOG_MAX_LEN 400
struct log_msg {
int used;
volatile int used;
time_t t;
int type;
char msg[LOG_MAX_LEN];
......@@ -68,6 +69,7 @@ static struct log_client clients[LOG_MAX_CLIENTS];
static struct log_msg msgs[LOG_MAX_QUEUE];
static int msgs_wr = 0;
static int msgs_rd = 0;
static int queue_size = 0;
static int lost_counter = 0;
static sem_t queue_wait;
......@@ -75,44 +77,59 @@ static sem_t queue_wait;
static enum log_type log_level_console;
static enum log_type log_level_remote;
static pthread_mutex_t log_mutex;
void log_send(enum log_type type, char *fmt, ...)
{
int val;
va_list ap;
int msg;
bool log_lost = false;
if (log_level_console < type && log_level_remote < type)
return;
va_start(ap, fmt);
pthread_mutex_lock(&log_mutex);
msg = __sync_fetch_and_add(&msgs_wr, 1);
msg %= LOG_MAX_QUEUE;
if (msgs[msgs_wr].used == 0) {
msgs[msgs_wr].t = time(NULL);
msgs[msgs_wr].type = type;
vsnprintf(msgs[msgs_wr].msg, LOG_MAX_LEN, fmt, ap);
msgs[msgs_wr].used = 1;
msgs_wr++;
msgs_wr %= LOG_MAX_QUEUE;
if (msgs[msg].used == 0) {
int lost;
lost = __sync_fetch_and_and(&lost_counter, 0);
if (lost) {
__sync_fetch_and_add(&queue_size, 1);
msgs[msg].t = time(NULL);
msgs[msg].type = LOG_T_ERROR;
snprintf(msgs[msg].msg, LOG_MAX_LEN,
"Lost %d messages", lost);
__sync_synchronize();
msgs[msg].used = 1;
/* Now try to get another entry for the real msg */
msg = __sync_fetch_and_add(&msgs_wr, 1);
msg %= LOG_MAX_QUEUE;
}
if (msgs[msg].used == 0) {
__sync_fetch_and_add(&queue_size, 1);
msgs[msg].t = time(NULL);
msgs[msg].type = type;
vsnprintf(msgs[msg].msg, LOG_MAX_LEN, fmt, ap);
__sync_synchronize();
msgs[msg].used = 1;
} else {
log_lost = true;
}
} else {
lost_counter++;
log_lost = true;
}
/* If we lost some in the past check if we can log now... */
if (lost_counter && msgs[msgs_wr].used == 0) {
msgs[msgs_wr].t = time(NULL);
msgs[msgs_wr].type = LOG_T_WARNING;
snprintf(msgs[msgs_wr].msg, LOG_MAX_LEN,
"Lost %d messages", lost_counter);
msgs[msgs_wr].used = 1;
msgs_wr++;
msgs_wr %= LOG_MAX_QUEUE;
lost_counter = 0;
if (log_lost) {
__sync_fetch_and_add(&lost_counter, 1);
}
pthread_mutex_unlock(&log_mutex);
va_end(ap);
sem_getvalue(&queue_wait, &val);
......@@ -228,6 +245,8 @@ static void *log_server(void *arg)
char *header;
struct tm gmt;
__sync_synchronize();
gmtime_r(&msgs[msgs_rd].t, &gmt);
timestamp(&gmt, time);
......@@ -260,10 +279,13 @@ static void *log_server(void *arg)
if (msgs[msgs_rd].type <= log_level_console)
printf("%s %s%s\n", time, header, msgs[msgs_rd].msg);
__sync_synchronize();
msgs[msgs_rd].used = 0;
msgs_rd++;
msgs_rd %= LOG_MAX_QUEUE;
__sync_fetch_and_sub(&queue_size, 1);
}
}
......@@ -320,6 +342,8 @@ static void *log_client(void *arg)
char *header;
struct tm gmt;
__sync_synchronize();
gmtime_r(&msgs[msgs_rd].t, &gmt);
timestamp(&gmt, time);
......@@ -356,9 +380,12 @@ static void *log_client(void *arg)
if (msgs[msgs_rd].type <= log_level_console)
printf("%s %s%s\n", time, header, msgs[msgs_rd].msg);
__sync_synchronize();
msgs[msgs_rd].used = 0;
msgs_rd++;
msgs_rd %= LOG_MAX_QUEUE;
__sync_fetch_and_sub(&queue_size, 1);
}
}
......@@ -406,10 +433,11 @@ int log_client_start(char *host, int port, enum log_type console_level,
void log_server_flush(void)
{
sleep(1);
do {
sleep(1);
} while (msgs[msgs_rd].used);
sem_post(&queue_wait);
sched_yield();
__sync_synchronize();
} while (queue_size);
}
void log_string(char *str, size_t len, enum log_type type, char *fmt, ...)
......
......@@ -6,7 +6,8 @@ TARGETS += $(DIR)/weather_test
WEATHER_SRCS := $(DIR)/weather_test.c $(DIR)/weather.c $(DIR)/tcp_connect.c
WEATHER_OBJS := $(WEATHER_SRCS:.c=.o)
$(DIR)/weather_test_LDFLAGS += -lpthread
$(DIR)/weather_test_CFLAGS += -pthread
$(DIR)/weather_test_LDFLAGS += -pthread
$(DIR)/weather_test: $(WEATHER_OBJS)
ARCHSRCS := $(DIR)/tcp_connect.c \
......@@ -21,8 +22,8 @@ ARCHSRCS := $(DIR)/tcp_connect.c \
ARCHOBJS := $(ARCHSRCS:.c=.lo)
LU_CFLAGS := -Wall -g -fPIC
LU_LDFLAGS := -lpthread -lrt -lm
LU_CFLAGS := -Wall -g -fPIC -pthread
LU_LDFLAGS := -lrt -lm
LU_CFLAGS += `pkg-config --cflags glib-2.0`
LU_LDFLAGS += -lglib-2.0
......
all:
@$(MAKE) --no-print-directory -C .. targets_controller
$(MAKECMDGOALS):
@$(MAKE) --no-print-directory -C .. $(MAKECMDGOALS)
all:
@$(MAKE) --no-print-directory -C ../.. targets_controller/block
$(MAKECMDGOALS):
@$(MAKE) --no-print-directory -C ../.. $(MAKECMDGOALS)
/*
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 */
uint64_t period_nsec;
/* 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;
uint64_t command_t;
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;
priv->cur_x = priv->cmd_x;
priv->cur_done = true;
priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
priv->cmd_v = 0.0;
priv->cur_v = 0.0;
return;
}
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) {
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) {
uint64_t command_t;
int64_t t;
command_t = priv->cur_command.t.tv_nsec +
priv->cur_command.t.tv_sec * 1000000000;
priv->command_t = command_t;
t = (command_t - controller_time_nseconds) / priv->period_nsec;
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 < %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.0);
priv->cur_start = true;
}
}
if (priv->command_t <= controller_time_nseconds) {
priv->cur_done = true;
priv->cmd_x =
priv->cur_command.value.f -
priv->cmd_v;
}
break;
default:
break;
}
}
if (priv->cur_command.type == COMMAND_PTYPE_SPEED) {
cur_v = priv->cmd_v * controller_sample_period();
priv->cmd_x = cur_x + cur_v;
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;
} else {
float x_diff;
priv->cmd_x += priv->cmd_v;
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();
spg->private->period_nsec = controller_sample_period() * 1000000000;
}
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(