comander: trigger failsafe when navigator reports failure

msg/FailsafeFlags.msg

@@ -49,6 +49,7 @@ bool vtol_fixed_wing_system_failure   # vehicle in fixed-wing system failure fai
 bool wind_limit_exceeded              # Wind limit exceeded
 bool flight_time_limit_exceeded       # Maximum flight time exceeded
 bool local_position_accuracy_low      # Local position estimate has dropped below threshold, but is currently still declared valid
+bool navigator_failure        	      # Navigator failed to execute a mode
 
 # Failure detector
 bool fd_critical_failure              # Critical failure (attitude/altitude limit exceeded, or external ATS)

src/modules/commander/HealthAndArmingChecks/CMakeLists.txt

@@ -55,6 +55,7 @@ px4_add_library(health_and_arming_checks
 	checks/manualControlCheck.cpp
 	checks/missionCheck.cpp
 	checks/modeCheck.cpp
+	checks/navigatorCheck.cpp
 	checks/offboardCheck.cpp
 	checks/openDroneIDCheck.cpp
 	checks/parachuteCheck.cpp

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -49,6 +49,7 @@
 #include "checks/escCheck.hpp"
 #include "checks/estimatorCheck.hpp"
 #include "checks/failureDetectorCheck.hpp"
+#include "checks/navigatorCheck.hpp"
 #include "checks/gyroCheck.hpp"
 #include "checks/imuConsistencyCheck.hpp"
 #include "checks/loggerCheck.hpp"
@@ -129,6 +130,7 @@ class HealthAndArmingChecks : public ModuleParams
 	EscChecks _esc_checks;
 	EstimatorChecks _estimator_checks;
 	FailureDetectorChecks _failure_detector_checks;
+	NavigatorChecks _navigator_checks;
 	GyroChecks _gyro_checks;
 	ImuConsistencyChecks _imu_consistency_checks;
 	LoggerChecks _logger_checks;
@@ -167,6 +169,7 @@ class HealthAndArmingChecks : public ModuleParams
 		&_esc_checks,
 		&_estimator_checks,
 		&_failure_detector_checks,
+		&_navigator_checks,
 		&_gyro_checks,
 		&_imu_consistency_checks,
 		&_logger_checks,

src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp

@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "navigatorCheck.hpp"
+
+void NavigatorChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	navigator_status_s status;
+
+	if (!_navigator_status_sub.copy(&status)) {
+		status = {};
+	}
+
+	if (context.status().nav_state == status.nav_state) {
+		reporter.failsafeFlags().navigator_failure = status.failure;
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/navigator_status.h>
+
+
+class NavigatorChecks : public HealthAndArmingCheckBase
+{
+public:
+	NavigatorChecks() = default;
+	~NavigatorChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	uORB::Subscription _navigator_status_sub{ORB_ID(navigator_status)};
+};

src/modules/commander/failsafe/failsafe.cpp

@@ -472,6 +472,16 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 		CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL));
 	}
 
+	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
+	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
+		CHECK_FAILSAFE(status_flags, navigator_failure,
+			       ActionOptions(Action::Land).clearOn(ClearCondition::OnModeChangeOrDisarm));
+
+	} else {
+		CHECK_FAILSAFE(status_flags, navigator_failure,
+			       ActionOptions(Action::Hold).clearOn(ClearCondition::OnModeChangeOrDisarm));
+	}
+
 	CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred());
 
 	// Battery flight time remaining failsafe

src/modules/navigator/rtl_direct.cpp

@@ -161,8 +161,8 @@ void RtlDirect::set_rtl_item()
 	if (_global_pos_sub.get().terrain_alt_valid
 	    && ((_rtl_alt - _global_pos_sub.get().terrain_alt) > _navigator->get_local_position()->hagl_max)) {
 		// Handle case where the RTL altidude is above the maximum HAGL and land in place instead of RTL
-		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL, landing\t");
-		events::send(events::ID("rtl_fail_max_hagl"), events::Log::Warning, "RTL: return alt higher than max HAGL, landing");
+		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());
 		_rtl_state = RTLState::IDLE;