From 6b507fbe4db9406752752e6e54de6987f88ecf00 Mon Sep 17 00:00:00 2001
From: PonomarevDA <ponomarevda96@gmail.com>
Date: Thu, 30 May 2024 23:41:09 +0300
Subject: [PATCH] set cyphal node health and node

---
 Src/cyphal_application/application.cpp | 33 ++++++++++++++++++--------
 1 file changed, 23 insertions(+), 10 deletions(-)

diff --git a/Src/cyphal_application/application.cpp b/Src/cyphal_application/application.cpp
index 2fc421b..c4dc216 100644
--- a/Src/cyphal_application/application.cpp
+++ b/Src/cyphal_application/application.cpp
@@ -29,26 +29,39 @@ __attribute__((noreturn)) void application_entry_point() {
     cyphal::NodeGetInfoSubscriber::setHardwareVersion(2, 1);
 
     cyphal::Cyphal cyphal;
-    int init_res = cyphal.init();
+    cyphal.init();
 
     SetpointModule setpoint;
-    setpoint.init();
-
     FeedbackModule feedback;
-    feedback.init();
-
     CircuitStatus crct;
-    crct.init();
+
+    std::array<BaseModule*, 3> modules = { &setpoint, &feedback, &crct };
+
+    for (auto module : modules) {
+        module->init();
+    }
 
     while (true) {
-        auto led_color = (init_res >= 0) ? LedColor::BLUE_COLOR : LedColor::RED_COLOR;
-        LedPeriphery::toggle(led_color);
+        auto health = ModuleStatus::OK;
+        auto mode = ModuleMode::OPEARTIONAL;
 
         cyphal.process();
+        for (auto module : modules) {
+            module->process();
 
-        feedback.process();
-        crct.process();
+            if (module->get_health() > health) {
+                health = module->get_health();
+            }
 
+            if (module->get_mode() > mode) {
+                mode = module->get_mode();
+            }
+        }
+
+        auto led_color = (health == ModuleStatus::OK) ? LedColor::BLUE_COLOR : LedColor::RED_COLOR;
+        cyphal.setNodeHealth(uavcan_node_Health_1_0{static_cast<uint8_t>(health)});
+        cyphal.setNodeMode(uavcan_node_Mode_1_0{static_cast<uint8_t>(mode)});
+        LedPeriphery::toggle(led_color);
         WatchdogPeriphery::refresh();
     }
 }