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 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(health)}); + cyphal.setNodeMode(uavcan_node_Mode_1_0{static_cast(mode)}); + LedPeriphery::toggle(led_color); WatchdogPeriphery::refresh(); } }