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

Introduce status poll and recover functions

Modify idle loop of dt_ctrl to poll all busses and recover when
needed and possible.
parent cf2b006f
......@@ -130,7 +130,10 @@ struct controller_bus_client *controller_bus_add_input_client(
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_CLIENT_STATE_OK;
bus->input_clients++;
log_send(LOG_T_DEBUG, "Block '%s' is an input client of bus '%s'",
......@@ -159,7 +162,10 @@ struct controller_bus_client *controller_bus_add_output_client(
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_CLIENT_STATE_OK;
bus->output_clients++;
log_send(LOG_T_DEBUG, "Block '%s' is an output client of bus '%s'",
......@@ -215,3 +221,95 @@ struct controller_bus *controller_bus_linked_bus(struct controller_block *src,
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;
for (i = 0; i < entry->input_clients; i++) {
if (entry->input_client[i].poll) {
enum controller_bus_client_state newstate;
newstate = entry->input_client[i].poll(entry->input_client+i);
if (newstate == CONTROLLER_BUS_CLIENT_STATE_ERROR)
error++;
else if (newstate == CONTROLLER_BUS_CLIENT_STATE_RECOVERABLE)
recoverable++;
else if (newstate == CONTROLLER_BUS_CLIENT_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);
}
}
}
for (i = 0; i < entry->output_clients; i++) {
if (entry->output_client[i].poll) {
enum controller_bus_client_state newstate;
newstate = entry->output_client[i].poll(entry->output_client+i);
if (newstate == CONTROLLER_BUS_CLIENT_STATE_ERROR)
error++;
else if (newstate == CONTROLLER_BUS_CLIENT_STATE_RECOVERABLE)
recoverable++;
else if (newstate == CONTROLLER_BUS_CLIENT_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);
}
}
}
}
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;
for (i = 0; i < entry->input_clients; i++) {
if (entry->input_client[i].state == CONTROLLER_BUS_CLIENT_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);
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 &&
entry->output_client[i].recover) {
log_send(LOG_T_INFO,
"Attempt to recover output client %d on bus %s\n",
i, entry->name);
entry->output_client[i].recover(entry->output_client+i);
}
}
}
return 0;
}
......@@ -21,10 +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,
};
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);
int (*recover)(struct controller_bus_client *client);
size_t offset;
enum controller_bus_client_state state;
struct controller_bus_client_private *private;
};
struct controller_bus {
......@@ -73,4 +86,8 @@ struct controller_bus *controller_bus_linked_bus(struct controller_block *src,
struct controller_bus *controller_bus_by_owner(struct controller_block *owner);
int controller_bus_poll_states(int *oks, int *errors, int *recoverables);
int controller_bus_recover(void);
#endif /* _INCLUDE_CONTROLLER_BUS_H_ */
......@@ -41,8 +41,8 @@
#include "controller_sample.h"
#include "controller_dumpdot.h"
#include "controller_load.h"
#include "controller_bus.h"
#include "shell.h"
#include "ec.h"
#include "log.h"
#include "dt_port_numbers.h"
......@@ -99,23 +99,16 @@ int main(int argc, char **argv)
controller_sample_start();
log_send(LOG_T_DEBUG, "Entering state polling loop");
while (1) {
float az_pos;
float el_pos;
float el_tor;
if (controller_block_output_get_float("elevation_input_matrix", "out0", &el_pos))
log_send(LOG_T_ERROR, "Getting elevation_input_matrix.out0 failed\n");
if (controller_block_output_get_float("elevation_input_matrix", "out1", &el_tor))
log_send(LOG_T_ERROR, "Getting elevation_input_matrix.out1 failed\n");
if (controller_block_output_get_float("dt_az", "position", &az_pos))
log_send(LOG_T_ERROR, "Getting dt_az.position failed\n");
log_send(LOG_T_INFO, "Elevation: %e (Torsion: %e) Azimuth: %e",
el_pos, el_tor, az_pos);
int oks, errors, recoverables;
/* Log once every 15 minutes */
sleep(15 * 60);
controller_bus_poll_states(&oks, &errors, &recoverables);
if (recoverables) {
controller_bus_recover();
}
sleep(1);
}
return 0;
......
......@@ -21,7 +21,6 @@
#include <stdio.h>
#include "controller_block.h"
#include "controller_bus.h"
#include "esc.h"
#include "esc_esi.h"
#include "canopen.h"
......
......@@ -96,192 +96,148 @@ void esc_device_destroy(struct esc_device *dev)
free(dev);
}
static void *esc_device_poll(void *arg)
struct controller_bus_client_private {
struct esc_device dev;
};
static enum controller_bus_client_state esc_device_poll(struct controller_bus_client *client)
{
struct esc_device *dev = arg;
struct esc_device *dev = &client->private->dev;
enum controller_bus_client_state poll_state = CONTROLLER_BUS_CLIENT_STATE_OK;
char *str;
enum esc_al_status status;
enum esc_al_state state;
bool err_status = false;
bool err_state = false;
log_send(LOG_T_DEBUG, "Poll thread started for esc device %s",
dev->name);
while (1) {
char *str;
bool recover = false;
bool reinit = false;
int recover_state = ESC_AL_STATE_SAFE_OPERATIONAL;
sleep(1);
status = esc_al_status_code_get(dev);
state = esc_al_state_get(dev);
if (state == ESC_AL_STATE_ERROR) {
if (!err_state) {
log_send(LOG_T_ERROR,
"%s: Could not read ESC state",
dev->name);
}
err_state = true;
} else {
if (err_state) {
log_send(LOG_T_WARNING,
"%s: ESC state readable again",
dev->name);
}
err_state = false;
}
if (status < 0) {
if (!err_status) {
log_send(LOG_T_ERROR,
"%s: Could not read ESC status",
dev->name);
}
err_status = true;
} else {
if (err_status) {
log_send(LOG_T_WARNING,
"%s: ESC status readable again",
dev->name);
}
err_status = false;
status = esc_al_status_code_get(dev);
state = esc_al_state_get(dev);
if (state == ESC_AL_STATE_ERROR || status < 0) {
poll_state = CONTROLLER_BUS_CLIENT_STATE_ERROR;
} else {
if (state != dev->state) {
log_send(LOG_T_WARNING,
"%s: ESC state changed: %d %s -> %d %s",
dev->name,
dev->state, esc_al_state2str(dev->state),
state, esc_al_state2str(state));
dev->state = state;
}
if (!err_state) {
if (state != dev->state) {
if (status != dev->status) {
if (!status) {
log_send(LOG_T_WARNING,
"%s: ESC state changed: %d %s -> %d %s",
dev->name,
dev->state, esc_al_state2str(dev->state),
state, esc_al_state2str(state));
}
if (dev->auto_recover && state < dev->state) {
if (state == ESC_AL_STATE_INIT) {
recover_state =
ESC_AL_STATE_PRE_OPERATIONAL;
reinit = true;
} else if (state == ESC_AL_STATE_PRE_OPERATIONAL) {
recover_state =
ESC_AL_STATE_SAFE_OPERATIONAL;
recover = true;
} else {
recover = true;
recover_state = dev->state;
}
"%s: ESC status changed: No error", dev->name);
} else {
dev->state = state;
}
}
if (status == dev->status || err_status)
if (!recover && !reinit)
continue;
dev->status = status;
str = esc_al_status2str(status);
if (!status) {
log_send(LOG_T_WARNING,
"%s: ESC status changed: No error", dev->name);
if (!recover && !reinit)
continue;
} else {
switch(status) {
case ESC_AL_STATUS_SM_WATCHDOG:
str = "Sync manager watchdog";
recover = true;
recover_state = ESC_AL_STATE_OPERATIONAL;
break;
case ESC_AL_STATUS_NO_ERROR:
case ESC_AL_STATUS_UNSPECIFIED:
case ESC_AL_STATUS_INVALID_REQ:
case ESC_AL_STATUS_UNKNOWN_REQ:
case ESC_AL_STATUS_BOOTSTRAP_UNSUP:
case ESC_AL_STATUS_NO_VALID_FMW:
case ESC_AL_STATUS_INVALID_MBOX1:
case ESC_AL_STATUS_INVALID_MBOX2:
case ESC_AL_STATUS_INVALID_SM:
case ESC_AL_STATUS_NO_VALID_INPUT:
case ESC_AL_STATUS_NO_VALID_OUTPUT:
case ESC_AL_STATUS_SYNC_ERROR:
case ESC_AL_STATUS_INVALID_SM_TYPES:
case ESC_AL_STATUS_INVALID_OUT_CONF:
case ESC_AL_STATUS_INVALID_IN_CONF:
case ESC_AL_STATUS_INVALID_WD_CONF:
case ESC_AL_STATUS_SLAVE_COLD:
case ESC_AL_STATUS_SLAVE_INIT:
case ESC_AL_STATUS_SLAVE_PREOP:
case ESC_AL_STATUS_SLAVE_SAFEOP:
case ESC_AL_STATUS_INVALID_DC_S_CONF:
case ESC_AL_STATUS_INVALID_DC_L_CONF:
case ESC_AL_STATUS_PLL_ERROR:
case ESC_AL_STATUS_INVALID_DC_IO:
case ESC_AL_STATUS_INVALID_DC_TO:
case ESC_AL_STATUS_MBOX_EOE:
case ESC_AL_STATUS_MBOX_COE:
case ESC_AL_STATUS_MBOX_FOE:
case ESC_AL_STATUS_MBOX_SOE:
case ESC_AL_STATUS_MBOX_VOE:
default:
break;
poll_state = CONTROLLER_BUS_CLIENT_STATE_RECOVERABLE;
switch(status) {
case ESC_AL_STATUS_SM_WATCHDOG:
str = "Sync manager watchdog";
break;
case ESC_AL_STATUS_NO_ERROR:
case ESC_AL_STATUS_UNSPECIFIED:
case ESC_AL_STATUS_INVALID_REQ:
case ESC_AL_STATUS_UNKNOWN_REQ:
case ESC_AL_STATUS_BOOTSTRAP_UNSUP:
case ESC_AL_STATUS_NO_VALID_FMW:
case ESC_AL_STATUS_INVALID_MBOX1:
case ESC_AL_STATUS_INVALID_MBOX2:
case ESC_AL_STATUS_INVALID_SM:
case ESC_AL_STATUS_NO_VALID_INPUT:
case ESC_AL_STATUS_NO_VALID_OUTPUT:
case ESC_AL_STATUS_SYNC_ERROR:
case ESC_AL_STATUS_INVALID_SM_TYPES:
case ESC_AL_STATUS_INVALID_OUT_CONF:
case ESC_AL_STATUS_INVALID_IN_CONF:
case ESC_AL_STATUS_INVALID_WD_CONF:
case ESC_AL_STATUS_SLAVE_COLD:
case ESC_AL_STATUS_SLAVE_INIT:
case ESC_AL_STATUS_SLAVE_PREOP:
case ESC_AL_STATUS_SLAVE_SAFEOP:
case ESC_AL_STATUS_INVALID_DC_S_CONF:
case ESC_AL_STATUS_INVALID_DC_L_CONF:
case ESC_AL_STATUS_PLL_ERROR:
case ESC_AL_STATUS_INVALID_DC_IO:
case ESC_AL_STATUS_INVALID_DC_TO:
case ESC_AL_STATUS_MBOX_EOE:
case ESC_AL_STATUS_MBOX_COE:
case ESC_AL_STATUS_MBOX_FOE:
case ESC_AL_STATUS_MBOX_SOE:
case ESC_AL_STATUS_MBOX_VOE:
default:
break;
}
log_send(LOG_T_ERROR, "%s: ESC status code changed: 0x%04x: %s",
dev->name, status, str);
}
log_send(LOG_T_ERROR, "%s: ESC status code changed: 0x%04x: %s",
dev->name, status, str);
dev->status = status;
}
}
return poll_state;
}
if (reinit) {
log_send(LOG_T_WARNING,
"%s: Trying to re-initialize to state %d %s",
dev->name, recover_state,
esc_al_state2str(recover_state));
static int esc_device_recover(struct controller_bus_client *client)
{
struct esc_device *dev = &client->private->dev;
enum esc_al_state state;
struct timespec to = { 1, 0 };
int r;
state = esc_al_state_get(dev);
if (state == ESC_AL_STATE_ERROR) {
return -1;
}
int r = esc_device_initialize_pre_operational(dev);
if (state == ESC_AL_STATE_INIT) {
log_send(LOG_T_WARNING,
"%s: Trying to re-initialize to state %d %s",
dev->name, ESC_AL_STATE_PRE_OPERATIONAL,
esc_al_state2str(ESC_AL_STATE_PRE_OPERATIONAL));
r = esc_device_initialize_pre_operational(dev);
if (r) {
log_send(LOG_T_ERROR,
"%s: Failed to re-initialize device",
dev->name);
return r;
} else {
state = ESC_AL_STATE_PRE_OPERATIONAL;
}
}
if (state == ESC_AL_STATE_PRE_OPERATIONAL) {
log_send(LOG_T_WARNING,
"%s: Trying to recover to state %d %s",
dev->name, ESC_AL_STATE_SAFE_OPERATIONAL,
esc_al_state2str(ESC_AL_STATE_SAFE_OPERATIONAL));
if (dev->pre_operational_hook) {
r = dev->pre_operational_hook(dev);
if (r) {
log_send(LOG_T_ERROR,
"%s: Failed to re-initialize device",
dev->name);
}
}
if (recover) {
struct timespec to = { 1, 0 };
int r;
log_send(LOG_T_WARNING,
"%s: Trying to recover to state %d %s",
dev->name, recover_state,
esc_al_state2str(recover_state));
if (recover_state == ESC_AL_STATE_SAFE_OPERATIONAL &&
dev->pre_operational_hook) {
r = dev->pre_operational_hook(dev);
if (r) {
log_send(LOG_T_ERROR,
"%s: pre_operational_hook failed",
dev->name);
}
"%s: pre_operational_hook failed",
dev->name);
return r;
}
}
esc_al_error_ack(dev);
esc_al_state_set(dev, recover_state, &to);
esc_al_error_ack(dev);
state = esc_al_state_set(dev, ESC_AL_STATE_SAFE_OPERATIONAL, &to);
if (state != ESC_AL_STATE_SAFE_OPERATIONAL) {
return -1;
}
}
if (state == ESC_AL_STATE_SAFE_OPERATIONAL) {
log_send(LOG_T_WARNING,
"%s: Trying to recover to state %d %s",
dev->name, ESC_AL_STATE_OPERATIONAL,
esc_al_state2str(ESC_AL_STATE_OPERATIONAL));
esc_al_error_ack(dev);
state = esc_al_state_set(dev, ESC_AL_STATE_OPERATIONAL, &to);
if (state != ESC_AL_STATE_OPERATIONAL) {
return -1;
}
}
return NULL;
}
int esc_device_poll_start(struct esc_device *dev)
{
pthread_t thread_id;
pthread_attr_t attr;
pthread_attr_init(&attr);
pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN * 2);
pthread_create(&thread_id, &attr, esc_device_poll, dev);
return 0;
}
......@@ -307,8 +263,12 @@ int esc_device_initialize_pre_operational(struct esc_device *dev)
if (dev->block_tx) {
dev->client_tx = controller_bus_add_output_client(
dev->bus, dev->block_tx);
if (dev->client_tx)
if (dev->client_tx) {
dev->client_tx->callback = dev->callback_tx;
dev->client_tx->poll = esc_device_poll;
dev->client_tx->recover = esc_device_recover;
dev->client_tx->private = (struct controller_bus_client_private*)dev;
}
esc_fmmu_map_tx(dev,
dev->sm[dev->tx_pdo_sm].start,
dev->sm[dev->tx_pdo_sm].len);
......@@ -323,8 +283,12 @@ int esc_device_initialize_pre_operational(struct esc_device *dev)
if (dev->block_rx) {
dev->client_rx = controller_bus_add_input_client(
dev->bus, dev->block_rx);
if (dev->client_rx)
if (dev->client_rx) {
dev->client_rx->callback = dev->callback_rx;
dev->client_rx->poll = esc_device_poll;
dev->client_rx->recover = esc_device_recover;
dev->client_rx->private = (struct controller_bus_client_private*)dev;
}
esc_fmmu_map_rx(dev,
dev->sm[dev->rx_pdo_sm].start,
dev->sm[dev->rx_pdo_sm].len);
......@@ -348,13 +312,6 @@ int esc_device_initialize_pre_operational(struct esc_device *dev)
esc_dc_init(&dev->addr, &dev->dc_sync);
esc_watchdog_init(dev);
if (!dev->poll_active) {
r = esc_device_poll_start(dev);
dev->poll_active = true;
}
dev->auto_recover = true;
dev->state = ESC_AL_STATE_PRE_OPERATIONAL;
return r;
......
......@@ -58,9 +58,6 @@ struct esc_device {
enum esc_al_state state;
enum esc_al_status status;
bool auto_recover;
bool poll_active;
struct controller_block *block; /* block this device 'belongs' to */
struct controller_block *block_tx; /* block producing tx pdo data */
struct controller_block *block_rx; /* block consuming rx pdo data */
......
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