Fix some logging

Add poll/recover to bus

controller/controller/controller_bus.c

 /*
-	Copyright Jeroen Vreeken (jeroen@vreeken.net), 2013
+	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
@@ -54,6 +54,8 @@ struct controller_bus *controller_bus_create(char *type, char *name,
 	(*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);
 
@@ -87,6 +89,15 @@ int controller_bus_output_set(struct controller_bus *bus,
 	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)
 {
@@ -133,7 +144,7 @@ struct controller_bus_client *controller_bus_add_input_client(
 	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_CLIENT_STATE_OK;
+	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'",
@@ -165,7 +176,7 @@ struct controller_bus_client *controller_bus_add_output_client(
 	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_CLIENT_STATE_OK;
+	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'",
@@ -231,44 +242,73 @@ int controller_bus_poll_states(int *oks, int *errors, int *recoverables)
 	
 	for (entry = bus_list; entry; entry = entry->next) {
 		int i;
+
+		if (entry->poll) {
+			enum controller_bus_state newstate;
+			
+			newstate = entry->poll(entry);
+				
+			if (newstate == CONTROLLER_BUS_STATE_ERROR)
+				error++;
+			else if (newstate == CONTROLLER_BUS_STATE_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_client_state newstate;
+				enum controller_bus_state newstate;
 				
 				newstate = entry->input_client[i].poll(entry->input_client+i);
 				
-				if (newstate == CONTROLLER_BUS_CLIENT_STATE_ERROR)
+				if (newstate == CONTROLLER_BUS_STATE_ERROR)
 					error++;
-				else if (newstate == CONTROLLER_BUS_CLIENT_STATE_RECOVERABLE)
+				else if (newstate == CONTROLLER_BUS_STATE_RECOVERABLE)
 					recoverable++;
-				else if (newstate == CONTROLLER_BUS_CLIENT_STATE_OK)
+				else if (newstate == CONTROLLER_BUS_STATE_OK)
 					ok++;
 				
 				if (newstate != entry->input_client[i].state) {
 					log_send(LOG_T_WARNING, 
-					    "Input client %d of bus %s changed state from %d to %d\n",
-					    i, entry->name, newstate, entry->input_client[i].state);
+					    "%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_client_state newstate;
+				enum controller_bus_state newstate;
 				
 				newstate = entry->output_client[i].poll(entry->output_client+i);
 				
-				if (newstate == CONTROLLER_BUS_CLIENT_STATE_ERROR)
+				if (newstate == CONTROLLER_BUS_STATE_ERROR)
 					error++;
-				else if (newstate == CONTROLLER_BUS_CLIENT_STATE_RECOVERABLE)
+				else if (newstate == CONTROLLER_BUS_STATE_RECOVERABLE)
 					recoverable++;
-				else if (newstate == CONTROLLER_BUS_CLIENT_STATE_OK)
+				else if (newstate == CONTROLLER_BUS_STATE_OK)
 					ok++;
 				
 				if (newstate != entry->output_client[i].state) {
 					log_send(LOG_T_WARNING, 
-					    "output client %d of bus %s changed state from %d to %d\n",
-					    i, entry->name, newstate, entry->output_client[i].state);
+					    "%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;
 			}
 		}
 	}
@@ -288,22 +328,31 @@ int controller_bus_recover(void)
 
 	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_CLIENT_STATE_RECOVERABLE &&
+			if (entry->input_client[i].state == CONTROLLER_BUS_STATE_RECOVERABLE &&
 			    entry->input_client[i].recover) {
 				log_send(LOG_T_INFO, 
-				    "Attempt to recover input client %d on bus %s\n",
-				    i, entry->name);
+				    "%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_CLIENT_STATE_RECOVERABLE &&
+			if (entry->output_client[i].state == CONTROLLER_BUS_STATE_RECOVERABLE &&
 			    entry->output_client[i].recover) {
 				log_send(LOG_T_INFO, 
-				    "Attempt to recover output client %d on bus %s\n",
-				    i, entry->name);
+				    "%s: Attempt to recover output client %s",
+				    entry->name, entry->output_client[i].block->name);
 				
 				entry->output_client[i].recover(entry->output_client+i);
 			}
@@ -313,3 +362,16 @@ int controller_bus_recover(void)
 	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";
+}

controller/controller/controller_bus.h

@@ -21,21 +21,23 @@
 
 #include "controller_block.h"
 
-enum controller_bus_client_state {
-	CONTROLLER_BUS_CLIENT_STATE_OK,
-	CONTROLLER_BUS_CLIENT_STATE_RECOVERABLE,
-	CONTROLLER_BUS_CLIENT_STATE_ERROR,
+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_client_state (*poll)(struct controller_bus_client *client);
+	enum controller_bus_state (*poll)(struct controller_bus_client *client);
 	int (*recover)(struct controller_bus_client *client);
 	
 	size_t offset;
-	enum controller_bus_client_state state;
+	enum controller_bus_state state;
 
 	struct controller_bus_client_private *private;	
 };
@@ -54,6 +56,11 @@ struct controller_bus {
 	int output_clients;
 	struct controller_bus_client *output_client;
 
+	enum controller_bus_state (*poll)(struct controller_bus *bus);
+	int (*recover)(struct controller_bus *bus);
+
+	enum controller_bus_state state;
+
 	struct controller_bus_private *private;
 	struct controller_bus *next;
 };
@@ -68,6 +75,11 @@ int controller_bus_input_set(struct controller_bus *bus,
 int controller_bus_output_set(struct controller_bus *bus,
     struct controller_block *block);
 
+int controller_bus_poll_set(struct controller_bus *bus,
+    enum controller_bus_state (*poll)(struct controller_bus *bus),
+    int (*recover)(struct controller_bus *bus));
+    
+
 struct controller_bus *controller_bus_find(char *name);
 
 struct controller_bus_client *controller_bus_add_input_client(

controller/dt_ctrl.c

@@ -100,10 +100,30 @@ int main(int argc, char **argv)
 	controller_sample_start();
 
 	log_send(LOG_T_DEBUG, "Entering state polling loop");
+	int oks_p = 0, errors_p = 0, recoverables_p = 0;
 	while (1) {
 		int oks, errors, recoverables;
 		
 		controller_bus_poll_states(&oks, &errors, &recoverables);
+		if (oks != oks_p) {
+			log_send(LOG_T_WARNING, 
+			    "Number of OK blocks changed: %d -> %d",
+			    oks_p, oks);
+			oks_p = oks;
+		}
+		if (errors != errors_p) {
+			log_send(LOG_T_WARNING, 
+			    "Number of ERROR blocks changed: %d -> %d",
+			    errors_p, errors);
+			errors_p = errors;
+		}
+		if (recoverables != recoverables_p) {
+			log_send(LOG_T_WARNING, 
+			    "Number of RECOVERABLE blocks changed: %d -> %d",
+			    recoverables_p, recoverables);
+			recoverables_p = recoverables;
+		}
+		
 		if (recoverables) {
 			controller_bus_recover();
 		}

controller/ec/ec.c

@@ -541,7 +541,7 @@ int ec_slave_count(void)
 
 	if (ret)
 	{
-		log_send(LOG_T_ERROR, "Did not send/receive ethercat frame!\n");
+		log_send(LOG_T_ERROR, "Did not send/receive ethercat frame!");
 		return -1;
 	}
 	

controller/ec/ec_test.ctrl

+frequency 1000
+
+# simple set of inputs and outputs to test ethercat stack
+
+blocks {
+	{ "ec",              "ethercat",   "eth0", 6, 1 }
+
+	{ "beckhoff_el1xxx", "input8",     "ethercat", 1, "EL1008" }
+	{ "beckhoff_el2xxx", "output8",    "ethercat", 2, "EL2008" }
+
+	{ "beckhoff_el3xxx", "input_an2",  "ethercat", 3, "EL3102" }
+	{ "beckhoff_el4xxx", "output_an2", "ethercat", 4, "EL4132" }
+	
+	{ "beckhoff_el5101", "encoder", "ethercat", 5 }
+
+}
+
+links {
+	{ "input8", "input1", "output8", "output1", true }
+	{ "input8", "input2", "output8", "output2", true }
+	{ "input8", "input3", "output8", "output3", true }
+	{ "input8", "input4", "output8", "output4", true }
+	{ "input8", "input5", "output8", "output5", true }
+	{ "input8", "input6", "output8", "output6", true }
+	{ "input8", "input7", "output8", "output7", true }
+	{ "input8", "input8", "output8", "output8", true }
+
+	{ "input_an2", "input1", "output_an2", "output1", true }
+	{ "input_an2", "input2", "output_an2", "output2", true }
+}

controller/ec/esc.c

@@ -104,26 +104,29 @@ int esc_al_state_set(struct esc_device *esc, enum esc_al_state newstate,
 				return newstate;
 			else if (rd_state == ESC_AL_STATE_ERROR)
 				log_send(LOG_T_ERROR, 
-				    "Could not read state (trying to set %d %s)",
+				    "%s: Could not read state (trying to set %d %s)",
+				    esc->name,
 				    newstate,
 				    esc_al_state2str(newstate));
 				    
 			status = esc_al_status_code_get(esc);
 			if (status < 0) {
 				log_send(LOG_T_ERROR,
-				    "Could not read status");
+				    "%s: Could not read status",
+				    esc->name);
 				return -1;
 			} else if (status != 0) {
 				log_send(LOG_T_DEBUG, 
-				    "status code: 0x%x %s", status,
-				    esc_al_status2str(status));
+				    "%s: status code: 0x%x %s", esc->name,
+				    status, esc_al_status2str(status));
 				esc_al_error_ack(esc);
 				
 				return -1;
 			}
 		} while (!esc_timeout_expired(&t_start, timeout));
 		log_send(LOG_T_ERROR, 
-		    "Timeout while trying to set state %d %s, stuck in state %d %s",
+		    "%s: Timeout while trying to set state %d %s, stuck in state %d %s",
+		    esc->name,
 		    newstate, esc_al_state2str(newstate), 
 		    rd_state, esc_al_state2str(rd_state));
 		/* timeout... */

controller/ec/esc_device.c

@@ -23,7 +23,6 @@
  */
 
 #include <string.h>
-#include <pthread.h>
 #include <limits.h>
 
 #include "ec.h"
@@ -86,8 +85,6 @@ err_dev:
 
 void esc_device_destroy(struct esc_device *dev)
 {
-	/* todo: kill poll thread */
-
 	if (dev->categories)
 		esc_esi_categories_destroy(dev->categories);
 	if (dev->name)
@@ -100,10 +97,10 @@ struct controller_bus_client_private {
 	struct esc_device dev;
 };
 
-static enum controller_bus_client_state esc_device_poll(struct controller_bus_client *client)
+static enum controller_bus_state esc_device_poll(struct controller_bus_client *client)
 {
 	struct esc_device *dev = &client->private->dev;
-	enum controller_bus_client_state poll_state = CONTROLLER_BUS_CLIENT_STATE_OK;
+	enum controller_bus_state poll_state = CONTROLLER_BUS_STATE_OK;
 	char *str;
 	enum esc_al_status status;
 	enum esc_al_state state;
@@ -112,7 +109,7 @@ static enum controller_bus_client_state esc_device_poll(struct controller_bus_cl
 	state = esc_al_state_get(dev);
 
 	if (state == ESC_AL_STATE_ERROR || status < 0) {
-		poll_state = CONTROLLER_BUS_CLIENT_STATE_ERROR;
+		poll_state = CONTROLLER_BUS_STATE_ERROR;
 	} else {
 		if (state != dev->state) {
 			log_send(LOG_T_WARNING,
@@ -127,7 +124,7 @@ static enum controller_bus_client_state esc_device_poll(struct controller_bus_cl
 				log_send(LOG_T_WARNING,
 				    "%s: ESC status changed: No error", dev->name);
 			} else {
-				poll_state = CONTROLLER_BUS_CLIENT_STATE_RECOVERABLE;
+				poll_state = CONTROLLER_BUS_STATE_RECOVERABLE;
 				switch(status) {
 					case ESC_AL_STATUS_SM_WATCHDOG:
 						str = "Sync manager watchdog";