diff --git a/msg/NavigatorStatus.msg b/msg/NavigatorStatus.msg index 21517d61e7bb..da666533574f 100644 --- a/msg/NavigatorStatus.msg +++ b/msg/NavigatorStatus.msg @@ -3,4 +3,7 @@ uint64 timestamp # time since system start (microseconds) uint8 nav_state # Source mode (values in VehicleStatus) -bool failure # True when the current mode cannot continue +uint8 failure # Navigator failure enum + +uint8 FAILURE_NONE = 0 +uint8 FAILURE_HAGL = 1 # Target altitude exceeds maximum height above ground diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp index 8c7d0e314af4..2a0245bb1fbd 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp @@ -42,6 +42,17 @@ void NavigatorChecks::checkAndReport(const Context &context, Report &reporter) } if (context.status().nav_state == status.nav_state) { - reporter.failsafeFlags().navigator_failure = status.failure; + + reporter.failsafeFlags().navigator_failure = (status.failure != navigator_status_s::FAILURE_NONE); + + if (status.failure == navigator_status_s::FAILURE_HAGL) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, + health_component_t::system, + events::ID("check_navigator_failure_hagl"), + events::Log::Error, + "Waypoint above maximum height"); + } } } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 70068a9c515b..ec72881fc527 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -288,7 +288,7 @@ class Navigator : public ModuleBase, public ModuleParams void sendWarningDescentStoppedDueToTerrain(); - void trigger_failsafe(uint8_t nav_state); + void trigger_hagl_failsafe(uint8_t nav_state); private: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a00ca0e79c49..15db1abdc44d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1358,10 +1358,10 @@ void Navigator::set_mission_failure_heading_timeout() } } -void Navigator::trigger_failsafe(const uint8_t nav_state) +void Navigator::trigger_hagl_failsafe(const uint8_t nav_state) { - if (!_navigator_status.failure || _navigator_status.nav_state != nav_state) { - _navigator_status.failure = true; + if ((_navigator_status.failure != navigator_status_s::FAILURE_HAGL) || _navigator_status.nav_state != nav_state) { + _navigator_status.failure = navigator_status_s::FAILURE_HAGL; _navigator_status.nav_state = nav_state; _navigator_status_updated = true; @@ -1378,7 +1378,7 @@ void Navigator::publish_navigator_status() if (_navigator_status.nav_state != current_nav_state) { _navigator_status.nav_state = current_nav_state; - _navigator_status.failure = false; + _navigator_status.failure = navigator_status_s::FAILURE_NONE; _navigator_status_updated = true; } diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 9be8ce24a937..f26df56530d2 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -164,7 +164,7 @@ void RtlDirect::set_rtl_item() mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL\t"); events::send(events::ID("rtl_fail_max_hagl"), events::Log::Error, "RTL: return alt higher than max HAGL"); - _navigator->trigger_failsafe(getNavigatorStateId()); + _navigator->trigger_hagl_failsafe(getNavigatorStateId()); _rtl_state = RTLState::IDLE; }