Some changes to reflect real configuration at DT. (beX connections)

Also some changes to properly log to console when no slaves are found

common/utils/tcp_connect.c

@@ -38,7 +38,7 @@ int tcp_connect(char *host, int port)
 	int error;
 	int sock = -1;
 	char port_str[10];
-	int tcp_connect_timeout = 5;
+	int tcp_connect_timeout = 1;
 	
 	sprintf(port_str, "%d", port);
 

controller/controller/controller_load.c

@@ -56,13 +56,19 @@ void yyerror(yyscan_t scanner, char const *s)
 {
 	struct controller_load_extra *extra = yyget_extra(scanner);
 	
-	log_send(LOG_T_ERROR, "%s:%d: %s\n", 
+	log_send(LOG_T_ERROR, "%s:%d: %s", 
 	    extra->filename,
 	    yyget_lineno(scanner),
 	    s);
 	extra->ret = -1;
 }
 
+void controller_load_fatal_error (char* msg , yyscan_t yyscanner)
+{
+    	log_send(LOG_T_ERROR, "Fatal: %s", msg );
+	log_server_flush();
+	exit(1);
+}
 
 struct controller_load_va_entry {
 	struct controller_load_va_entry *next;

controller/controller/controller_load.h

@@ -30,6 +30,7 @@ int controller_load_shell_add(void);
 int controller_load(char *filename);
 
 int controller_load_yy_input(char *buf, int *readbytes, int sizebytes, yyscan_t scanner);
+void controller_load_fatal_error (char* msg , yyscan_t yyscanner);
 
 void controller_load_var_add_str(char *string, yyscan_t scanner);
 void controller_load_var_add_dbl(double dbl, yyscan_t scanner);

controller/controller/controller_load_parser.l

@@ -27,10 +27,13 @@
 #include <inttypes.h>
 #include "controller_load_parser.tab.h"
 #include "controller_load.h"
+#include "log.h"
 
 #undef YY_INPUT
 #define YY_INPUT(buf,readbytes,sizebytes) controller_load_yy_input(buf,&readbytes,sizebytes, yyscanner)
 
+#define YY_FATAL_ERROR(msg) controller_load_fatal_error( msg , yyscanner)
+
 %}
 
 string		"\""[^"]*"\""

controller/dt_azimuth/dt_az_safety.c

@@ -37,13 +37,11 @@
      |	                             |
   ---| 2  torque_in    2  torque_out |----
      |	                             |
-  ---| 3  safety                     |
+  ---| 3  safety_in_positive         |
      |                               |
-  ---| 4  safety_in_positive         |
+  ---| 4  safety_in_negative         |
      |                               |
-  ---| 5  safety_in_negative         |
-     |                               |
-  ---| 6  enable                     |
+  ---| 5  enable                     |
      |                               |
      ---------------------------------
 
@@ -66,7 +64,6 @@ struct controller_block_private {
 	float *speed_in;
 	float *position_in;
 	float *torque_in;
-	bool *safety_in;
 	bool *safety_in_positive;
 	bool *safety_in_negative;
 	float speed_out;
@@ -88,7 +85,6 @@ struct controller_block_private {
 	bool err_min;
 	bool warn_safe_max;
 	bool warn_safe_min;
-	bool err_hwsafe;
 	bool err_hwsafe_positive;
 	bool err_hwsafe_negative;
 
@@ -151,15 +147,6 @@ static void calculate(struct controller_block *safety)
 		safety->private->warn_safe_min = false;
 	}
 	
-	if (*safety->private->safety_in == 0) {
-		safe = false;
-		if (!safety->private->err_hwsafe) {
-			safety->private->err_hwsafe = true;
-			log_send(LOG_T_ERROR, "Azimuth HW safety switch is open.");
-		}
-	} else {
-		safety->private->err_hwsafe = false;
-	}
 	if (*safety->private->safety_in_positive == 0) {
 		safe = false;
 		if (!safety->private->err_hwsafe_positive) {
@@ -285,7 +272,6 @@ static struct controller_block_interm_list interms[] = {
 	{ "speed_in",           CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, speed_in)           },
 	{ "position_in",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, position_in)        },
 	{ "torque_in",          CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_in)          },
-	{ "safety_in",          CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private,  safety_in)          },
 	{ "safety_in_positive", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private,  safety_in_positive) },
 	{ "safety_in_negative", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private,  safety_in_negative) },
 	{ "enable",             CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private,  enable_in)          },
@@ -330,7 +316,6 @@ struct controller_block * block_dt_az_safety_create(char *name)
 	safety->private->err_min = false;
 	safety->private->warn_safe_max = false;
 	safety->private->warn_safe_min = false;
-	safety->private->err_hwsafe = false;
 	safety->private->err_hwsafe_positive = false;
 	safety->private->err_hwsafe_negative = false;
 	

controller/dt_azimuth/dt_az_stoeber_sim.c

@@ -40,11 +40,11 @@ static struct ghost_term ghost_outputs[] = {
 	{ "speed",    	"dt_az_accelerate",   	"out"    },
 	{ "torque",   	"dt_az_torque_limit", 	"out"    },
 	{ "enabled", 	"dt_az_enabled",      	"output" },
-	{ "be1",	"dt_az_safety",		"value" },
-	{ "be2",	"dt_az_safety_n",	"value" },
-	{ "be3",	"dt_az_safety_n_not",	"output" },
-	{ "be4",	"dt_az_safety_p",	"value" },
-	{ "be5",	"dt_az_safety_p_not",	"output" },
+	{ "be1",	"dt_az_safety_n_not",	"output" },
+	{ "be2",	"dt_az_safety_n",	"value"  },
+	{ "be3",	"dt_az_safety_p_not",	"output" },
+	{ "be4",	"dt_az_safety_p",	"value"  },
+	{ "be5",	"dt_az_bex",		"value"  },
 	{ "ae1", 	"dt_az_focusbox",	"value"  },
 };
 

controller/dt_ctrl.c

@@ -53,7 +53,7 @@ int main(int argc, char **argv)
 		printf("Usage: %s <controllerfile>\n", argv[0]);
 		printf("\n");
 		printf("E.g.: %s dt_ctrl.ctrl\n", argv[0]);
-		return 1;
+		goto err_init;
 	}
 	
 	ctrl_filename = argv[1];
@@ -61,8 +61,8 @@ int main(int argc, char **argv)
 	/* Create and link blocks */
 
 	if (controller_load(ctrl_filename)) {
-		printf("Could load controller file\n");
-		return 1;
+		printf("Could not load controller file\n");
+		goto err_init;
 	}
 
 	controller_trace_server_start(CTRL_TRACE_PORT, 100);
@@ -84,7 +84,7 @@ int main(int argc, char **argv)
 	/* Start 'sample' */
 
 	if (controller_block_sample_init()) {
-		return 1;
+		goto err_init;
 	}
 
 	controller_sample_start();
@@ -109,4 +109,8 @@ int main(int argc, char **argv)
 	}
 
 	return 0;
+
+err_init:
+	log_server_flush();
+	return 1;
 }

controller/dt_ctrl.ctrl

+#	dt_ctrl.ctrl
+#
+#	Controller network for Dwingeloo Telescope.
+#
+#	This file contains the fixed part of the network and its parameters.
+#	Other .ctrl files are used for those parts which are conditional,
+#	such as the use of simulation networks or real servo drives.
+#
+
+# Frequency is limited by the stoeber drives.
 frequency 250
 
+# dt_ctrl_el.ctrl	Elevation with real servo drives
+# dt_ctrl_el_sim.ctrl	Elevation with simulated network
+# dt_ctrl_az.ctrl	Azimuth with real servo drives
+# dt_ctrl_az_sim.ctrl	Azimuth with simulated network
+#
 # Uncomment either the real network, or the sim network.
 # But never both!
 
@@ -9,6 +24,7 @@ include "dt_ctrl_el_sim.ctrl"
 #include "dt_ctrl_az.ctrl"
 include "dt_ctrl_az_sim.ctrl"
 
+
 blocks {
 	{ "setpoint_generator", "azimuth_spg", 			"Azimuth_Setpoint", "rad" }
 	{ "servo_state",        "azimuth_servo_state"              }
@@ -80,7 +96,6 @@ links {
 	{ "azimuth_speed_limit",            "out",        "azimuth_safety",                   "speed_in"     , true }
 	{ "azimuth_position_offset_sum",    "out",        "azimuth_safety",                   "position_in"  , true }
 	{ "azimuth_torque",                 "value",      "azimuth_safety",                   "torque_in"    , true }
-	{ "dt_az",                          "be1",        "azimuth_safety",                   "safety_in"    , true }
 	{ "dt_az",                          "be4",        "azimuth_safety",                   "safety_in_positive", true }
 	{ "dt_az",                          "be2",        "azimuth_safety",                   "safety_in_negative", true }
 	{ "azimuth_servo_state",            "enable",     "azimuth_safety",                   "enable"       , true }
@@ -130,8 +145,6 @@ links {
 	{ "elevation_torque_l",             "value",      "elevation_safety",                 "torque_in_l"   , true }
 	{ "dt_el_l",                        "be4",        "elevation_safety",                 "safety_in_top",    true }
 	{ "dt_el_l",                        "be2",        "elevation_safety",                 "safety_in_bottom", true }
-	{ "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 }

controller/dt_ctrl_az_sim.ctrl

@@ -16,11 +16,11 @@ blocks {
 	{ "add",	"dt_az_offset_add"	}
 	{ "quantize",  	"dt_az_pos_quantize"	}
 	{ "register",	"dt_az_enabled"	}
-	{ "value_bool",	"dt_az_safety"	}
 	{ "value_bool",	"dt_az_safety_n"	}
 	{ "not",	"dt_az_safety_n_not"	}
 	{ "value_bool",	"dt_az_safety_p"	}
 	{ "not",	"dt_az_safety_p_not"	}
+	{ "value_bool",	"dt_az_bex" 		}
 	{ "value",	"dt_az_focusbox"	}
 
 	{ "dt_az_stoeber_sim", "dt_az" }
@@ -75,7 +75,7 @@ params {
 	{ "dt_az_torque_noise_sine", "f",               (float)0.0 }
 	{ "dt_az_focusbox",          "value",           (float)5.0 }
 	{ "dt_az_offset",            "value",      	(float)-21730.3997815 }
-	{ "dt_az_safety",            "value",      	(int)1 }
+	{ "dt_az_bex",               "value",      	(int)0 }
 	{ "dt_az_safety_p",          "value",      	(int)1 }
 	{ "dt_az_safety_n",          "value",      	(int)1 }
 }

controller/dt_ctrl_el_sim.ctrl

@@ -36,8 +36,6 @@ blocks {
 	{ "value_bool",	"dt_el_safety_b" 	}
 	{ "not",	"dt_el_safety_t_not"	}
 	{ "not",	"dt_el_safety_b_not" 	}
-	{ "value_bool",	"dt_el_safety_r" 	}
-	{ "value_bool",	"dt_el_safety_l" 	}
 	{ "value_bool",	"dt_el_bex" 		}
 	{ "value",	"dt_el_ae1"	 	}
 
@@ -97,8 +95,6 @@ params {
 
 	{ "dt_el_safety_t",         "value",             true    }
 	{ "dt_el_safety_b",         "value",             (bool)1 }
-	{ "dt_el_safety_r",         "value",             (bool)1 }
-	{ "dt_el_safety_l",         "value",             (bool)1 }
 	{ "dt_el_bex",              "value",             (bool)0 }
 }
 

controller/dt_elevation/dt_el_safety.c

@@ -48,13 +48,9 @@
      |	                              |
   ---| 7  torque_in_l  4 torque_out_l |----
      |	                              |
-  ---| 8  safety_in_l                 |
+  ---| 8  safety_in_top               |
      |                                |
-  ---| 9  safety_in_r                 |
-     |                                |
-  ---| 10  safety_in_top              |
-     |                                |
-  ---| 11  safety_in_bottom           |
+  ---| 9  safety_in_bottom            |
      |                                |
   ---| 10 enable                      |
      |                                |
@@ -84,8 +80,6 @@ struct controller_block_private {
 	float *torque_in;
 	float *torque_in_r;
 	float *torque_in_l;
-	bool *safety_in_l;
-	bool *safety_in_r;
 	bool *safety_in_top;
 	bool *safety_in_bottom;
 	float speed_out_r;
@@ -122,8 +116,6 @@ struct controller_block_private {
 	bool warn_safe_min_l;
 	bool warn_safe_max_r;
 	bool warn_safe_min_r;
-	bool err_hwsafe_r;
-	bool err_hwsafe_l;
 	bool err_hwsafe_top;
 	bool err_hwsafe_bottom;
 };
@@ -347,30 +339,6 @@ static void calculate(struct controller_block *safety)
 		safety->private->err_hwsafe_bottom = false;
 	}
 	
-	/* left generic safety input */
-	if (!*safety->private->safety_in_l) {
-		safe = false;
-		if (!safety->private->err_hwsafe_l) {
-			safety->private->err_hwsafe_l = true;
-			log_send(LOG_T_ERROR, 
-			    "Elevation HW left drive safety switch is open.");
-		}
-	} else {
-		safety->private->err_hwsafe_l = false;
-	}
-	
-	/* right generic safety input */
-	if (!*safety->private->safety_in_r) {
-		safe = false;
-		if (!safety->private->err_hwsafe_r) {
-			safety->private->err_hwsafe_r = true;
-			log_send(LOG_T_ERROR, 
-			    "Elevation HW right drive safety switch is open.");
-		}
-	} else {
-		safety->private->err_hwsafe_r = false;
-	}
-
 	if (!safe)
 		enabled = false;
 
@@ -520,8 +488,6 @@ static struct controller_block_interm_list interms[] = {
 	{ "torque_in_l",      CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_in_l)     },
 	{ "safety_in_top",    CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safety_in_top)    },
 	{ "safety_in_bottom", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safety_in_bottom) },
-	{ "safety_in_l",      CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safety_in_l)      },
-	{ "safety_in_r",      CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safety_in_r)      },
 	{ "enable",           CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable_in)        },
 	{ NULL }
 };
@@ -571,8 +537,6 @@ struct controller_block * block_dt_el_safety_create(char *name)
 	safety->private->warn_safe_min_l = false;
 	safety->private->err_hwsafe_top = false;
 	safety->private->err_hwsafe_bottom = false;
-	safety->private->err_hwsafe_r = false;
-	safety->private->err_hwsafe_l = false;
 	safety->private->err_torsion_recover = false;
 
 	if (controller_block_interm_list_init(safety, interms))

controller/dt_elevation/dt_el_stoeber_l_sim.c

@@ -24,6 +24,7 @@
 #include "controller_block.h"
 #include "esc.h"
 #include "ec_stoeber.h"
+#include "log.h"
 
 struct controller_block_private {
 	bool *ba1;
@@ -41,11 +42,11 @@ static struct ghost_term ghost_outputs[] = {
 	{ "speed",    	"dt_el_accelerate_l",   "out"    },
 	{ "torque",   	"dt_el_torque_limit_l", "out"    },
 	{ "enabled", 	"dt_el_enabled",      	"output" },
-	{ "be1",	"dt_el_safety_l",	"value"  },
+	{ "be1",	"dt_el_safety_b_not",	"output" },
 	{ "be2",	"dt_el_safety_b",	"value"  },
-	{ "be3",	"dt_el_safety_b_not",	"output" },
+	{ "be3",	"dt_el_safety_t_not",	"output" },
 	{ "be4",	"dt_el_safety_t",	"value"  },
-	{ "be5",	"dt_el_safety_t_not",	"output"  },
+	{ "be5",	"dt_el_bex",		"value"  },
 	{ "ae1", 	"dt_el_ae1",		"value"  },
 };
 
@@ -90,7 +91,7 @@ struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap
 		stoeber->output[i].name = ghost_outputs[i].name;
 		subblock = controller_block_find(ghost_outputs[i].realblock);
 		if (!subblock) {
-			printf("subblock %s not found\n", ghost_outputs[i].realblock);
+			log_send(LOG_T_ERROR, "subblock %s not found", ghost_outputs[i].realblock);
 			goto err_ghost_output;
 		}
 		for (j = 0; j < subblock->outputs; j++) {
@@ -105,6 +106,11 @@ struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap
 				break;
 			}
 		}
+		if (j == subblock->outputs) {
+			log_send(LOG_T_ERROR, "Could not find subblock output %s",
+			    ghost_outputs[i].realname);
+			goto err_ghost_output;
+		}
 	}
 
 	for (i = 0; ghost_inputs[i].name; i++);
@@ -130,6 +136,11 @@ struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap
 				break;
 			}
 		}
+		if (j == subblock->inputs) {
+			log_send(LOG_T_ERROR, "Could not find subblock input %s",
+			    ghost_inputs[i].realname);
+			goto err_ghost_output;
+		}
 	}
 	stoeber->input[i].name = "ba1";
 	stoeber->input[i].value.b = &stoeber->private->ba1;

controller/dt_elevation/dt_el_stoeber_r_sim.c

@@ -41,7 +41,7 @@ static struct ghost_term ghost_outputs[] = {
 	{ "speed",    	"dt_el_accelerate_r",   "out"   },
 	{ "torque",   	"dt_el_torque_limit_r", "out"   },
 	{ "enabled", 	"dt_el_enabled",      	"output" },
-	{ "be1",	"dt_el_safety_r",	"value" },
+	{ "be1",	"dt_el_bex",		"value" },
 	{ "be2",	"dt_el_bex",		"value" },
 	{ "be3",	"dt_el_bex",		"value" },
 	{ "be4",	"dt_el_bex",		"value" },

controller/ec/ec.c

@@ -92,7 +92,8 @@ static int ec_rx_pdo_add_time(void)
 	rx_pdo.flen += sizeof(struct ec_dgram_hdr);
 	rx_pdo.flen += sizeof(uint64_t) + 2;
 
-	printf("Adding %zu bytes for clock distribution to rx_pdo, total pdo is now: %zu bytes\n",
+	log_send(LOG_T_DEBUG,
+	    "Adding %zu bytes for clock distribution to rx_pdo, total pdo is now: %zu bytes",
 	    sizeof(uint64_t), rx_pdo.flen);
 
 	wkc = (struct unaligned_uint16_t *)
@@ -475,7 +476,7 @@ int ec_slave_count(void)
 	flen += sizeof(struct ec_dgram_hdr);
 	flen += sizeof(char) + 2;
 
-	printf("Reading byte 0 of all ethercat slaves\n");
+	log_send(LOG_T_DEBUG, "Reading byte 0 of all ethercat slaves");
 	wkc = (struct unaligned_uint16_t *)(frame + flen - 2);
 	wkc->u16 = 0;
 	*payload = 0x00;
@@ -745,7 +746,8 @@ int ec_init(char *ifname, bool single_cycle)
 		printf("Using a single PDO for rx and tx.\n");
 	}
 	
-	printf("Going to initialize interface %s for control data\n", ifname);
+	log_send(LOG_T_DEBUG, 
+	    "Going to initialize interface %s for control data", ifname);
 
 	ec_sock = eth_open(ifname);
 	if (ec_sock < 0)
@@ -755,7 +757,8 @@ int ec_init(char *ifname, bool single_cycle)
 	
 	timeout.tv_sec = (int)(timeout_sec);
 	timeout.tv_usec = (int)(timeout_sec * 1000000) % 1000000;
-	printf("Setting rx/tx timeout to %d.%06d sec based on %f sec default period\n", 
+	log_send(LOG_T_DEBUG,
+	    "Setting rx/tx timeout to %d.%06d sec based on %f sec default period", 
 	    (int)timeout.tv_sec, (int)timeout.tv_usec, controller_sample_period());
 
 	eth_timeout_set(ec_sock, &timeout, &timeout);
@@ -765,7 +768,8 @@ int ec_init(char *ifname, bool single_cycle)
 	} else {
 		goto err_init;
 	}
-	printf("Successfull init, %d slaves found.\n", ec_slave_count());
+	log_send(LOG_T_DEBUG, 
+	    "Successfull init, %d slaves found.", ec_slave_count());
 
 	ec_rx_pdo_add_time();
 
@@ -779,5 +783,6 @@ err_init:
 	close(ec_sock);
 	ec_sock = -1;
 err_nosocket:
+	log_send(LOG_T_ERROR, "Failed to initialize Ethercat subsystem");
 	return -1;
 }

controller/log/log.c

@@ -290,3 +290,11 @@ int log_server_start(int port, enum log_type console_level,
 
 	return 0;
 }
+
+void log_server_flush(void)
+{
+	sleep(1);
+	do {
+		sleep(1);
+	} while (msgs[msgs_rd].used);
+}

controller/log/log.h

@@ -30,5 +30,6 @@ enum log_type {
 void log_send(enum log_type type, char *fmt, ...) __attribute__ ((format (printf, 2, 3)));
 int log_server_start(int port, enum log_type console_level,
     enum log_type remote_level);
+void log_server_flush(void);
 
 #endif /* _INCLUDE_LOG_H_ */

test_sets/azimuth_safety_safe.set

 params {
-	{ "dt_az_safety", 	"value", (int)1 }
 	{ "dt_az_safety_p", 	"value", (int)1 }
 	{ "dt_az_safety_n", 	"value", (int)1 }
 }

test_sets/azimuth_safety_unsafe.set

-params {
-	{ "dt_az_safety", "value", (int)0 }
-}

test_sets/elevation_safety_left_unsafe.set

-params {
-	{ "dt_el_safety_l", "value", (int)0 }
-}

test_sets/elevation_safety_right_unsafe.set

-params {
-	{ "dt_el_safety_r", "value", (int)0 }
-}