navigator: add failure enum

TOTON95 · Aug 14, 2024 · 4ed3e9e · 4ed3e9e
1 parent 25fcb3c
commit 4ed3e9e
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 8 deletions.
diff --git 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
@@ -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
@@ -288,7 +288,7 @@ class Navigator : public ModuleBase<Navigator>, 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
@@ -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
@@ -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;
 	}