add faults, fault reporting

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

modules/faults/faults.c

@@ -0,0 +1,73 @@
+#include "faults.h"
+#include <common/helpers.h>
+
+#ifdef MODULE_PUBSUB_ENABLED
+#include <modules/pubsub/pubsub.h>
+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) {
+    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;
+    }
+
+#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) {
+    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;
+}

modules/faults/faults.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <ch.h>
+
+#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); \
+}
+
+#ifdef MODULE_PUBSUB_ENABLED
+extern struct pubsub_topic_s fault_raised_topic;
+#endif
+
+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);

modules/uavcan_fault_reporter/module.mk

@@ -0,0 +1 @@
+MESSAGES_ENABLED += uavcan.protocol.debug.LogMessage

modules/uavcan_fault_reporter/uavcan_fault_reporter.c

@@ -0,0 +1,69 @@
+#include <modules/uavcan/uavcan.h>
+#include <common/ctor.h>
+#include <modules/lpwork_thread/lpwork_thread.h>
+#include <uavcan.protocol.debug.LogMessage.h>
+#include <modules/faults/faults.h>
+#include <string.h>
+
+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) {
+    (void)task;
+
+    struct fault_s* worst_fault = fault_get_worst_fault();
+
+    if (worst_fault) {
+        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);
+}

modules/uavcan_nodestatus_publisher/uavcan_nodestatus_publisher.c

@@ -3,6 +3,10 @@
 #include <common/ctor.h>
 #include <modules/lpwork_thread/lpwork_thread.h>
 
+#ifdef MODULE_FAULTS_ENABLED
+#include <modules/faults/faults.h>
+#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);
 }