Skip to content

GitLab

Commit 7a2c1190 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

ethercat status

parent 5edaf3bc
Hide whitespace changes
Inline Side-by-side
controller/block/block_servo_state.c
View file @ 7a2c1190
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2011
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2011, 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2011
This program is free software: you can redistribute it and/or modify
......@@ -71,56 +71,56 @@ struct controller_block_private {
bool override;
};
static void calculate(struct controller_block *servo_state)
static void servo_state_calculate(struct controller_block *servo_state)
{
bool safe = *servo_state->private->safe;
struct controller_block_private *priv = servo_state->private;
bool safe = *priv->safe;
bool unsafe_disable = false;
if (servo_state->private->emergency) {
servo_state->private->emergency = false;
if (priv->emergency) {
priv->emergency = false;
safe = false;
log_send(LOG_T_ERROR, "%s: External emergency triggered",
servo_state->name);
}
if (!safe) {
servo_state->private->enable_param = false;
if (priv->enable_param)
unsafe_disable = true;
priv->enable_param = false;
}
if (servo_state->private->override) {
servo_state->private->out_x = *servo_state->private->spg_x;
servo_state->private->out_v = *servo_state->private->spg_v;
servo_state->private->out_a = *servo_state->private->spg_a;
servo_state->private->enable = true;
servo_state->private->reset = false;
if (priv->override) {
priv->out_x = *priv->spg_x;
priv->out_v = *priv->spg_v;
priv->out_a = *priv->spg_a;
priv->enable = true;
priv->reset = false;
return;
}
switch (servo_state->private->state) {
switch (priv->state) {
case SERVO_STATE_ENABLED:
servo_state->private->out_x =
*servo_state->private->spg_x;
servo_state->private->out_v =
*servo_state->private->spg_v;
servo_state->private->out_a =
*servo_state->private->spg_a;
priv->out_x = *priv->spg_x;
priv->out_v = *priv->spg_v;
priv->out_a = *priv->spg_a;
if (!safe) {
servo_state->private->enable = false;
servo_state->private->reset = true;
servo_state->private->state =
SERVO_STATE_DISABLED;
priv->enable = false;
priv->reset = true;
priv->state = SERVO_STATE_DISABLED;
log_send(LOG_T_ERROR,
"%s: Unsafe, going to state DISABLED",
servo_state->name);
break;
}
servo_state->private->enable = true;
servo_state->private->reset = false;
priv->enable = true;
priv->reset = false;
if (servo_state->private->enable_param) {
if (priv->enable_param) {
break;
} else {
servo_state->private->state =
priv->state =
SERVO_STATE_DISABLING;
log_send(LOG_T_INFO,
"%s: Going to state DISABLING",
......@@ -128,16 +128,16 @@ static void calculate(struct controller_block *servo_state)
}
/* Fallthrough if disabling */
case SERVO_STATE_DISABLING: {
float out_x = servo_state->private->out_x;
float out_v = servo_state->private->out_v;
float out_x = priv->out_x;
float out_v = priv->out_v;
float out_a = 0.0;
float t = controller_sample_period();
if (!safe) {
servo_state->private->enable = false;
servo_state->private->reset = true;
servo_state->private->state =
priv->enable = false;
priv->reset = true;
priv->state =
SERVO_STATE_DISABLED;
log_send(LOG_T_ERROR,
"%s: Unsafe, going to state DISABLED",
......@@ -146,14 +146,14 @@ static void calculate(struct controller_block *servo_state)
}
if (out_v > 0.0) {
out_a = -servo_state->private->max_a;
out_a = -priv->max_a;
out_v += out_a * t;
if (out_v < 0.0) {
out_a += out_v;
out_v = 0.0;
}
} else if (out_v < 0.0) {
out_a = servo_state->private->max_a;
out_a = priv->max_a;
out_v += out_a * t;
if (out_v > 0.0) {
out_a -= out_v;
......@@ -162,7 +162,7 @@ static void calculate(struct controller_block *servo_state)
}
if (out_v == 0.0) {
servo_state->private->state =
priv->state =
SERVO_STATE_DISABLED;
log_send(LOG_T_INFO,
"%s: Going to state DISABLED",
......@@ -171,36 +171,39 @@ static void calculate(struct controller_block *servo_state)
out_x += out_v * t;
if (out_x > servo_state->private->max_x)
out_x = servo_state->private->max_x;
if (out_x < servo_state->private->min_x)
out_x = servo_state->private->min_x;
if (out_x > priv->max_x)
out_x = priv->max_x;
if (out_x < priv->min_x)
out_x = priv->min_x;
servo_state->private->out_x = out_x;
servo_state->private->out_v = out_v;
servo_state->private->out_a = out_a;
priv->out_x = out_x;
priv->out_v = out_v;
priv->out_a = out_a;
servo_state->private->enable = true;
servo_state->private->reset = true;
priv->enable = true;
priv->reset = true;
break;
}
case SERVO_STATE_DISABLED:
servo_state->private->out_x =
*servo_state->private->spg_x;
servo_state->private->out_v = 0.0;
servo_state->private->out_a = 0.0;
priv->out_x = *priv->spg_x;
priv->out_v = 0.0;
priv->out_a = 0.0;
servo_state->private->enable = false;
servo_state->private->reset = true;
priv->enable = false;
priv->reset = true;
if (servo_state->private->enable_param) {
servo_state->private->state =
SERVO_STATE_ENABLED;
if (priv->enable_param) {
priv->state = SERVO_STATE_ENABLED;
log_send(LOG_T_INFO,
"%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;
}
}
......@@ -303,19 +306,9 @@ struct controller_block * block_servo_state_create(char *name)
{
struct controller_block *servo_state;
servo_state = malloc(sizeof(struct controller_block));
if (!servo_state)
if (!(servo_state = controller_block_alloc("servo_state", name, sizeof(struct controller_block_private))))
return NULL;
servo_state->type = "servo_state";
servo_state->name = malloc(strlen(name)+1);
if (!servo_state->name)
goto err_servo_state;
strcpy(servo_state->name, name);
servo_state->private = malloc(sizeof(struct controller_block_private));
if (!servo_state->private)
goto err_name;
servo_state->private->out_x = 0.0;
servo_state->private->out_v = 0.0;
servo_state->private->out_a = 0.0;
......@@ -327,15 +320,15 @@ struct controller_block * block_servo_state_create(char *name)
servo_state->private->override = false;
if (controller_block_interm_list_init(servo_state, interms))
goto err_private;
goto err_block;
if (controller_block_outterm_list_init(servo_state, outterms))
goto err_input;
goto err_block;
servo_state->calculate = calculate;
servo_state->calculate = servo_state_calculate;
if (controller_block_param_list_init(servo_state, params))
goto err_output;
goto err_block;
servo_state->param_get = param_get;
servo_state->param_set = param_set;
......@@ -343,15 +336,7 @@ struct controller_block * block_servo_state_create(char *name)
controller_block_add(servo_state);
return servo_state;
err_output:
free(servo_state->output);
err_input:
free(servo_state->input);
err_private:
free(servo_state->private);
err_name:
free(servo_state->name);
err_servo_state:
free(servo_state);
err_block:
controller_block_free(servo_state);
return NULL;
}
controller/dt_ctrl.ctrl
View file @ 7a2c1190
......@@ -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" }
......@@ -45,6 +48,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" }
......@@ -81,7 +85,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 }
......@@ -111,7 +117,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 }
......
controller/dt_ctrl_ec_sim.ctrl 0 → 100644
View file @ 7a2c1190
blocks {
{ "ec_sim", "ethercat" }
}
controller/ec/Makefile
View file @ 7a2c1190
......@@ -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 \
......
controller/ec/block_ec.c
View file @ 7a2c1190
......@@ -36,8 +36,6 @@ struct controller_block_private {
static void calculate(struct controller_block *ec)
{
if (ec_rx_pdo()) {
ec->private->pdo_data = false;
} else {
ec->private->pdo_data = true;
}
}
......@@ -48,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);
......@@ -87,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,
......
controller/ec/block_ec_sim.c 0 → 100644
View file @ 7a2c1190
/*
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;
}
controller/ec/ec.c
View file @ 7a2c1190
......@@ -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());
......
controller/ec/ec.h
View file @ 7a2c1190
......@@ -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);
......
controller/ec/ec_enum.c
View file @ 7a2c1190
......@@ -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;
}
......
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