Commit 98ac0a8f authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

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

Also some changes to properly log to console when no slaves are found
parent fdd289fe
......@@ -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);
......
......@@ -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;
......
......@@ -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);
......
......@@ -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 "\""[^"]*"\""
......
......@@ -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;
......
......@@ -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" },
};
......
......@@ -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;
}
# 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 }
......
......@@ -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 }
}
......@@ -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 }
}
......
......@@ -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))
......
......@@ -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;
......
......@@ -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" },
......
......@@ -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;
}
......@@ -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);
}
......@@ -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_ */
params {
{ "dt_az_safety", "value", (int)1 }
{ "dt_az_safety_p", "value", (int)1 }
{ "dt_az_safety_n", "value", (int)1 }
}
params {
{ "dt_az_safety", "value", (int)0 }
}
params {
{ "dt_el_safety_l", "value", (int)0 }
}
params {
{ "dt_el_safety_r", "value", (int)0 }
}
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