diff --git a/Src/dronecan_application/application.cpp b/Src/dronecan_application/application.cpp index ac6a33d..6aef601 100644 --- a/Src/dronecan_application/application.cpp +++ b/Src/dronecan_application/application.cpp @@ -29,19 +29,34 @@ __attribute__((noreturn)) void application_entry_point() { uavcanInitApplication(node_id); - CircuitStatusModule& status_module = CircuitStatusModule::get_instance(); - PWMModule& pwm_module = PWMModule::get_instance(); - LedColor color = LedColor::BLUE_COLOR; - if (!status_module.instance_initialized) { - color = LedColor::RED_COLOR; - } + CircuitStatus circuit_status; + PWMModule pwm_module; + + std::array modules = { &circuit_status, &pwm_module }; + for (auto module : modules) { + module->init(); + } while(true) { + auto health = Module::Status::OK; + auto mode = Module::Mode::OPEARTIONAL; + for (auto module : modules) { + module->process(); + + if (module->get_health() > health) { + health = module->get_health(); + } + + if (module->get_mode() > mode) { + mode = module->get_mode(); + } + } + + auto color = (health == Module::Status::OK) ? LedColor::BLUE_COLOR : LedColor::RED_COLOR; LedPeriphery::toggle(color); - status_module.spin_once(); - pwm_module.spin_once(); - uavcanSetNodeHealth((NodeStatusHealth_t) pwm_module.module_status); + uavcanSetNodeHealth((NodeStatusHealth_t)(health)); + uavcanSetNodeStatusMode((NodeStatusMode_t)(mode)); uavcanSpinOnce(); WatchdogPeriphery::refresh(); diff --git a/Src/dronecan_application/modules/circuit_status/CircuitStatusModule.cpp b/Src/dronecan_application/modules/circuit_status/CircuitStatusModule.cpp index 1a8ad27..e08f09b 100644 --- a/Src/dronecan_application/modules/circuit_status/CircuitStatusModule.cpp +++ b/Src/dronecan_application/modules/circuit_status/CircuitStatusModule.cpp @@ -6,26 +6,14 @@ #include "CircuitStatusModule.hpp" -CircuitStatusModule CircuitStatusModule::instance = CircuitStatusModule(); -bool CircuitStatusModule::instance_initialized = false; -Logger CircuitStatusModule::logger = Logger("CircuitStatus"); - -CircuitStatusModule& CircuitStatusModule::get_instance() { - if (!instance_initialized) { - if (instance.init() != 0) { - logger.log_debug("ADC init error"); - } else { - instance_initialized = true; - } - } - return instance; -} +Logger CircuitStatus::logger = Logger("CircuitStatus"); -int8_t CircuitStatusModule::init() { - return CircuitPeriphery::init(); +void CircuitStatus::init() { + CircuitPeriphery::init(); + mode = Module::Mode::OPEARTIONAL; } -void CircuitStatusModule::spin_once() { +void CircuitStatus::spin_once() { static uint32_t next_temp_pub_ms = 1000; static uint32_t next_status_pub_ms = 1000; @@ -52,5 +40,9 @@ void CircuitStatusModule::spin_once() { next_status_pub_ms += 1000; } + if (circuit_status.error_flags != ERROR_FLAG_CLEAR) { + health = Status::MINOR_FAILURE; + } + circuit_status.error_flags = ERROR_FLAG_CLEAR; } diff --git a/Src/dronecan_application/modules/circuit_status/CircuitStatusModule.hpp b/Src/dronecan_application/modules/circuit_status/CircuitStatusModule.hpp index f453603..f9be466 100644 --- a/Src/dronecan_application/modules/circuit_status/CircuitStatusModule.hpp +++ b/Src/dronecan_application/modules/circuit_status/CircuitStatusModule.hpp @@ -10,31 +10,25 @@ #include "uavcan/equipment/temperature/Temperature.h" #include "periphery/adc/circuit_periphery.hpp" #include "uavcan/equipment/power/CircuitStatus.h" +#include "common/module.hpp" #include "logger.hpp" - -class CircuitStatusModule { +class CircuitStatus : public Module { public: - static CircuitStatusModule &get_instance(); - void spin_once(); + CircuitStatus() : Module(2) {} + void init() override; - static bool instance_initialized; -private: - static CircuitStatusModule instance; +protected: + void spin_once() override; +private: CircuitStatus_t circuit_status = {}; Temperature_t temperature_status = {}; static Logger logger; - CircuitStatusModule() = default; - uint8_t circuit_status_transfer_id = 0; uint8_t temperature_transfer_id = 0; - - int8_t init(); - CircuitStatusModule(const CircuitStatusModule &other) = delete; - void operator=(const CircuitStatusModule &) = delete; }; #endif // SRC_MODULES_CIRCUIT_STATUS_CIRCUIT_STATUS_HPP_ diff --git a/Src/dronecan_application/modules/pwm/PWMModule.cpp b/Src/dronecan_application/modules/pwm/PWMModule.cpp index 4abc946..2fb2eaf 100644 --- a/Src/dronecan_application/modules/pwm/PWMModule.cpp +++ b/Src/dronecan_application/modules/pwm/PWMModule.cpp @@ -30,14 +30,6 @@ uint16_t PWMModule::ttl_cmd = 500; uint16_t PWMModule::pwm_freq = 50; CommandType PWMModule::pwm_cmd_type = CommandType::RAW_COMMAND; -ModuleStatus PWMModule::module_status = ModuleStatus::MODULE_OK; - -PWMModule PWMModule::instance = PWMModule(); - -PWMModule::PWMModule() { - update_params(); - init(); -} std::array(PwmPin::PWM_AMOUNT)> PWMModule::params = {{ {{.min = MIN(1), .max = MAX(1), .def = DEF(1), .ch = CHANNEL(1), .fb = FB(1)}, PwmPin::PWM_1}, @@ -46,16 +38,11 @@ std::array(PwmPin::PWM_AMOUNT)> PWMModule:: {{.min = MIN(4), .max = MAX(4), .def = DEF(4), .ch = CHANNEL(4), .fb = FB(4)}, PwmPin::PWM_4}, }}; -PWMModule& PWMModule::get_instance() { - static bool instance_initialized = false; - if (!instance_initialized) { - instance_initialized = true; - instance.init(); - } - return instance; -} - void PWMModule::init() { + mode = Module::Mode::INITIALIZATION; + + update_params(); + logger.init("PWMModule"); for (auto param : params) { PwmPeriphery::init(param.pin); @@ -69,13 +56,7 @@ void PWMModule::init() { void PWMModule::spin_once() { uint32_t crnt_time_ms = HAL_GetTick(); - static uint32_t next_update_ms = 0; - if (crnt_time_ms > next_update_ms) { - next_update_ms = crnt_time_ms + 1000; - instance.update_params(); - instance.apply_params(); - } - + mode = Module::Mode::OPEARTIONAL; for (auto& pwm : params) { if (crnt_time_ms > pwm.cmd_end_time_ms) { pwm.command_val = pwm.def; @@ -86,15 +67,15 @@ void PWMModule::spin_once() { status_pub_timeout_ms = 1; static uint32_t next_pub_ms = 5000; - if (module_status == ModuleStatus::MODULE_OK && crnt_time_ms > next_pub_ms) { + if (health == Status::OK && crnt_time_ms > next_pub_ms) { publish_state(); next_pub_ms = crnt_time_ms + status_pub_timeout_ms; } } void PWMModule::update_params() { - module_status = ModuleStatus::MODULE_OK; - + health = Status::OK; + mode = Mode::MAINTENANCE; pwm_freq = paramsGetIntegerValue(IntParamsIndexes::PARAM_PWM_FREQUENCY); pwm_cmd_type = (CommandType)paramsGetIntegerValue(IntParamsIndexes::PARAM_PWM_CMD_TYPE); @@ -117,7 +98,9 @@ void PWMModule::update_params() { params_error = true; break; } - + if (health != Status::OK&& HAL_GetTick() %1000 == 0) { + logger.log_info("102"); + } static uint32_t last_warn_pub_time_ms = 0; for (int i = 0; i < static_cast(PwmPin::PWM_AMOUNT); i++) { params[i].fb = paramsGetIntegerValue(params[i].names.fb); @@ -135,12 +118,17 @@ void PWMModule::update_params() { } if (params_error) { - module_status = ModuleStatus::MODULE_WARN; + health = Status::MINOR_FAILURE; if (last_warn_pub_time_ms < HAL_GetTick()) { last_warn_pub_time_ms = HAL_GetTick() + 10000; logger.log_debug("check parameters"); } } + if (health != Status::OK && HAL_GetTick() %1000 == 0) { + logger.log_info("128"); + } + apply_params(); + mode = Mode::OPEARTIONAL; } void PWMModule::apply_params() { @@ -232,7 +220,7 @@ void PWMModule::publish_hardpoint_status() { } void PWMModule::raw_command_callback(CanardRxTransfer* transfer) { - if (module_status != ModuleStatus::MODULE_OK || pwm_cmd_type != CommandType::RAW_COMMAND) { + if (pwm_cmd_type != CommandType::RAW_COMMAND) { return; } @@ -248,7 +236,7 @@ void PWMModule::raw_command_callback(CanardRxTransfer* transfer) { } void PWMModule::array_command_callback(CanardRxTransfer* transfer) { - if (module_status != ModuleStatus::MODULE_OK || pwm_cmd_type != CommandType::ARRAY_COMMAND) { + if (pwm_cmd_type != CommandType::ARRAY_COMMAND) { return; } @@ -274,8 +262,7 @@ void PWMModule::array_command_callback(CanardRxTransfer* transfer) { } void PWMModule::hardpoint_callback(CanardRxTransfer* transfer) { - if (module_status != ModuleStatus::MODULE_OK || - pwm_cmd_type != CommandType::HARDPOINT_COMMAND) { + if (pwm_cmd_type != CommandType::HARDPOINT_COMMAND) { return; } diff --git a/Src/dronecan_application/modules/pwm/PWMModule.hpp b/Src/dronecan_application/modules/pwm/PWMModule.hpp index 3a330c5..17f33ed 100644 --- a/Src/dronecan_application/modules/pwm/PWMModule.hpp +++ b/Src/dronecan_application/modules/pwm/PWMModule.hpp @@ -14,15 +14,9 @@ #include "params.hpp" #include "logger.hpp" #include "periphery/pwm/pwm.hpp" +#include "common/module.hpp" -enum class ModuleStatus: uint8_t { - MODULE_OK = 0, // The module is functioning properly - MODULE_WARN = 1, // The module encountered a minor failure - MODULE_ERROR = 2, // The module encountered a major failure - MODULE_CRITICAL = 3, // The module suffered a fatal malfunction -}; - enum class CommandType: uint8_t { RAW_COMMAND, ARRAY_COMMAND, @@ -52,23 +46,22 @@ struct PwmChannelInfo { uint8_t fb{0}; }; -class PWMModule { +class PWMModule : public Module { public: - void spin_once(); - static PWMModule &get_instance(); - static std::array(PwmPin::PWM_AMOUNT)> params; - static ModuleStatus module_status; + void init() override; + + inline PWMModule() : Module(50) {} protected: - PWMModule(); + void update_params() override; + void spin_once() override; + + static std::array(PwmPin::PWM_AMOUNT)> params; private: - static PWMModule instance; - void (*callback)(CanardRxTransfer*); - void (*publish_state)(); + void (*callback)(CanardRxTransfer*) = {}; + void (*publish_state)() = {}; - void init(); - void update_params(); void update_pwm(); void apply_params(); @@ -86,14 +79,11 @@ class PWMModule { static CommandType pwm_cmd_type; static uint16_t ttl_cmd; - uint16_t status_pub_timeout_ms; - bool verbose; + uint16_t status_pub_timeout_ms = 0; + bool verbose = false; static bool publish_error; static Logger logger; - - PWMModule& operator = (const PWMModule&) = delete; - explicit PWMModule(PWMModule *other) = delete; }; #endif // SRC_MODULES_PWM_PWMMODULE_HPP_