Add id to commands

Fix block dependencies

common/command/command.c

@@ -177,6 +177,7 @@ int command_packet_entry_set(struct command_pkt *pkt,
 	size_t vsize;
 	size_t size;
 	unsigned char *p;
+	struct command_id *id;
 	
 	switch (type) {
 		default:
@@ -196,10 +197,10 @@ int command_packet_entry_set(struct command_pkt *pkt,
 			break;
 	}
 	
+	size = sizeof(struct command_id);
+	size += vsize;
 	if (entry->type == COMMAND_PTYPE_SETPOINT_TIME) {
-		size = 8 + 4 + vsize;
-	} else {
-		size = vsize;
+		size += sizeof(struct command_timestamp);
 	}
 	
 	pkt->len = size + sizeof(struct command_header);
@@ -208,6 +209,10 @@ int command_packet_entry_set(struct command_pkt *pkt,
 	
 	p = &pkt->data[1];
 	
+	id = (void *)p;
+	id->id = htobe32(entry->id);
+	p += sizeof(struct command_id);
+	
 	if (entry->type == COMMAND_PTYPE_SETPOINT_TIME) {
 		struct command_timestamp *t = (void *)p;
 		
@@ -329,9 +334,10 @@ int command_handle(struct command *command, fd_set *set)
 				}
 				case COMMAND_PTYPE_SETPOINT_TIME: {
 					struct command_timestamp *t;
+					size_t pos_id = pos + sizeof(struct command_id);
 					
-					if (pos < pkt->len) {
-						t = (void *)pkt->data + pos;
+					if (pos_id < pkt->len) {
+						t = (void *)pkt->data + pos_id;
 						entry.t.tv_sec = be64toh(t->sec);
 						entry.t.tv_nsec = be32toh(t->nsec);
 					}
@@ -341,11 +347,15 @@ int command_handle(struct command *command, fd_set *set)
 				case COMMAND_PTYPE_SPEED:
 				case COMMAND_PTYPE_SETPOINT: {
 					struct command_ptype_value *v;
+					struct command_id *id;
+					size_t pos_id = pos + sizeof(struct command_id);
 					
 					entry.type = pkt->data[0];
 					
-					if (pos < pkt->len) {
-						v = (void *)pkt->data + pos;
+					if (pos_id < pkt->len) {
+						id = (void *)pkt->data + pos;
+						entry.id = be32toh(id->id);
+						v = (void *)pkt->data + pos_id;
 						switch (command->type) {
 							case COMMAND_VALUE_TYPE_FLOAT:
 							case COMMAND_VALUE_TYPE_UINT32:

common/command/command.h

@@ -63,8 +63,11 @@ enum command_state {
 
 char *enum_command_state2str(enum command_state state);
 
+#define COMMAND_ID_NONE	0xffffffff
+
 struct command_entry {
 	enum command_ptype type;
+	uint32_t id;
 	union {
 		float f;
 		bool b;

common/command/command_def.h

@@ -36,6 +36,10 @@ struct command_header {
 	uint8_t type;
 } __packed;
 
+struct command_id {
+	uint32_t id;
+} __packed;
+
 struct command_timestamp {
 	uint64_t sec;
 	uint32_t nsec;

common/include/controller_setpoint_command.h

-../../controller/controller/controller_setpoint_command.h
\ No newline at end of file

controller/block/build.mk

@@ -3,64 +3,72 @@ BLOCK_TARGETS := $(LIBDIR)/libblock.la
 
 $(eval $(call SUBDIR,il2c))
 
+BLOCKS := \
+	add \
+	bridge_pwm \
+	counter \
+	debug \
+	decoder_uint32_bool \
+	filter_iir \
+	filter_lp \
+	friction \
+	gain \
+	gain_var \
+	inverse_proportional \
+	limit \
+	limit_dyn \
+	limit_2nd \
+	limit_var \
+	matrix_2x2 \
+	multiplexer \
+	pid \
+	pid_aw \
+	quadrature_decoder \
+	quantize \
+	random \
+	rangecheck \
+	setpoint_generator \
+	servo_state \
+	sine \
+	state_machine \
+	subtract \
+	switch \
+	trajectplayer \
+	trigger \
+	value \
+	value_bool \
+	value_uint32
 
-BLOCK_SRCS := \
-	$(DIR)/block_add.c \
-	$(DIR)/block_and2.il \
-	$(DIR)/block_bridge_pwm.c \
-	$(DIR)/block_counter.c \
-	$(DIR)/block_debug.c \
-	$(DIR)/block_decoder_uint32_bool.c \
-	$(DIR)/block_filter_iir.c \
-	$(DIR)/block_filter_lp.c \
-	$(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 \
-	$(DIR)/block_limit_var.c \
-	$(DIR)/block_matrix_2x2.c \
-	$(DIR)/block_multiplexer.c \
-	$(DIR)/block_not.il \
-	$(DIR)/block_or2.il \
-	$(DIR)/block_or3.il \
-	$(DIR)/block_or4.il \
-	$(DIR)/block_pid.c \
-	$(DIR)/block_pid_aw.c \
-	$(DIR)/block_quadrature_decoder.c \
-	$(DIR)/block_quantize.c \
-	$(DIR)/block_random.c \
-	$(DIR)/block_rangecheck.c \
-	$(DIR)/block_register.il \
-	$(DIR)/block_setpoint_generator.c \
-	$(DIR)/block_setreset.il \
-	$(DIR)/block_servo_state.c \
-	$(DIR)/block_sine.c \
-	$(DIR)/block_state_machine.c \
-	$(DIR)/block_subtract.c \
-	$(DIR)/block_switch.c \
-	$(DIR)/block_trajectplayer.c \
-	$(DIR)/block_trigger.c \
-	$(DIR)/block_value.c \
-	$(DIR)/block_value_bool.c \
-	$(DIR)/block_value_uint32.c
+BLOCKS_IL := \
+	and2 \
+	not \
+	or2 \
+	or3 \
+	or4 \
+	register \
+	setreset
 
 ifneq ($(OS), FreeBSD)
-BLOCK_SRCS+= \
-	$(DIR)/block_joystick.c
+BLOCKS+= \
+	joystick
 endif
 
+BLOCK_SRCS := \
+	$(addsuffix .c,$(addprefix $(DIR)/block_,$(BLOCKS))) \
+	$(addsuffix .il,$(addprefix $(DIR)/block_,$(BLOCKS_IL)))
+
 BLOCK_OBJSC := $(BLOCK_SRCS:.c=.lo)
 BLOCK_OBJS := $(BLOCK_OBJSC:.il=.il2c.lo)
 
 
 $(BLOCK_OBJS): CFLAGS += -O3 -Wall
-$(LIBDIR)/libblock.la_LDFLAGS += -lm
+$(LIBDIR)/libblock.la_LDFLAGS += -lm -lcontroller -llog
+$(LIBDIR)/libblock.la: libcontroller.la liblog.la
 $(LIBDIR)/libblock.la: $(BLOCK_OBJS)
 	$(LIB_LINK)
 
+CTRL_BLOCKS += $(BLOCKS) $(BLOCKS_IL)
+CTRL_BLOCK_LIBS += libblock.la
 
 TARGETS += $(BLOCK_TARGETS)
 CLEAN += $(BLOCK_TARGETS) $(BLOCK_OBJS)

controller/build.mk

 LIBDIR:=$(DIR)/lib
 CLEAN += $(DIR)/lib/.libs
 
+CTRL_BLOCKS :=
+CTRL_BLOCK_LIBS :=
+
 $(eval $(call SUBDIR,controller))
 $(eval $(call SUBDIR,shell))
 $(eval $(call SUBDIR,block))
@@ -14,23 +17,22 @@ DT_CTRL_TARGETS += $(DIR)/dt_ctrl
 
 DT_CTRL_SRCS := $(DIR)/dt_ctrl.c
 DT_CTRL_OBJS := $(DT_CTRL_SRCS:.c=.o)
-BLOCKS := $(shell $(DIR)/controller/block_list.sh $(DIR)/lib/*.a)
-BLOCKDEP := $(shell $(DIR)/controller/block_dep.sh $(DIR)/lib/*.a)
+
+, := ,
+BLOCKS := $(addprefix -Wl$(,)-u -Wl$(,)block_,$(addsuffix _create,$(CTRL_BLOCKS)))
+BLOCKS += $(subst .la,,$(subst lib,-l,$(CTRL_BLOCK_LIBS)))
 
 $(DIR)/dt_ctrl: \
 	libcontroller.la \
 	liblog.la \
 	libshell.la \
-	$(BLOCKDEP)
+	$(CTRL_BLOCK_LIBS)
 $(DIR)/dt_ctrl: CFLAGS += -Wall -O3
 $(DIR)/dt_ctrl: LDFLAGS += \
 	-Wl,-E \
 	-lcontroller \
-	-lethercat \
 	-llog \
 	-lshell \
-	-ldt_azimuth \
-	-ldt_elevation \
 	$(BLOCKS)
 $(DIR)/dt_ctrl: $(DT_CTRL_OBJS)
 

controller/controller/block_dep.sh

-#!/bin/sh
-
-for lib in $@; do
-	SYMBOLS=`nm $lib |grep "block_.*_create" |cut -d' ' -f3`
-	if [ "${SYMBOLS}none" != "none" ]; then
-		echo -n "$lib "
-	fi
-done

controller/controller/block_list.sh

-#!/bin/sh
-
-for lib in $@; do
-	SYMBOLS=`nm $lib |grep "block_.*_create" |cut -d' ' -f3`
-	for symbol in $SYMBOLS; do
-		echo -n "-Wl,-u -Wl,${symbol} "
-	done
-done

controller/controller/controller_block.h

@@ -245,7 +245,6 @@ void controller_block_trace_init(int max);
 int controller_block_trace_add(char *block, char *outterm,
     struct controller_trace *trace);
 void controller_block_trace_del(struct controller_trace *trace);
-void controller_block_trace_wait(void);
 
 void controller_block_trace(void);
 

controller/controller/controller_block_trace.c

@@ -34,7 +34,6 @@ static int max_traces = 0;
 
 static pthread_mutex_t trace_lock;
 static sem_t trace_sync_sem;
-static sem_t trace_waitsem;
 static struct controller_trace *trace_add_ptr = NULL;
 static struct controller_trace *trace_del_ptr = NULL;
 
@@ -52,7 +51,6 @@ void controller_block_trace_init(int max)
 
 	pthread_mutex_init(&trace_lock, NULL);
 	sem_init(&trace_sync_sem, 0, 0);
-	sem_init(&trace_waitsem, 0, 0);
 }
 
 int controller_block_trace_add(char *blockname, char *outterm, 
@@ -126,11 +124,6 @@ void controller_block_trace_del(struct controller_trace *trace)
 	return;
 }
 
-void controller_block_trace_wait(void)
-{
-	sem_wait(&trace_waitsem);
-}
-
 void controller_block_trace(void)
 {
 	int i;
@@ -209,10 +202,5 @@ void controller_block_trace(void)
 		}
 		
 	}
-
-	sem_getvalue(&trace_waitsem, &semval);
-	if (!semval) {
-		sem_post(&trace_waitsem);
-	}
 }
 

controller/controller/controller_trace.c

@@ -138,6 +138,7 @@ static void *controller_trace_handle(void *arg)
 	fd_set fdrx;
 	int high;
 	struct timeval tv;
+	long usec = controller_sample_period() * 1000000;
 
 	while (1) {
 		FD_ZERO(&fdrx);
@@ -151,7 +152,7 @@ static void *controller_trace_handle(void *arg)
 		}
 		
 		tv.tv_sec = 0;
-		tv.tv_usec = 0;
+		tv.tv_usec = usec;
 	
 		select(high + 1, &fdrx, NULL, NULL, &tv);
 		
@@ -271,8 +272,6 @@ static void *controller_trace_handle(void *arg)
 					trace_hdl[i].ctrace.rd_pos %= trace_hdl[i].ctrace.len;
 			}
 		}
-		
-		controller_block_trace_wait();
 	}
 
 	return NULL;

controller/dt_azimuth/build.mk

@@ -15,6 +15,8 @@ $(LIBDIR)/libdt_azimuth.la_LDFLAGS += -lcontroller -llog
 $(LIBDIR)/libdt_azimuth.la: $(DT_AZIMUTH_OBJS)
 	$(LIB_LINK)
 
+CTRL_BLOCKS += dt_az_safety dt_az_stoeber_sim
+CTRL_BLOCK_LIBS += libdt_azimuth.la
 
 TARGETS += $(DT_AZIMUTH_TARGETS)
 CLEAN += $(DT_AZIMUTH_TARGETS) $(DT_AZIMUTH_OBJS)

controller/dt_elevation/build.mk

@@ -16,6 +16,8 @@ $(LIBDIR)/libdt_elevation.la_LDFLAGS += -lcontroller -llog
 $(LIBDIR)/libdt_elevation.la: $(DT_ELEVATION_OBJS)
 	$(LIB_LINK)
 
+CTRL_BLOCKS += dt_el_safety dt_el_stoeber_l_sim dt_el_stoeber_r_sim
+CTRL_BLOCK_LIBS += libdt_elevation.la
 
 TARGETS += $(DT_ELEVATION_TARGETS)
 CLEAN += $(DT_ELEVATION_TARGETS) $(DT_ELEVATION_OBJS)

controller/ec/build.mk

@@ -14,17 +14,24 @@ EC_SRCS = \
 	$(DIR)/esc_fmmu.c \
 	$(DIR)/esc_sm.c \
 	$(DIR)/canopen.c \
-	$(DIR)/block_ec.c \
-	$(DIR)/block_ec_sim.c \
-	$(DIR)/block_beckhoff_el1xxx.c \
-	$(DIR)/block_beckhoff_el2xxx.c \
-	$(DIR)/block_beckhoff_el2502.c \
-	$(DIR)/block_beckhoff_el3xxx.c \
-	$(DIR)/block_beckhoff_el4xxx.c \
-	$(DIR)/block_beckhoff_el5001.c \
-	$(DIR)/block_beckhoff_el5101.c \
-	$(DIR)/block_beckhoff_el7031.c \
-	$(DIR)/block_stoeber.c
+
+EC_BLOCKS = \
+       ec \
+       ec_sim \
+       beckhoff_el1xxx \
+       beckhoff_el2xxx \
+       beckhoff_el2502 \
+       beckhoff_el3xxx \
+       beckhoff_el4xxx \
+       beckhoff_el5001 \
+       beckhoff_el5101 \
+       beckhoff_el7031 \
+       stoeber
+
+EC_SRCS += $(addsuffix .c,$(addprefix $(DIR)/block_,$(EC_BLOCKS)))
+
+CTRL_BLOCKS += $(EC_BLOCKS)
+CTRL_BLOCK_LIBS += libethercat.la
 
 ifeq ($(OS), FreeBSD)
 	EC_SRCS+= $(DIR)/eth_bsd.c