From ac4b688384a42fc24afe7d211957a7274905f421 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 27 Nov 2017 04:31:02 -0800 Subject: [PATCH 1/2] add faults module --- examples/basic_uavcan_functionality/Makefile | 4 +- modules/faults/faults.c | 58 +++++++++++++++++++ modules/faults/faults.h | 33 +++++++++++ modules/uavcan_fault_reporter/module.mk | 1 + .../uavcan_fault_reporter.c | 51 ++++++++++++++++ .../uavcan_nodestatus_publisher.c | 9 ++- 6 files changed, 154 insertions(+), 2 deletions(-) create mode 100644 modules/faults/faults.c create mode 100644 modules/faults/faults.h create mode 100644 modules/uavcan_fault_reporter/module.mk create mode 100644 modules/uavcan_fault_reporter/uavcan_fault_reporter.c diff --git a/examples/basic_uavcan_functionality/Makefile b/examples/basic_uavcan_functionality/Makefile index 97a447cc..f664e0f9 100755 --- a/examples/basic_uavcan_functionality/Makefile +++ b/examples/basic_uavcan_functionality/Makefile @@ -23,7 +23,9 @@ uavcan_beginfirmwareupdate_server \ uavcan_debug \ uavcan_param_interface \ uavcan_allocatee \ -uavcan_restart +uavcan_restart \ +faults \ +uavcan_fault_reporter MESSAGES_ENABLED = \ uavcan.protocol.debug.LogMessage diff --git a/modules/faults/faults.c b/modules/faults/faults.c new file mode 100644 index 00000000..8aba8374 --- /dev/null +++ b/modules/faults/faults.c @@ -0,0 +1,58 @@ +#include "faults.h" +#include + +static struct fault_s* faults_head; + +void fault_register(struct fault_s* fault, enum fault_level_t level, const char* description) { + fault->description = description; + fault->level = level; + fault->timeout = TIME_IMMEDIATE; + fault->next = NULL; + + chSysLock(); + LINKED_LIST_APPEND(struct fault_s, faults_head, fault); + chSysUnlock(); +} + +void fault_raise(struct fault_s* fault, systime_t timeout) { + if (!fault) { + return; + } + + fault->raised_time = chVTGetSystemTimeX(); + fault->timeout = timeout; +} + +void fault_clear(struct fault_s* fault) { + if (!fault) { + return; + } + + fault->timeout = TIME_IMMEDIATE; +} + +enum fault_level_t fault_get_level(struct fault_s* fault) { + if (!fault) { + return FAULT_LEVEL_OK; + } + + systime_t systime_now = chVTGetSystemTimeX(); + if (fault->timeout != TIME_INFINITE && (fault->timeout == TIME_IMMEDIATE || systime_now - fault->raised_time > fault->timeout)) { + fault->timeout = TIME_IMMEDIATE; + return FAULT_LEVEL_OK; + } + + return fault->level; +} + +struct fault_s* fault_get_worst_fault(void) { + struct fault_s* ret = NULL; + + for (struct fault_s* fault = faults_head; fault != NULL; fault = fault->next) { + if (fault_get_level(fault) > fault_get_level(ret)) { + ret = fault; + } + } + + return ret; +} diff --git a/modules/faults/faults.h b/modules/faults/faults.h new file mode 100644 index 00000000..c10e17d1 --- /dev/null +++ b/modules/faults/faults.h @@ -0,0 +1,33 @@ +#pragma once + +#include + +#define FAULT_DEFINE_STATIC(HANDLE_NAME, LEVEL, DESCRIPTION) \ +static struct fault_s HANDLE_NAME; \ +RUN_AFTER(CH_SYS_INIT) { \ + fault_register(&HANDLE_NAME, LEVEL, DESCRIPTION); \ +} + +enum fault_level_t { + FAULT_LEVEL_OK, + FAULT_LEVEL_WARNING, + FAULT_LEVEL_ERROR, + FAULT_LEVEL_CRITICAL +}; + +struct fault_s { + const char* description; + enum fault_level_t level; + systime_t raised_time; + systime_t timeout; + struct fault_s* next; +}; + +void fault_register(struct fault_s* fault, enum fault_level_t level, const char* description); + +void fault_raise(struct fault_s* fault, systime_t timeout); +void fault_clear(struct fault_s* fault); + +enum fault_level_t fault_get_level(struct fault_s* fault); + +struct fault_s* fault_get_worst_fault(void); diff --git a/modules/uavcan_fault_reporter/module.mk b/modules/uavcan_fault_reporter/module.mk new file mode 100644 index 00000000..a795df45 --- /dev/null +++ b/modules/uavcan_fault_reporter/module.mk @@ -0,0 +1 @@ +MESSAGES_ENABLED += uavcan.protocol.debug.LogMessage diff --git a/modules/uavcan_fault_reporter/uavcan_fault_reporter.c b/modules/uavcan_fault_reporter/uavcan_fault_reporter.c new file mode 100644 index 00000000..7073e608 --- /dev/null +++ b/modules/uavcan_fault_reporter/uavcan_fault_reporter.c @@ -0,0 +1,51 @@ +#include +#include +#include +#include +#include +#include + +static struct worker_thread_timer_task_s log_msg_publisher_task; + +static void log_msg_publisher_task_func(struct worker_thread_timer_task_s* task); + +RUN_AFTER(UAVCAN_INIT) { + worker_thread_add_timer_task(&lpwork_thread, &log_msg_publisher_task, log_msg_publisher_task_func, NULL, S2ST(3), true); +} + +static void log_msg_publisher_task_func(struct worker_thread_timer_task_s* task) { + (void)task; + + struct fault_s* worst_fault = fault_get_worst_fault(); + + if (worst_fault) { + struct uavcan_protocol_debug_LogMessage_s log_msg; + memset(&log_msg, 0, sizeof(log_msg)); + + switch(fault_get_level(worst_fault)) { + case FAULT_LEVEL_WARNING: + log_msg.level.value = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_WARNING; + break; + case FAULT_LEVEL_ERROR: + log_msg.level.value = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_ERROR; + break; + case FAULT_LEVEL_CRITICAL: + log_msg.level.value = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_ERROR; + break; + default: + break; + } + + const char* source = "faults"; + + log_msg.source_len = strnlen(source, sizeof(log_msg.source)); + memcpy(log_msg.source, source, log_msg.source_len); + + if (worst_fault->description) { + log_msg.text_len = strnlen(worst_fault->description, sizeof(log_msg.text)); + memcpy(log_msg.text, worst_fault->description, log_msg.text_len); + } + + uavcan_broadcast(0, &uavcan_protocol_debug_LogMessage_descriptor, CANARD_TRANSFER_PRIORITY_LOW, &log_msg); + } +} diff --git a/modules/uavcan_nodestatus_publisher/uavcan_nodestatus_publisher.c b/modules/uavcan_nodestatus_publisher/uavcan_nodestatus_publisher.c index 82890dee..923f63ba 100644 --- a/modules/uavcan_nodestatus_publisher/uavcan_nodestatus_publisher.c +++ b/modules/uavcan_nodestatus_publisher/uavcan_nodestatus_publisher.c @@ -3,6 +3,10 @@ #include #include +#ifdef MODULE_FAULTS_ENABLED +#include +#endif + static struct uavcan_protocol_NodeStatus_s node_status; static struct worker_thread_timer_task_s node_status_publisher_task; @@ -26,7 +30,10 @@ RUN_AFTER(UAVCAN_INIT) { static void node_status_publisher_task_func(struct worker_thread_timer_task_s* task) { (void)task; - + +#ifdef MODULE_FAULTS_ENABLED + node_status.health = fault_get_level(fault_get_worst_fault()); +#endif node_status.uptime_sec++; uavcan_broadcast(0, &uavcan_protocol_NodeStatus_descriptor, CANARD_TRANSFER_PRIORITY_LOW, &node_status); } From 850ddb29e7cf2bdb48f733797e8a29e6fca994d8 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 27 Nov 2017 05:56:29 -0800 Subject: [PATCH 2/2] faults: publish topic when fault raised --- modules/faults/faults.c | 17 ++++- modules/faults/faults.h | 4 + .../uavcan_fault_reporter.c | 74 ++++++++++++------- 3 files changed, 66 insertions(+), 29 deletions(-) diff --git a/modules/faults/faults.c b/modules/faults/faults.c index 8aba8374..afd2674a 100644 --- a/modules/faults/faults.c +++ b/modules/faults/faults.c @@ -1,6 +1,11 @@ #include "faults.h" #include +#ifdef MODULE_PUBSUB_ENABLED +#include +struct pubsub_topic_s fault_raised_topic; +#endif + static struct fault_s* faults_head; void fault_register(struct fault_s* fault, enum fault_level_t level, const char* description) { @@ -18,9 +23,19 @@ void fault_raise(struct fault_s* fault, systime_t timeout) { if (!fault) { return; } - + +#ifdef MODULE_PUBSUB_ENABLED + bool edge = (fault_get_level(fault) == FAULT_LEVEL_OK); +#endif + fault->raised_time = chVTGetSystemTimeX(); fault->timeout = timeout; + +#ifdef MODULE_PUBSUB_ENABLED + if (edge) { + pubsub_publish_message(&fault_raised_topic, sizeof(struct fault_s*), pubsub_copy_writer_func, &fault); + } +#endif } void fault_clear(struct fault_s* fault) { diff --git a/modules/faults/faults.h b/modules/faults/faults.h index c10e17d1..cf9c369e 100644 --- a/modules/faults/faults.h +++ b/modules/faults/faults.h @@ -8,6 +8,10 @@ RUN_AFTER(CH_SYS_INIT) { \ fault_register(&HANDLE_NAME, LEVEL, DESCRIPTION); \ } +#ifdef MODULE_PUBSUB_ENABLED +extern struct pubsub_topic_s fault_raised_topic; +#endif + enum fault_level_t { FAULT_LEVEL_OK, FAULT_LEVEL_WARNING, diff --git a/modules/uavcan_fault_reporter/uavcan_fault_reporter.c b/modules/uavcan_fault_reporter/uavcan_fault_reporter.c index 7073e608..f2f9697b 100644 --- a/modules/uavcan_fault_reporter/uavcan_fault_reporter.c +++ b/modules/uavcan_fault_reporter/uavcan_fault_reporter.c @@ -6,11 +6,25 @@ #include static struct worker_thread_timer_task_s log_msg_publisher_task; +static struct pubsub_listener_s fault_raised_listener; +static struct worker_thread_listener_task_s fault_raised_listener_task; +static void fault_raised_handler(size_t msg_size, const void* buf, void* ctx); static void log_msg_publisher_task_func(struct worker_thread_timer_task_s* task); +static void publish_fault(struct fault_s* fault); RUN_AFTER(UAVCAN_INIT) { worker_thread_add_timer_task(&lpwork_thread, &log_msg_publisher_task, log_msg_publisher_task_func, NULL, S2ST(3), true); + + pubsub_init_and_register_listener(&fault_raised_topic, &fault_raised_listener, fault_raised_handler, NULL); + + worker_thread_add_listener_task(&lpwork_thread, &fault_raised_listener_task, &fault_raised_listener); +} + +static void fault_raised_handler(size_t msg_size, const void* buf, void* ctx) { + (void)ctx; + (void)msg_size; + publish_fault(*(struct fault_s**)buf); } static void log_msg_publisher_task_func(struct worker_thread_timer_task_s* task) { @@ -19,33 +33,37 @@ static void log_msg_publisher_task_func(struct worker_thread_timer_task_s* task) struct fault_s* worst_fault = fault_get_worst_fault(); if (worst_fault) { - struct uavcan_protocol_debug_LogMessage_s log_msg; - memset(&log_msg, 0, sizeof(log_msg)); - - switch(fault_get_level(worst_fault)) { - case FAULT_LEVEL_WARNING: - log_msg.level.value = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_WARNING; - break; - case FAULT_LEVEL_ERROR: - log_msg.level.value = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_ERROR; - break; - case FAULT_LEVEL_CRITICAL: - log_msg.level.value = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_ERROR; - break; - default: - break; - } - - const char* source = "faults"; - - log_msg.source_len = strnlen(source, sizeof(log_msg.source)); - memcpy(log_msg.source, source, log_msg.source_len); - - if (worst_fault->description) { - log_msg.text_len = strnlen(worst_fault->description, sizeof(log_msg.text)); - memcpy(log_msg.text, worst_fault->description, log_msg.text_len); - } - - uavcan_broadcast(0, &uavcan_protocol_debug_LogMessage_descriptor, CANARD_TRANSFER_PRIORITY_LOW, &log_msg); + publish_fault(worst_fault); } } + +static void publish_fault(struct fault_s* fault) { + struct uavcan_protocol_debug_LogMessage_s log_msg; + memset(&log_msg, 0, sizeof(log_msg)); + + switch(fault_get_level(fault)) { + case FAULT_LEVEL_WARNING: + log_msg.level.value = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_WARNING; + break; + case FAULT_LEVEL_ERROR: + log_msg.level.value = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_ERROR; + break; + case FAULT_LEVEL_CRITICAL: + log_msg.level.value = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_ERROR; + break; + default: + break; + } + + const char* source = "faults"; + + log_msg.source_len = strnlen(source, sizeof(log_msg.source)); + memcpy(log_msg.source, source, log_msg.source_len); + + if (fault->description) { + log_msg.text_len = strnlen(fault->description, sizeof(log_msg.text)); + memcpy(log_msg.text, fault->description, log_msg.text_len); + } + + uavcan_broadcast(0, &uavcan_protocol_debug_LogMessage_descriptor, CANARD_TRANSFER_PRIORITY_LOW, &log_msg); +}