Commit 6f5658ac authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Merge branch 'controller_bus' into build

Conflicts:
	controller/controller/Makefile
	controller/controller/controller_block.c
	controller/controller/controller_load.c
	controller/dt_ctrl.c
	controller/ec/Makefile
	controller/ec/block_beckhoff_el7031.c
	controller/ec/block_stoeber.c
	controller/ec/canopen.c
	controller/ec/ec.c
	controller/ec/ec_stoeber.c
	controller/ec/ec_stoeber.h
	controller/ec/ec_test.c
	controller/ec/esc.c
	controller/ec/esc_coe.c
	controller/ec/esc_device.c
	controller/ec/esc_device.h
parents edc0ebe7 64b1a680
......@@ -52,6 +52,12 @@
<div style="border: thin solid black; background: lightgrey"
id="Azimuth_Enabled">
Azimuth Drive<br>
<ul>
<div style="border: thin solid black; background: lightgrey"
id="dt_az.external_enable">
External_enable<br>
</div>
</ul>
</div>
</td>
......@@ -103,7 +109,7 @@
<td>
<div style="border: thin solid black; background: lightgrey"
id="Azimuth_Soft_Enabled">
id="ethercat.pdo_data">
EtherCat Communication<br>
<center>
......@@ -183,6 +189,16 @@
<div style="border: thin solid black; background: lightgrey"
id="Elevation_Enabled">
Elevation Drives<br>
<ul>
<div style="border: thin solid black; background: lightgrey"
id="dt_el_r.external_enable">
External_enable Right<br>
</div>
<div style="border: thin solid black; background: lightgrey"
id="dt_el_l.external_enable">
External_enable Left<br>
</div>
</ul>
</div>
</td>
......@@ -484,6 +500,9 @@ for (i = 0; i < traceval_list.length; i++) {
Azimuth_Enabled_trace = new trace();
trace_bool('Azimuth_Enabled', Azimuth_Enabled_trace);
Azimuth_External_Enable_trace = new trace();
trace_bool('dt_az.external_enable', Azimuth_External_Enable_trace);
Azimuth_Drive_Safety_p270_trace = new trace();
trace_bool('Azimuth_Drive_Safety_p270', Azimuth_Drive_Safety_p270_trace);
......@@ -511,6 +530,11 @@ trace_bool('Elevation_Safe', Elevation_Safe_trace);
Elevation_Enabled_trace = new trace();
trace_bool('Elevation_Enabled', Elevation_Enabled_trace);
Elevation_External_Enable_Right_trace = new trace();
trace_bool('dt_el_r.external_enable', Elevation_External_Enable_Right_trace);
Elevation_External_Enable_Left_trace = new trace();
trace_bool('dt_el_l.external_enable', Elevation_External_Enable_Left_trace);
......@@ -556,6 +580,17 @@ weather.callback = dt_weather_callback;
weather.open();
/******************************************************************
*
* Ethercat
*
******************************************************************/
Ethercat_PDO_trace = new trace();
trace_bool('ethercat.pdo_data', Ethercat_PDO_trace);
</script>
</html>
......
......@@ -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;
}
}
......
......@@ -5,6 +5,7 @@ CONTROLLER_SRCS= \
$(DIR)/controller_block.c \
$(DIR)/controller_block_param.c \
$(DIR)/controller_block_trace.c \
$(DIR)/controller_bus.c \
$(DIR)/controller_command.c \
$(DIR)/controller_trace.c \
$(DIR)/controller_sample.c \
......
......@@ -26,6 +26,7 @@
#include <controller/controller_block.h>
#include <controller/controller_sample.h>
#include <controller/controller_bus.h>
#include <log/log.h>
int nr_blocks = 0;
......@@ -245,6 +246,17 @@ int controller_block_sample_init(void)
}
if (i < k)
break;
else {
for (k = i; k < nr_blocks; k++) {
if (controller_bus_linked(blocks[k], blocks[i])) {
break;
}
}
if (k == nr_blocks)
k = -1;
else
break;
}
}
if (i < k) {
struct controller_block *dep = blocks[k];
......@@ -284,13 +296,14 @@ int controller_block_sample_init(void)
"Block %s has a calculate function",
blocks[i]->name);
tmp = realloc(calculates,
sizeof(struct calculate_func) * (nr_calculates+1));
sizeof(struct calculate_func) * (nr_calculates+2));
if (tmp) {
calculates = tmp;
calculates[nr_calculates].func =
blocks[i]->calculate;
calculates[nr_calculates].block = blocks[i];
nr_calculates++;
calculates[nr_calculates].block = NULL;
} else {
log_send(LOG_T_ERROR,
"Out of memory allocating memory");
......@@ -471,10 +484,17 @@ uint32_t controller_samplenr = 0;
void controller_block_calculate(void)
{
int i;
struct calculate_func *calculate;
struct controller_block *block;
calculate = calculates;
for (i = 0; i < nr_calculates; i++)
calculates[i].func(calculates[i].block);
for (calculate = calculates; true; calculate++) {
block = calculate->block;
if (!block)
return;
calculate->func(block);
}
}
......
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2013, 2014
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 <stdlib.h>
#include <string.h>
#include <log/log.h>
#include <controller/controller_bus.h>
static struct controller_bus *bus_list = NULL;
struct controller_bus *controller_bus_create(char *type, char *name,
struct controller_block *owner,
size_t private_size)
{
struct controller_bus **entry;
for (entry = &bus_list; *entry; entry = &(*entry)->next) {
if (!strcmp((*entry)->name, name)) {
log_send(LOG_T_ERROR,
"Bus named '%s' already exists. (type= %s)",
name, (*entry)->type);
return NULL;
}
}
*entry = calloc(1, sizeof(struct controller_bus));
if (!*entry)
return NULL;
if (private_size) {
(*entry)->private = calloc(1, private_size);
if (!(*entry)->private) {
goto err_private;
}
}
(*entry)->name = strdup(name);
(*entry)->type = strdup(type);
(*entry)->owner = owner;
(*entry)->input_block = owner;
(*entry)->output_block = owner;
(*entry)->state = CONTROLLER_BUS_STATE_OK;
log_send(LOG_T_DEBUG, "New bus '%s' created of type '%s'",
name, type);
return *entry;
err_private:
free(*entry);
*entry = NULL;
return NULL;
}
int controller_bus_input_set(struct controller_bus *bus,
struct controller_block *block)
{
bus->input_block = block;
log_send(LOG_T_DEBUG, "Block '%s' handles input for bus '%s'",
block->name, bus->name);
return 0;
}
int controller_bus_output_set(struct controller_bus *bus,
struct controller_block *block)
{
bus->output_block = block;
log_send(LOG_T_DEBUG, "Block '%s' handles output for bus '%s'",
block->name, bus->name);
return 0;
}
int controller_bus_poll_set(struct controller_bus *bus,
enum controller_bus_state (*poll)(struct controller_bus *bus),
int (*recover)(struct controller_bus *bus))
{
bus->poll = poll;
bus->recover = recover;
return 0;
}
struct controller_bus *controller_bus_find(char *name)
{
struct controller_bus *entry;
for (entry = bus_list; entry; entry = entry->next)
if (!strcmp(entry->name, name))
return entry;
return NULL;
}
struct controller_bus *controller_bus_by_owner(struct controller_block *owner)
{
struct controller_bus *entry;
for (entry = bus_list; entry; entry = entry->next)
if (entry->owner == owner)
return entry;
return NULL;
}
struct controller_bus_client *controller_bus_add_input_client(
struct controller_bus *bus,
struct controller_block *client)
{
struct controller_bus_client *ic;
int i;
for (i = 0; i < bus->input_clients; i++) {
if (bus->input_client[i].block == client)
return &bus->input_client[i];
}
ic = realloc(bus->input_client,
sizeof(struct controller_bus_client) * (bus->input_clients + 1));
if (!ic)
return NULL;
bus->input_client = ic;
bus->input_client[bus->input_clients].block = client;
bus->input_client[bus->input_clients].callback = NULL;
bus->input_client[bus->output_clients].poll = NULL;
bus->input_client[bus->output_clients].recover = NULL;
bus->input_client[bus->input_clients].offset = 0;
bus->input_client[bus->output_clients].state = CONTROLLER_BUS_STATE_OK;
bus->input_clients++;
log_send(LOG_T_DEBUG, "Block '%s' is an input client of bus '%s'",
client->name, bus->name);
return &bus->input_client[bus->input_clients-1];
}
struct controller_bus_client *controller_bus_add_output_client(
struct controller_bus *bus,
struct controller_block *client)
{
struct controller_bus_client *oc;
int i;
for (i = 0; i < bus->output_clients; i++) {
if (bus->output_client[i].block == client)
return &bus->output_client[i];
}
oc = realloc(bus->output_client,
sizeof(struct controller_bus_client) * (bus->output_clients + 1));
if (!oc)
return NULL;
bus->output_client = oc;
bus->output_client[bus->output_clients].block = client;
bus->output_client[bus->output_clients].callback = NULL;
bus->output_client[bus->output_clients].poll = NULL;
bus->output_client[bus->output_clients].recover = NULL;
bus->output_client[bus->output_clients].offset = 0;
bus->output_client[bus->output_clients].state = CONTROLLER_BUS_STATE_OK;
bus->output_clients++;
log_send(LOG_T_DEBUG, "Block '%s' is an output client of bus '%s'",
client->name, bus->name);
return &bus->output_client[bus->output_clients-1];
}
bool controller_bus_linked(struct controller_block *src,
struct controller_block *dst)
{
struct controller_bus *entry;
for (entry = bus_list; entry; entry = entry->next) {
int i;
if (entry->output_block == dst) {
for (i = 0; i < entry->output_clients; i++)
if (entry->output_client[i].block == src) {
return true;
}
}
if (entry->input_block == src) {
for (i = 0; i < entry->input_clients; i++)
if (entry->input_client[i].block == dst) {
return true;
}
}
}
return false;
}
struct controller_bus *controller_bus_linked_bus(struct controller_block *src,
struct controller_block *dst)
{
struct controller_bus *entry;
for (entry = bus_list; entry; entry = entry->next) {
int i;
if (entry->output_block == dst) {
for (i = 0; i < entry->output_clients; i++)
if (entry->output_client[i].block == src) {
return entry;
}
}
if (entry->input_block == src) {
for (i = 0; i < entry->input_clients; i++)
if (entry->input_client[i].block == dst) {
return entry;
}
}
}
return NULL;
}
int controller_bus_poll_states(int *oks, int *errors, int *recoverables)
{
struct controller_bus *entry;
int error = 0;
int recoverable = 0;
int ok = 0;
for (entry = bus_list; entry; entry = entry->next) {
int i;
int entry_error = 0;
int entry_recoverable = 0;
if (entry->poll) {
enum controller_bus_state newstate;
newstate = entry->poll(entry);
if (newstate == CONTROLLER_BUS_STATE_ERROR) {
entry_error++;
error++;
}
else if (newstate == CONTROLLER_BUS_STATE_RECOVERABLE) {
entry_recoverable++;
recoverable++;
}
else if (newstate == CONTROLLER_BUS_STATE_OK)
ok++;
if (newstate != entry->state) {
log_send(LOG_T_WARNING,
"%s: changed state from %s to %s",
entry->name,
controller_bus_state_2_str(entry->state),
controller_bus_state_2_str(newstate));
}
entry->state = newstate;
}
for (i = 0; i < entry->input_clients; i++) {
if (entry->input_client[i].poll) {
enum controller_bus_state newstate;
newstate = entry->input_client[i].poll(entry->input_client+i);
if (newstate == CONTROLLER_BUS_STATE_ERROR) {
entry_error++;
error++;
} else if (newstate == CONTROLLER_BUS_STATE_RECOVERABLE) {
entry_recoverable++;
recoverable++;
}
else if (newstate == CONTROLLER_BUS_STATE_OK)
ok++;
if (newstate != entry->input_client[i].state) {
log_send(LOG_T_WARNING,
"%s: Input client %s changed state from %s to %s",
entry->name, entry->input_client[i].block->name,
controller_bus_state_2_str(entry->input_client[i].state),
controller_bus_state_2_str(newstate));
}
entry->input_client[i].state = newstate;
}
}
for (i = 0; i < entry->output_clients; i++) {
if (entry->output_client[i].poll) {
enum controller_bus_state newstate;
newstate = entry->output_client[i].poll(entry->output_client+i);
if (newstate == CONTROLLER_BUS_STATE_ERROR) {
entry_error++;
error++;
}
else if (newstate == CONTROLLER_BUS_STATE_RECOVERABLE) {
entry_recoverable++;
recoverable++;
}
else if (newstate == CONTROLLER_BUS_STATE_OK)
ok++;
if (newstate != entry->output_client[i].state) {
log_send(LOG_T_WARNING,
"%s: output client %s changed state from %s to %s",
entry->name, entry->output_client[i].block->name,
controller_bus_state_2_str(entry->output_client[i].state),
controller_bus_state_2_str(newstate));
}
entry->output_client[i].state = newstate;
}
}
if (entry_error)
entry->state = CONTROLLER_BUS_STATE_ERROR;
if (entry_recoverable)
entry->state = CONTROLLER_BUS_STATE_RECOVERABLE;
}
if (oks)
*oks = ok;
if (errors)
*errors = error;
if (recoverables)
*recoverables = recoverable;
return 0;
}
int controller_bus_recover(void)
{
struct controller_bus *entry;
for (entry = bus_list; entry; entry = entry->next) {
int i;
if (entry->state == CONTROLLER_BUS_STATE_RECOVERABLE &&
entry->recover) {
log_send(LOG_T_INFO,
"%s: Attempt to recover", entry->name);
entry->recover(entry);
}
for (i = 0; i < entry->input_clients; i++) {
if (entry->input_client[i].state == CONTROLLER_BUS_STATE_RECOVERABLE &&
entry->input_client[i].recover) {
log_send(LOG_T_INFO,
"%s: Attempt to recover input client %s",
entry->name, entry->input_client[i].block->name);
entry->input_client[i].recover(entry->input_client+i);
}
}
for (i = 0; i < entry->output_clients; i++) {
if (entry->output_client[i].state == CONTROLLER_BUS_STATE_RECOVERABLE &&
entry->output_client[i].recover) {
log_send(LOG_T_INFO,
"%s: Attempt to recover output client %s",
entry->name, entry->output_client[i].block->name);
entry->output_client[i].recover(entry->output_client+i);
}
}
}
return 0;
}
char *controller_bus_state_2_str(enum controller_bus_state state)
{
switch (state) {
case CONTROLLER_BUS_STATE_OK:
return "OK";
case CONTROLLER_BUS_STATE_RECOVERABLE:
return "RECOVERABLE";
case CONTROLLER_BUS_STATE_ERROR:
return "ERROR";
}
return "INVALID";
}
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 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/>.
*/
#ifndef _INCLUDE_CONTROLLER_BUS_H_
#define _INCLUDE_CONTROLLER_BUS_H_
#include "controller_block.h"
enum controller_bus_state {
CONTROLLER_BUS_STATE_OK,
CONTROLLER_BUS_STATE_RECOVERABLE,
CONTROLLER_BUS_STATE_ERROR,
};
char *controller_bus_state_2_str(enum controller_bus_state);
struct controller_bus_client {
struct controller_block *block;
void (*callback)(struct controller_block *block, void *data);
enum controller_bus_state (*poll)(struct controller_bus_client *client);