Commit f9a8136f authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Merge branch 'controller_bus' into corso2013

Conflicts:
	controller/block/block_servo_state.c
parents 3f28c014 7a2c1190
......@@ -75,6 +75,7 @@ static void servo_state_calculate(struct controller_block *servo_state)
{
struct controller_block_private *priv = servo_state->private;
bool safe = *priv->safe;
bool unsafe_disable = false;
if (priv->emergency) {
priv->emergency = false;
......@@ -83,6 +84,8 @@ static void servo_state_calculate(struct controller_block *servo_state)
servo_state->name);
}
if (!safe) {
if (priv->enable_param)
unsafe_disable = true;
priv->enable_param = false;
}
......@@ -196,6 +199,11 @@ static void servo_state_calculate(struct controller_block *servo_state)
"%s: Going to state ENABLED",
servo_state->name);
}
if (unsafe_disable) {
log_send(LOG_T_WARNING,
"%s: Unsafe, enabling is currently not allowed",
servo_state->name);
}
break;
}
}
......
......@@ -14,6 +14,7 @@ frequency 250
# 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
# dt_ctrl_ec_sim.ctrl Ethercat simulation block
#
# Uncomment either the real network, or the sim network.
# But never both!
......@@ -24,10 +25,12 @@ include "dt_ctrl_el_sim.ctrl"
#include "dt_ctrl_az.ctrl"
include "dt_ctrl_az_sim.ctrl"
include "dt_ctrl_ec_sim.ctrl"
blocks {
{ "setpoint_generator", "azimuth_spg", "Azimuth_Setpoint", "rad" }
{ "servo_state", "azimuth_servo_state" }
{ "and2", "azimuth_safe_and" }
{ "subtract", "azimuth_setpoint_error" }
{ "subtract", "azimuth_error" }
{ "pid_aw", "azimuth_pid" }
......@@ -46,6 +49,7 @@ blocks {
{ "matrix_2x2", "elevation_input_matrix" }
{ "setpoint_generator", "elevation_spg", "Elevation_Setpoint", "rad" }
{ "servo_state", "elevation_servo_state" }
{ "and2", "elevation_safe_and" }
{ "setpoint_generator", "elevation_torsion_spg", "Elevation_Torsion_Setpoint", "rad" }
{ "subtract", "elevation_setpoint_error" }
{ "subtract", "elevation_error" }
......@@ -82,7 +86,9 @@ links {
{ "azimuth_spg", "x", "azimuth_servo_state", "spg_x" , true }
{ "azimuth_spg", "v", "azimuth_servo_state", "spg_v" , true }
{ "azimuth_spg", "a", "azimuth_servo_state", "spg_a" , true }
{ "azimuth_safety", "safe_out", "azimuth_servo_state", "safe" , false }
{ "azimuth_safety", "safe_out", "azimuth_safe_and", "a" , false }
{ "ethercat", "pdo_data", "azimuth_safe_and", "b" , true }
{ "azimuth_safe_and", "q", "azimuth_servo_state", "safe" , true }
{ "azimuth_servo_state", "out_x", "azimuth_error", "positive" , true }
{ "azimuth_position_offset_sum", "out", "azimuth_setpoint_error", "negative" , true }
{ "azimuth_spg", "setpoint", "azimuth_setpoint_error", "positive" , true }
......@@ -113,7 +119,9 @@ links {
{ "elevation_spg", "x", "elevation_servo_state", "spg_x" , true }
{ "elevation_spg", "v", "elevation_servo_state", "spg_v" , true }
{ "elevation_spg", "a", "elevation_servo_state", "spg_a" , true }
{ "elevation_safety", "safe_out", "elevation_servo_state", "safe" , false }
{ "elevation_safety", "safe_out", "elevation_safe_and", "a" , false }
{ "ethercat", "pdo_data", "elevation_safe_and", "b" , true }
{ "elevation_safe_and", "q", "elevation_servo_state", "safe" , true }
{ "elevation_servo_state", "out_x", "elevation_error", "positive" , true }
{ "elevation_servo_state", "out_v", "elevation_speed_ff", "in0" , true }
{ "dt_el_r", "position", "elevation_position_offset_r_sum", "in0" , true }
......
blocks {
{ "ec_sim", "ethercat" }
}
......@@ -12,7 +12,7 @@ endif
LIBSRCS= ec.c \
esc.c esc_coe.c esc_esi.c esc_mailbox.c esc_device.c esc_watchdog.c \
esc_dc.c canopen.c block_ec.c esc_sm.c esc_fmmu.c
esc_dc.c canopen.c esc_sm.c esc_fmmu.c
ifeq ($(OS), FreeBSD)
LIBSRCS+= eth_bsd.c
......@@ -31,7 +31,9 @@ BLOCKSRCS= \
block_beckhoff_el5001.c \
block_beckhoff_el5101.c \
block_beckhoff_el7031.c \
block_stoeber.c
block_stoeber.c \
block_ec.c \
block_ec_sim.c
# block_beckhoff_ax5xxx.c \
......
......@@ -46,7 +46,7 @@ static void calculate_tx(struct controller_block *ec)
ec_datagram_sample();
}
static void param_set(struct controller_block *spg, int param, va_list val)
static void param_set(struct controller_block *ec, int param, va_list val)
{
int enabled = va_arg(val, int);
......@@ -85,26 +85,11 @@ struct controller_block * block_ec_create(char *name, va_list ap)
nr_slaves = va_arg(ap, int);
single_cycle = va_arg(ap, int);
ret = ec_init(ifname, single_cycle);
ret = ec_init(ifname, nr_slaves, single_cycle);
if (ret != 0)
return NULL;
while ((ret = ec_slave_count()) != nr_slaves) {
if (ret == -1) {
log_send(LOG_T_ERROR, "Error counting slaves");
} else if (ret < nr_slaves) {
log_send(LOG_T_ERROR,
"Found only %d slaves, waiting for %d.",
ret, nr_slaves);
} else {
log_send(LOG_T_WARNING,
"More slaves found (%d) than expected (%d).",
ret, nr_slaves);
break;
}
sleep(10);
}
sprintf(name_tx, "%s_pdo_tx", name);
ec = controller_block_alloc("ec", name,
......
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2013
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <controller/controller_block.h>
#include "log.h"
struct controller_block *ec_block;
struct controller_block_private {
bool pdo_data;
};
static void param_set(struct controller_block *ec, int param, va_list val)
{
int enabled = va_arg(val, int);
ec->private->pdo_data = enabled;
}
static struct controller_block_param_list params[] = {
{ "pdo_enabled", true },
{ NULL },
};
static struct controller_block_outterm_list outterms[] = {
{ "pdo_data", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, pdo_data) },
{ NULL },
};
struct controller_block * block_ec_sim_create(char *name, va_list ap)
{
struct controller_block *ec;
/* For now we only support a single ethercat interface,
but we allow multiple inits
*/
if (ec_block) {
return ec_block;
}
ec = controller_block_alloc("ec_sim", name,
sizeof(struct controller_block_private));
if (!ec)
goto err;
ec->private->pdo_data = true;
if (controller_block_outterm_list_init(ec, outterms))
goto err_ec;
if (controller_block_param_list_init(ec, params))
goto err_ec;
ec->param_get = NULL;
ec->param_set = param_set;
controller_block_add(ec);
ec_block = ec;
return ec;
err_ec:
controller_block_free(ec);
err:
return NULL;
}
......@@ -850,10 +850,11 @@ bool ec_initialized(void)
return init_done;
}
int ec_init(char *ifname, bool single_cycle)
int ec_init(char *ifname, int nr_slaves, bool single_cycle)
{
struct timeval timeout;
double timeout_sec;
int ret;
if (init_done)
return 0;
......@@ -881,10 +882,22 @@ int ec_init(char *ifname, bool single_cycle)
eth_timeout_set(ec_sock, &timeout, &timeout);
if (ec_slave_count() >= 0) {
log_send(LOG_T_DEBUG, "Found ethercat slaves");
} else {
log_send(LOG_T_WARNING, "No ethercat slaves found");
while ((ret = ec_slave_count()) != nr_slaves) {
if (ret == -1) {
log_send(LOG_T_ERROR, "Error counting ethercat slaves");
} else if (nr_slaves > 0) {
if (ret < nr_slaves) {
log_send(LOG_T_ERROR,
"Found only %d ethercat slaves, waiting for %d.",
ret, nr_slaves);
} else {
log_send(LOG_T_WARNING,
"More ethercat slaves found (%d) than expected (%d).",
ret, nr_slaves);
break;
}
}
sleep(10);
}
log_send(LOG_T_DEBUG,
"Successfull init, %d slaves found.", ec_slave_count());
......
......@@ -36,7 +36,7 @@
#include "canopen.h"
int ec_init(char *ifname, bool single_cycle);
int ec_init(char *ifname, int nr_slaves, bool single_cycle);
bool ec_initialized(void);
int ec_slave_count(void);
......
......@@ -300,7 +300,7 @@ int main(int argc, char **argv)
return -1;
}
if (ec_init(argv[1], 0)) {
if (ec_init(argv[1], 0, 0)) {
printf("EtherCat init failed\n");
return -1;
}
......
Supports Markdown
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