RTL: do not RTL if RTL alt is above max HAGL

msg/CMakeLists.txt

@@ -146,6 +146,7 @@ set(msg_files
 	MountOrientation.msg
 	ModeCompleted.msg
 	NavigatorMissionItem.msg
+	NavigatorStatus.msg
 	NormalizedUnsignedSetpoint.msg
 	NpfgStatus.msg
 	ObstacleDistance.msg

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)

msg/NavigatorStatus.msg

@@ -0,0 +1,9 @@
+# Current status of a Navigator mode
+# The possible values of nav_state are defined in the VehicleStatus msg.
+uint64 timestamp  # time since system start (microseconds)
+
+uint8 nav_state   # Source mode (values in VehicleStatus)
+uint8 failure     # Navigator failure enum
+
+uint8 FAILURE_NONE = 0
+uint8 FAILURE_HAGL = 1 # Target altitude exceeds maximum height above ground

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,58 @@
+/****************************************************************************
+ *
+ *   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 != 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");
+		}
+	}
+}

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/logger/logged_topics.cpp

@@ -91,6 +91,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("manual_control_switches");
 	add_topic("mission_result");
 	add_topic("navigator_mission_item");
+	add_topic("navigator_status");
 	add_topic("npfg_status", 100);
 	add_topic("offboard_control_mode", 100);
 	add_topic("onboard_computer_status", 10);

src/modules/navigator/land.cpp

@@ -42,7 +42,7 @@
 #include "navigator.h"
 
 Land::Land(Navigator *navigator) :
-	MissionBlock(navigator)
+	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)
 {
 }
 
@@ -93,7 +93,7 @@ Land::on_active()
 	if (_navigator->get_land_detected()->landed) {
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
-		_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND);
+		_navigator->mode_completed(getNavigatorStateId());
 		set_idle_item(&_mission_item);
 
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

src/modules/navigator/loiter.cpp

@@ -43,7 +43,7 @@
 #include "navigator.h"
 
 Loiter::Loiter(Navigator *navigator) :
-	MissionBlock(navigator),
+	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER),
 	ModuleParams(navigator)
 {
 }

src/modules/navigator/mission.cpp

@@ -65,7 +65,7 @@ using namespace time_literals;
 static constexpr int32_t DEFAULT_MISSION_CACHE_SIZE = 10;
 
 Mission::Mission(Navigator *navigator) :
-	MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE)
+	MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE, vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
 {
 }
 

src/modules/navigator/mission_base.cpp

@@ -46,8 +46,8 @@
 #include "mission_feasibility_checker.h"
 #include "navigator.h"
 
-MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed) :
-	MissionBlock(navigator),
+MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id) :
+	MissionBlock(navigator, navigator_state_id),
 	ModuleParams(navigator),
 	_dataman_cache_size_signed(dataman_cache_size_signed)
 {
@@ -465,7 +465,7 @@ MissionBase::set_mission_items()
 
 	if (set_end_of_mission) {
 		setEndOfMissionItems();
-		_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
+		_navigator->mode_completed(getNavigatorStateId());
 	}
 }
 

src/modules/navigator/mission_base.h

@@ -65,7 +65,7 @@ class Navigator;
 class MissionBase : public MissionBlock, public ModuleParams
 {
 public:
-	MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed);
+	MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id);
 	~MissionBase() override = default;
 
 	virtual void on_inactive() override;

src/modules/navigator/mission_block.cpp

@@ -55,8 +55,8 @@
 
 using matrix::wrap_pi;
 
-MissionBlock::MissionBlock(Navigator *navigator) :
-	NavigatorMode(navigator)
+MissionBlock::MissionBlock(Navigator *navigator, uint8_t navigator_state_id) :
+	NavigatorMode(navigator, navigator_state_id)
 {
 
 }

src/modules/navigator/mission_block.h

@@ -64,7 +64,7 @@ class MissionBlock : public NavigatorMode
 	/**
 	 * Constructor
 	 */
-	MissionBlock(Navigator *navigator);
+	MissionBlock(Navigator *navigator, uint8_t navigator_state_id);
 	virtual ~MissionBlock() = default;
 
 	MissionBlock(const MissionBlock &) = delete;

src/modules/navigator/navigator.h

@@ -69,6 +69,7 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/navigator_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_controller_landing_status.h>
 #include <uORB/topics/position_controller_status.h>
@@ -283,8 +284,12 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 
 	void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS);
 
+	void set_failsafe_status(uint8_t nav_state, bool failsafe);
+
 	void sendWarningDescentStoppedDueToTerrain();
 
+	void trigger_hagl_failsafe(uint8_t nav_state);
+
 private:
 
 	int _local_pos_sub{-1};
@@ -305,6 +310,7 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 
 	uORB::Publication<geofence_result_s>		_geofence_result_pub{ORB_ID(geofence_result)};
 	uORB::Publication<mission_result_s>		_mission_result_pub{ORB_ID(mission_result)};
+	uORB::Publication<navigator_status_s>		_navigator_status_pub{ORB_ID(navigator_status)};
 	uORB::Publication<position_setpoint_triplet_s>	_pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
 	uORB::Publication<vehicle_command_ack_s>	_vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<vehicle_command_s>		_vehicle_cmd_pub{ORB_ID(vehicle_command)};
@@ -324,6 +330,7 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 
 	// Publications
 	geofence_result_s				_geofence_result{};
+	navigator_status_s				_navigator_status{};
 	position_setpoint_triplet_s			_pos_sp_triplet{};	/**< triplet of position setpoints */
 	position_setpoint_triplet_s			_reposition_triplet{};	/**< triplet for non-mission direct position command */
 	position_setpoint_triplet_s			_takeoff_triplet{};	/**< triplet for non-mission direct takeoff command */
@@ -333,7 +340,10 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 
 	Geofence	_geofence;			/**< class that handles the geofence */
 	GeofenceBreachAvoidance _gf_breach_avoidance;
-	hrt_abstime _last_geofence_check = 0;
+	hrt_abstime _last_geofence_check{0};
+
+	bool _navigator_status_updated{false};
+	hrt_abstime _last_navigator_status_publication{0};
 
 	hrt_abstime _wait_for_vehicle_status_timestamp{0}; /**< If non-zero, wait for vehicle_status update before processing next cmd */
 
@@ -386,6 +396,8 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 	 */
 	void publish_mission_result();
 
+	void publish_navigator_status();
+
 	void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
 
 	bool geofence_allows_position(const vehicle_global_position_s &pos);

src/modules/navigator/navigator_main.cpp

@@ -41,6 +41,7 @@
  * @author Julian Oes <[email protected]>
  * @author Anton Babushkin <[email protected]>
  * @author Thomas Gubler <[email protected]>
+ * and many more...
  */
 
 #include "navigator.h"
@@ -897,6 +898,8 @@ void Navigator::run()
 			publish_mission_result();
 		}
 
+		publish_navigator_status();
+
 		_geofence.run();
 
 		perf_end(_loop_perf);
@@ -1355,6 +1358,40 @@ void Navigator::set_mission_failure_heading_timeout()
 	}
 }
 
+void Navigator::trigger_hagl_failsafe(const uint8_t nav_state)
+{
+	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;
+	}
+}
+
+void Navigator::publish_navigator_status()
+{
+	uint8_t current_nav_state = _vstatus.nav_state;
+
+	if (_navigation_mode != nullptr) {
+		current_nav_state = _navigation_mode->getNavigatorStateId();
+	}
+
+	if (_navigator_status.nav_state != current_nav_state) {
+		_navigator_status.nav_state = current_nav_state;
+		_navigator_status.failure = navigator_status_s::FAILURE_NONE;
+		_navigator_status_updated = true;
+	}
+
+	if (_navigator_status_updated
+	    || (hrt_elapsed_time(&_last_navigator_status_publication) > 500_ms)) {
+		_navigator_status.timestamp = hrt_absolute_time();
+		_navigator_status_pub.publish(_navigator_status);
+
+		_navigator_status_updated = false;
+		_last_navigator_status_publication = hrt_absolute_time();
+	}
+}
+
 void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
 {
 	vcmd->timestamp = hrt_absolute_time();