navigator: publish navigator_state

feedback to commander

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/NavigatorStatus.msg

@@ -0,0 +1,6 @@
+# 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)
+bool failure      # True when the current mode cannot continue

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/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 @@ public:
 
 	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_failsafe(uint8_t nav_state);
+
 private:
 
 	int _local_pos_sub{-1};
@@ -305,6 +310,7 @@ private:
 
 	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 @@ private:
 
 	// 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 @@ private:
 
 	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 @@ private:
 	 */
 	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 <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Thomas Gubler <thomasgubler@gmail.com>
+ * 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_failsafe(const uint8_t nav_state)
+{
+	if (!_navigator_status.failure || _navigator_status.nav_state != nav_state) {
+		_navigator_status.failure = true;
+		_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 = false;
+		_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();

src/modules/navigator/rtl_direct.cpp

@@ -158,6 +158,16 @@ void RtlDirect::set_rtl_item()
 {
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
+	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");
+
+		_navigator->trigger_failsafe(getNavigatorStateId());
+		_rtl_state = RTLState::IDLE;
+	}
+
 	const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon,
 				       _global_pos_sub.get().lat, _global_pos_sub.get().lon);
 	const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt);