Add binary outputs to drives, needed for focusbox

controller/build.mk

@@ -9,8 +9,11 @@ VPATH= /lib64 /usr/lib64 /usr/local/lib64 /lib /usr/lib /usr/local/lib
 LIBTOOL=libtool
 
 %.o : %.c
-	@echo "     CC $<"
-	@$(CC) -MD $(CFLAGS) -c $<
+	@echo "LT   CC $<"
+	@${LIBTOOL} --quiet --mode=compile --tag=CC $(CC) -MD $(CFLAGS) -c $<
+
+#	@echo "     CC $<"
+#	@$(CC) -MD $(CFLAGS) -c $<
 
 %: %.o
 	@echo "     LD $@"

controller/dt_azimuth/Makefile

@@ -15,7 +15,7 @@ libdt_azimuth.la_install: libdt_azimuth.la
 
 
 clean:
-	rm -rf *.o *.a *.d *.lo *.la libs
+	rm -rf *.o *.a *.d *.lo *.la .libs
 
 SRCS=$(ARCHSRCS)
 

controller/dt_azimuth/dt_az_stoeber_sim.c

@@ -25,15 +25,8 @@
 #include "ec_stoeber.h"
 
 struct controller_block_private {
-	bool enabled;
-	
-/*	bool output_be1;
-	bool output_be2;
-	bool output_be3;
-	bool output_be4;
-	bool output_be5;
-	float output_ae1;
-*/
+	bool *ba1;
+	bool *ba2;
 };
 
 struct ghost_term {
@@ -112,8 +105,8 @@ struct controller_block *block_dt_az_stoeber_sim_create(char *name, va_list ap)
 	}
 
 	for (i = 0; ghost_inputs[i].name; i++);
-	stoeber->inputs = i;
-	stoeber->input = malloc(sizeof(struct controller_block_interm)*i);
+	stoeber->inputs = i + 2;
+	stoeber->input = malloc(sizeof(struct controller_block_interm)*stoeber->inputs);
 	if (!stoeber->input)
 		goto err_input;
 	for (i = 0; ghost_inputs[i].name; i++) {
@@ -135,6 +128,15 @@ struct controller_block *block_dt_az_stoeber_sim_create(char *name, va_list ap)
 			}
 		}
 	}
+	stoeber->input[i].name = "ba1";
+	stoeber->input[i].value.b = &stoeber->private->ba1;
+	stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
+	stoeber->input[i].ghostof = NULL;
+	i++;
+	stoeber->input[i].name = "ba2";
+	stoeber->input[i].value.b = &stoeber->private->ba2;
+	stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
+	stoeber->input[i].ghostof = NULL;
 
 	stoeber->params = 0;
 

controller/dt_ctrl.ctrl

@@ -42,6 +42,8 @@ blocks {
 	{ "value",	        "elevation_position_offset_l"	   }
 	{ "add",  	        "elevation_position_offset_r_sum"  }
 	{ "add",  	        "elevation_position_offset_l_sum"  }
+
+	{ "value_bool", 	"false"	}
 }
 
 links {
@@ -77,6 +79,8 @@ links {
 	{ "azimuth_speed_servo",            "out",        "dt_az",                            "speed"        , true }
 	{ "azimuth_safety",                 "torque_out", "dt_az",                            "torque"       , true }
 	{ "azimuth_servo_state",            "enable",     "dt_az",                            "enable"       , true }
+	{ "false",                          "value",      "dt_az",                            "ba1"          , true }
+	{ "false",			    "value",      "dt_az",                            "ba2"          , true }
 
 	{ "elevation_servo_state",          "reset",      "elevation_spg",                    "reset"        , false }
 	{ "elevation_input_matrix",         "out0",       "elevation_spg",                    "cur0"         , true }
@@ -120,13 +124,16 @@ links {
 	{ "dt_el_l",                        "be1",        "elevation_safety",                 "safety_in_l"  , true }
 	{ "dt_el_r",                        "be1",        "elevation_safety",                 "safety_in_r"  , true }
 	{ "elevation_servo_state",          "enable",     "elevation_safety",                 "enable"       , true }
-	{ "elevation_safety",               "speed_out_r", "dt_el_r",                          "speed"        , true } 
-	{ "elevation_safety",               "speed_out_l", "dt_el_l",                          "speed"        , true }
-	{ "elevation_safety",               "torque_out_r","dt_el_r",                          "torque"       , true }
-	{ "elevation_safety",               "torque_out_l","dt_el_l",                          "torque"       , true }
+	{ "elevation_safety",               "speed_out_r", "dt_el_r",                         "speed"        , true } 
+	{ "elevation_safety",               "speed_out_l", "dt_el_l",                         "speed"        , true }
+	{ "elevation_safety",               "torque_out_r","dt_el_r",                         "torque"       , true }
+	{ "elevation_safety",               "torque_out_l","dt_el_l",                         "torque"       , true }
 	{ "elevation_servo_state",          "enable",     "dt_el_r",                          "enable"       , true }
 	{ "elevation_servo_state",          "enable",     "dt_el_l",                          "enable"       , true }
-
+	{ "false",                          "value",      "dt_el_r",                          "ba1"          , true }
+	{ "false",			    "value",      "dt_el_r",                          "ba2"          , true }
+	{ "false",                          "value",      "dt_el_l",                          "ba1"          , true }
+	{ "false",			    "value",      "dt_el_l",                          "ba2"          , true }
 	
 	{ "elevation_servo_state",          "reset",      "elevation_torsion_spg",            "reset"        , false }
 	{ "elevation_input_matrix",         "out1",       "elevation_torsion_spg",            "cur0"         , true }

controller/dt_elevation/Makefile

@@ -19,6 +19,6 @@ dt_elevation: dt_elevation.a($(OBJS))
 
 
 clean:
-	rm -rf *.o *.a *.d
+	rm -rf *.o *.a *.d *.lo .libs *la
 
 include ../build.mk

controller/dt_elevation/dt_el_stoeber_l_sim.c

@@ -25,6 +25,11 @@
 #include "esc.h"
 #include "ec_stoeber.h"
 
+struct controller_block_private {
+	bool *ba1;
+	bool *ba2;
+};
+
 struct ghost_term {
 	char *name;
 	char *realblock;
@@ -54,6 +59,7 @@ static struct ghost_term ghost_inputs[] = {
 struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap)
 {
 	struct controller_block *stoeber;
+	struct controller_block_private *private;
 	int i;
 	
 	stoeber = malloc(sizeof(struct controller_block));
@@ -66,7 +72,11 @@ struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap
 	if (!stoeber->name)
 		goto err_name;
 
-	stoeber->private = NULL;
+	private = calloc(1, sizeof(struct controller_block_private));
+	if (!private)
+		goto err_private;
+	stoeber->private = private;
+
 
 	for (i = 0; ghost_outputs[i].name; i++);
 	stoeber->outputs = i;
@@ -98,8 +108,8 @@ struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap
 	}
 
 	for (i = 0; ghost_inputs[i].name; i++);
-	stoeber->inputs = i;
-	stoeber->input = malloc(sizeof(struct controller_block_interm)*i);
+	stoeber->inputs = i + 2;
+	stoeber->input = malloc(sizeof(struct controller_block_interm)*stoeber->inputs);
 	if (!stoeber->input)
 		goto err_input;
 	for (i = 0; ghost_inputs[i].name; i++) {
@@ -121,6 +131,15 @@ struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap
 			}
 		}
 	}
+	stoeber->input[i].name = "ba1";
+	stoeber->input[i].value.b = &stoeber->private->ba1;
+	stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
+	stoeber->input[i].ghostof = NULL;
+	i++;
+	stoeber->input[i].name = "ba2";
+	stoeber->input[i].value.b = &stoeber->private->ba2;
+	stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
+	stoeber->input[i].ghostof = NULL;
 
 	stoeber->params = 0;
 
@@ -134,6 +153,8 @@ err_input:
 err_ghost_output:
 	free(stoeber->output);
 err_output:
+	free(stoeber->private);
+err_private:
 	free(stoeber->name);
 err_name:
 	free(stoeber);

controller/dt_elevation/dt_el_stoeber_r_sim.c

@@ -25,6 +25,11 @@
 #include "esc.h"
 #include "ec_stoeber.h"
 
+struct controller_block_private {
+	bool *ba1;
+	bool *ba2;
+};
+
 struct ghost_term {
 	char *name;
 	char *realblock;
@@ -55,6 +60,7 @@ static struct ghost_term ghost_inputs[] = {
 struct controller_block *block_dt_el_stoeber_r_sim_create(char *name, va_list ap)
 {
 	struct controller_block *stoeber;
+	struct controller_block_private *private;
 	int i;
 	
 	stoeber = malloc(sizeof(struct controller_block));
@@ -67,7 +73,10 @@ struct controller_block *block_dt_el_stoeber_r_sim_create(char *name, va_list ap
 	if (!stoeber->name)
 		goto err_name;
 
-	stoeber->private = NULL;
+	private = calloc(1, sizeof(struct controller_block_private));
+	if (!private)
+		goto err_private;
+	stoeber->private = private;
 
 	for (i = 0; ghost_outputs[i].name; i++);
 	stoeber->outputs = i;
@@ -99,8 +108,8 @@ struct controller_block *block_dt_el_stoeber_r_sim_create(char *name, va_list ap
 	}
 
 	for (i = 0; ghost_inputs[i].name; i++);
-	stoeber->inputs = i;
-	stoeber->input = malloc(sizeof(struct controller_block_interm)*i);
+	stoeber->inputs = i + 2;
+	stoeber->input = malloc(sizeof(struct controller_block_interm)*stoeber->inputs);
 	if (!stoeber->input)
 		goto err_input;
 	for (i = 0; ghost_inputs[i].name; i++) {
@@ -126,6 +135,16 @@ struct controller_block *block_dt_el_stoeber_r_sim_create(char *name, va_list ap
 			    ghost_inputs[i].realblock, ghost_inputs[i].realname);
 		}
 	}
+	stoeber->input[i].name = "ba1";
+	stoeber->input[i].value.b = &stoeber->private->ba1;
+	stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
+	stoeber->input[i].ghostof = NULL;
+	i++;
+	stoeber->input[i].name = "ba2";
+	stoeber->input[i].value.b = &stoeber->private->ba2;
+	stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
+	stoeber->input[i].ghostof = NULL;
+
 
 	stoeber->params = 0;
 	stoeber->param_get = NULL;
@@ -141,6 +160,8 @@ err_input:
 err_ghost_output:
 	free(stoeber->output);
 err_output:
+	free(stoeber->private);
+err_private:
 	free(stoeber->name);
 err_name:
 	free(stoeber);

controller/ec/block_stoeber.c

@@ -44,6 +44,8 @@ struct controller_block_private {
 	float *input_speed;
 	float *input_torque;
 	bool *input_enable;
+	bool *input_ba1;
+	bool *input_ba2;
 };
 
 static void calculate_tx(struct controller_block *tx)
@@ -64,7 +66,7 @@ static void calculate_tx(struct controller_block *tx)
 	}
 	
 	*stbr->tx_A180 = enabled;
-	*stbr->tx_F210 = 0;
+	*stbr->tx_F210 = (*private->input_ba1 << 0) | (*private->input_ba2 << 1);
 	*stbr->tx_I210 = htole16(le16toh(*stbr->tx_I210) ^ 1);
 	
 	if (enabled)
@@ -330,7 +332,7 @@ struct controller_block *block_stoeber_create(char *name, va_list ap)
 	stoeber_tx->outputs = 0;
 	stoeber_tx->output = NULL;
 
-	stoeber_tx->inputs = 3;
+	stoeber_tx->inputs = 5;
 	stoeber_tx->input = calloc(stoeber_tx->inputs, sizeof(struct controller_block_interm));
 	if (!stoeber_tx->input)
 		goto err_tx_input;
@@ -346,6 +348,14 @@ struct controller_block *block_stoeber_create(char *name, va_list ap)
 	stoeber_tx->input[2].type = CONTROLLER_BLOCK_TERM_BOOL;
 	stoeber_tx->input[2].value.b = &private->input_enable;
 	stoeber_tx->input[2].ghostof = NULL;
+	stoeber_tx->input[3].name = "ba1";
+	stoeber_tx->input[3].type = CONTROLLER_BLOCK_TERM_BOOL;
+	stoeber_tx->input[3].value.b = &private->input_ba1;
+	stoeber_tx->input[3].ghostof = NULL;
+	stoeber_tx->input[4].name = "ba2";
+	stoeber_tx->input[4].type = CONTROLLER_BLOCK_TERM_BOOL;
+	stoeber_tx->input[4].value.b = &private->input_ba2;
+	stoeber_tx->input[4].ghostof = NULL;
 
 	stoeber->inputs = stoeber_tx->inputs;
 	stoeber->input = calloc(stoeber->inputs, sizeof(struct controller_block_interm));

controller/ec/ec_stoeber.c

@@ -87,126 +87,126 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
 
 	/* A180 device control, disable it for now */
 	val8 = 0x00;
-	canopen_write_param(canopen_dev, 0x20b4, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 180), 0x0, &val8, 1);
 
 	/* A61 fault reset source to parameter */
 	val8 = 2;
-	canopen_write_param(canopen_dev, 0x203d, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 61), 0x0, &val8, 1);
 	usleep(3000);
 	/* A180 device control, reset faults */
 	val8 = 0x02;
-	canopen_write_param(canopen_dev, 0x20b4, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 180), 0x0, &val8, 1);
 	usleep(50000);
 	/* A180 device control, reset faults */
 	val8 = 0x00;
-	canopen_write_param(canopen_dev, 0x20b4, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 180), 0x0, &val8, 1);
 
 	/* A21 brake resistor */
 	leval16 = htole16(stbr->brake_resistor * 10.0);
-	canopen_write_param(canopen_dev, 0x2015, 0x0, &leval16, 2);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 21), 0x0, &leval16, 2);
 	/* A22 brake resistor power */
 	leval16 = htole16(stbr->brake_power);
-	canopen_write_param(canopen_dev, 0x2016, 0x0, &leval16, 2);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 22), 0x0, &leval16, 2);
 
 	/* A29 fault quickstop */
 	val8 = 1;
-	canopen_write_param(canopen_dev, 0x201d, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 29), 0x0, &val8, 1);
 
 	/* A34 auto start */
 	val8 = 1;
-	canopen_write_param(canopen_dev, 0x2022, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 34), 0x0, &val8, 1);
 	
 	/* A45*/
 	val8 = 1;
-	canopen_write_param(canopen_dev, 0x202d, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 45), 0x0, &val8, 1);
 	
 	/* A60*/
 	val8 = 1;
-	canopen_write_param(canopen_dev, 0x203c, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 60), 0x0, &val8, 1);
 	
 	/* A150 cycle time */
 	val8 = 0x06;
-	canopen_write_param(canopen_dev, 0x2096, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 150), 0x0, &val8, 1);
 
 	/* A258 set PDO timeout */
 	leval16 = htole16(1000);
-	canopen_write_param(canopen_dev, 0x2102, 0x0, &leval16, 2);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 258), 0x0, &leval16, 2);
 
 	/* G90 activate PLL */
 	val8 = 1;
-	canopen_write_param(canopen_dev, 0x2c5a, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 90), 0x0, &val8, 1);
 
 	/* G91 pll phase offset */
 	leval32 = htole32(0);
-	canopen_write_param(canopen_dev, 0x2c5b, 0x0, &leval32, 4);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 91), 0x0, &leval32, 4);
 
 	/* G92 pll gain pk */
 	leval32 = htole32(10);
-	canopen_write_param(canopen_dev, 0x2c5c, 0x0, &leval32, 4);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 92), 0x0, &leval32, 4);
 
 	/* G93 pll low pass */
 	leval32 = htole32(999);
-	canopen_write_param(canopen_dev, 0x2c5d, 0x0, &leval32, 4);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 93), 0x0, &leval32, 4);
 
 	/* G98 cycle to 4ms */
 	leval16 = htole16(4000);
-	canopen_write_param(canopen_dev, 0x2c62, 0x0, &leval16, 2);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 98), 0x0, &leval16, 2);
 
 	/* Set I06 to 2 decimal digits */
 	val8 = 2;
-	canopen_write_param(canopen_dev, 0x3006, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 6), 0x0, &val8, 1);
 
 	/* C32 (motor integrator) to 3000 ms */
 	leval16 = htole16(32000);
-	canopen_write_param(canopen_dev, 0x2420, 0x0, &leval16, 2);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 32), 0x0, &leval16, 2);
 
 	/* C33 low pass reference speed 1 ms */
 	leval32 = htole32(10);
-	canopen_write_param(canopen_dev, 0x2421, 0x0, &leval32, 4);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 33), 0x0, &leval32, 4);
 
 	/* C130 torque limit source to parameter */
 	val8 = 4;
-	canopen_write_param(canopen_dev, 0x2482, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 130), 0x0, &val8, 1);
 
 	/* Set I10 to maximum speed */
 	leval32 = htole32(stbr->max_speed / (2*M_PI) * 360 * 100);
 //	leval32 = htole32(3000 * 360 / 60 * 100);
-	canopen_write_param(canopen_dev, 0x300a, 0x0, &leval32, 4);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 10), 0x0, &leval32, 4);
 
 	/* Set I11 to maximum acceleration */
 //	leval32 = htole32(2000000 * 100);
 	leval32 = htole32(stbr->max_accel * 100);
-	canopen_write_param(canopen_dev, 0x300b, 0x0, &leval32, 4);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 11), 0x0, &leval32, 4);
 		
 	/* Set I25 to 100% speed ff */
 	leval16 = htole16(100);
-	canopen_write_param(canopen_dev, 0x3019, 0x0, &leval16, 2);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 25), 0x0, &leval16, 2);
 
 	/* Set I30 to 1 (encoder signal 0) */
 	val8 = 1;
-	canopen_write_param(canopen_dev, 0x301e, 0x0, &val8, 1);
+	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 30), 0x0, &val8, 1);
 
 	/* Set I37 to 1 (active) */
 	val8 = 1;