diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 948b08bd77c..bdc43cd8790 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -95,6 +95,7 @@ set(msg_files gimbal_manager_status.msg gps_dump.msg gps_inject_data.msg + health_report.msg heater_status.msg home_position.msg hover_thrust_estimate.msg diff --git a/msg/health_report.msg b/msg/health_report.msg new file mode 100644 index 00000000000..5d38e6db2ac --- /dev/null +++ b/msg/health_report.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) + +uint64 can_arm_mode_flags # bitfield for each flight mode (NAVIGATION_STATE_*) if arming is possible +uint64 can_run_mode_flags # bitfield for each flight mode if it can run + +uint64 health_is_present_flags # flags for each health_component_t +uint64 health_warning_flags +uint64 health_error_flags +# A component is required but missing, if present==0 and error==1 + +uint64 arming_check_warning_flags +uint64 arming_check_error_flags + diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index ed14d6925b4..74e607120c0 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -33,6 +33,31 @@ #include "Common.hpp" +void Report::getHealthReport(health_report_s &report) const +{ + const Results ¤t_results = _results[_current_result]; + report.can_arm_mode_flags = 0; + report.can_run_mode_flags = 0; + + for (int i = 0; i < vehicle_status_s::NAVIGATION_STATE_MAX; ++i) { + NavModes group = getModeGroup(i); + + if ((uint32_t)(current_results.arming_checks.can_arm & group)) { + report.can_arm_mode_flags |= 1ull << i; + } + + if ((uint32_t)(current_results.arming_checks.can_run & group)) { + report.can_run_mode_flags |= 1ull << i; + } + } + + report.arming_check_error_flags = (uint64_t)current_results.arming_checks.error; + report.arming_check_warning_flags = (uint64_t)current_results.arming_checks.warning; + report.health_is_present_flags = (uint64_t)current_results.health.is_present; + report.health_error_flags = (uint64_t)current_results.health.error; + report.health_warning_flags = (uint64_t)current_results.health.warning; +} + void Report::healthFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id, const events::LogLevels &log_levels, const char *message) { diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 290898619b6..3a73ea554b6 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -207,6 +208,8 @@ public: (uint32_t)(_results[_current_result].arming_checks.can_run & getModeGroup(nav_state)) != 0; } + void getHealthReport(health_report_s &report) const; + /** * Report a health failure. A health issue generally refers to a hardware issue, independent from environment * or e.g. calibration. diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index a4a8e25806f..66c3a098a54 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -57,6 +57,10 @@ bool HealthAndArmingChecks::update(bool force_reporting) if (_reporter.report(_context.isArmed(), force_reporting)) { + health_report_s health_report; + _reporter.getHealthReport(health_report); + health_report.timestamp = hrt_absolute_time(); + _health_report_pub.publish(health_report); return true; } diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index 871822faddb..0493a7f5769 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -36,6 +36,8 @@ #include "Common.hpp" #include +#include +#include class HealthAndArmingChecks : public ModuleParams { @@ -62,6 +64,8 @@ private: Context _context; Report _reporter; + uORB::Publication _health_report_pub{ORB_ID(health_report)}; + // all checks HealthAndArmingCheckBase *_checks[10] = { };