Browse Source

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

Also some changes to properly log to console when no slaves are found
vitsch
Jeroen Vreeken 8 years ago
parent
commit
98ac0a8fa0
21 changed files with 83 additions and 98 deletions
  1. +1
    -1
      common/utils/tcp_connect.c
  2. +7
    -1
      controller/controller/controller_load.c
  3. +1
    -0
      controller/controller/controller_load.h
  4. +3
    -0
      controller/controller/controller_load_parser.l
  5. +3
    -18
      controller/dt_azimuth/dt_az_safety.c
  6. +5
    -5
      controller/dt_azimuth/dt_az_stoeber_sim.c
  7. +8
    -4
      controller/dt_ctrl.c
  8. +16
    -3
      controller/dt_ctrl.ctrl
  9. +2
    -2
      controller/dt_ctrl_az_sim.ctrl
  10. +0
    -4
      controller/dt_ctrl_el_sim.ctrl
  11. +2
    -38
      controller/dt_elevation/dt_el_safety.c
  12. +15
    -4
      controller/dt_elevation/dt_el_stoeber_l_sim.c
  13. +1
    -1
      controller/dt_elevation/dt_el_stoeber_r_sim.c
  14. +10
    -5
      controller/ec/ec.c
  15. +8
    -0
      controller/log/log.c
  16. +1
    -0
      controller/log/log.h
  17. +0
    -1
      test_sets/azimuth_safety_safe.set
  18. +0
    -3
      test_sets/azimuth_safety_unsafe.set
  19. +0
    -3
      test_sets/elevation_safety_left_unsafe.set
  20. +0
    -3
      test_sets/elevation_safety_right_unsafe.set
  21. +0
    -2
      test_sets/elevation_safety_safe.set

+ 1
- 1
common/utils/tcp_connect.c View File

@ -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);


+ 7
- 1
controller/controller/controller_load.c View File

@ -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;


+ 1
- 0
controller/controller/controller_load.h View File

@ -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);


+ 3
- 0
controller/controller/controller_load_parser.l View File

@ -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 "\""[^"]*"\""


+ 3
- 18
controller/dt_azimuth/dt_az_safety.c View File

@ -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;


+ 5
- 5
controller/dt_azimuth/dt_az_stoeber_sim.c View File

@ -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" },
};


+ 8
- 4
controller/dt_ctrl.c View File

@ -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;
}

+ 16
- 3
controller/dt_ctrl.ctrl View File

@ -1,5 +1,20 @@
# 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 }


+ 2
- 2
controller/dt_ctrl_az_sim.ctrl View File

@ -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 }
}

+ 0
- 4
controller/dt_ctrl_el_sim.ctrl View File

@ -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 }
}


+ 2
- 38
controller/dt_elevation/dt_el_safety.c View File

@ -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))


+ 15
- 4
controller/dt_elevation/dt_el_stoeber_l_sim.c View File

@ -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;


+ 1
- 1
controller/dt_elevation/dt_el_stoeber_r_sim.c View File

@ -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" },


+ 10
- 5
controller/ec/ec.c View File

@ -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;
}

+ 8
- 0
controller/log/log.c View File

@ -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);
}

+ 1
- 0
controller/log/log.h View File

@ -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_ */

+ 0
- 1
test_sets/azimuth_safety_safe.set View File

@ -1,5 +1,4 @@
params {
{ "dt_az_safety", "value", (int)1 }
{ "dt_az_safety_p", "value", (int)1 }
{ "dt_az_safety_n", "value", (int)1 }
}

+ 0
- 3
test_sets/azimuth_safety_unsafe.set View File

@ -1,3 +0,0 @@
params {
{ "dt_az_safety", "value", (int)0 }
}

+ 0
- 3
test_sets/elevation_safety_left_unsafe.set View File

@ -1,3 +0,0 @@
params {
{ "dt_el_safety_l", "value", (int)0 }
}

+ 0
- 3
test_sets/elevation_safety_right_unsafe.set View File

@ -1,3 +0,0 @@
params {
{ "dt_el_safety_r", "value", (int)0 }
}

+ 0
- 2
test_sets/elevation_safety_safe.set View File

@ -1,6 +1,4 @@
params {
{ "dt_el_safety_l", "value", (int)1 }
{ "dt_el_safety_r", "value", (int)1 }
{ "dt_el_safety_t", "value", (int)1 }
{ "dt_el_safety_b", "value", (int)1 }
}

Loading…
Cancel
Save