navigator: publish distance sensor mode change request when in RTL landing phase or during mission landing

src/modules/logger/logged_topics.cpp

@@ -58,6 +58,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("commander_state");
 	add_topic("config_overrides");
 	add_topic("cpuload");
+	add_topic("distance_sensor_mode_change_request");
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");
 	add_optional_topic("external_ins_local_position");

src/modules/navigator/mission_base.h

@@ -73,7 +73,7 @@ public:
 	virtual void on_activation() override;
 	virtual void on_active() override;
 
-	bool isLanding();
+	virtual bool isLanding();
 
 protected:
 

src/modules/navigator/navigator.h

@@ -64,6 +64,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/distance_sensor_mode_change_request.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/gimbal_manager_set_attitude.h>
 #include <uORB/topics/home_position.h>
@@ -316,6 +317,7 @@ private:
 	uORB::Publication<vehicle_command_s>		_vehicle_cmd_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_roi_s>		_vehicle_roi_pub{ORB_ID(vehicle_roi)};
 	uORB::Publication<mode_completed_s> _mode_completed_pub{ORB_ID(mode_completed)};
+	uORB::PublicationData<distance_sensor_mode_change_request_s> _distance_sensor_mode_change_request_pub{ORB_ID(distance_sensor_mode_change_request)};
 
 	orb_advert_t	_mavlink_log_pub{nullptr};	/**< the uORB advert to send messages over mavlink */
 
@@ -336,6 +338,7 @@ private:
 	position_setpoint_triplet_s			_takeoff_triplet{};	/**< triplet for non-mission direct takeoff command */
 	vehicle_roi_s					_vroi{};		/**< vehicle ROI */
 
+
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	Geofence	_geofence;			/**< class that handles the geofence */
@@ -400,6 +403,8 @@ private:
 
 	void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
 
+	void publish_distance_sensor_mode_request();
+
 	bool geofence_allows_position(const vehicle_global_position_s &pos);
 
 	DEFINE_PARAMETERS(

src/modules/navigator/navigator_main.cpp

@@ -110,6 +110,11 @@ Navigator::Navigator() :
 	_mission_sub = orb_subscribe(ORB_ID(mission));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
+	_distance_sensor_mode_change_request_pub.advertise();
+	_distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time();
+	_distance_sensor_mode_change_request_pub.get().req_mode = distance_sensor_mode_change_request_s::MODE_DISABLED;
+	_distance_sensor_mode_change_request_pub.update();
+
 	// Update the timeout used in mission_block (which can't hold it's own parameters)
 	_mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get());
 
@@ -898,6 +903,8 @@ void Navigator::run()
 
 		publish_navigator_status();
 
+		publish_distance_sensor_mode_request();
+
 		_geofence.run();
 
 		perf_end(_loop_perf);
@@ -1447,6 +1454,30 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
 	_vehicle_cmd_pub.publish(*vcmd);
 }
 
+void Navigator::publish_distance_sensor_mode_request()
+{
+	// Send request to enable distance sensor when in the landing phase of a mission or RTL
+	if (((_navigation_mode == &_rtl) && _rtl.isLanding()) || ((_navigation_mode == &_mission) && _mission.isLanding())) {
+
+		if (_distance_sensor_mode_change_request_pub.get().req_mode !=
+		    distance_sensor_mode_change_request_s::MODE_ENABLED) {
+
+			_distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time();
+			_distance_sensor_mode_change_request_pub.get().req_mode =
+				distance_sensor_mode_change_request_s::MODE_ENABLED;
+			_distance_sensor_mode_change_request_pub.update();
+		}
+
+	} else if (_distance_sensor_mode_change_request_pub.get().req_mode !=
+		   distance_sensor_mode_change_request_s::MODE_DISABLED) {
+
+		_distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time();
+		_distance_sensor_mode_change_request_pub.get().req_mode =
+			distance_sensor_mode_change_request_s::MODE_DISABLED;
+		_distance_sensor_mode_change_request_pub.update();
+	}
+}
+
 void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result)
 {
 	vehicle_command_ack_s command_ack = {};

src/modules/navigator/rtl.cpp

@@ -304,6 +304,28 @@ void RTL::on_active()
 	}
 }
 
+bool RTL::isLanding()
+{
+	bool is_landing{false};
+
+	switch (_rtl_type) {
+	case RtlType::RTL_MISSION_FAST:
+	case RtlType::RTL_MISSION_FAST_REVERSE:
+	case RtlType::RTL_DIRECT_MISSION_LAND:
+		is_landing = _rtl_mission_type_handle->isLanding();
+		break;
+
+	case RtlType::RTL_DIRECT:
+		is_landing = _rtl_direct.isLanding();
+		break;
+
+	default:
+		break;
+	}
+
+	return is_landing;
+}
+
 void RTL::setRtlTypeAndDestination()
 {
 

src/modules/navigator/rtl.h

@@ -89,6 +89,8 @@ public:
 
 	void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; }
 
+	bool isLanding();
+
 private:
 	enum class DestinationType {
 		DESTINATION_TYPE_HOME,

src/modules/navigator/rtl_direct.h

@@ -103,6 +103,8 @@ public:
 
 	void setRtlPosition(PositionYawSetpoint position, loiter_point_s loiter_pos);
 
+	bool isLanding() { return (_rtl_state != RTLState::IDLE) && (_rtl_state >= RTLState::LOITER_DOWN);};
+
 private:
 	/**
 	 * @brief Return to launch state machine.

src/modules/navigator/rtl_mission_fast_reverse.cpp

@@ -60,6 +60,12 @@ void RtlMissionFastReverse::on_inactive()
 				   _mission.current_seq : -1;
 }
 
+void RtlMissionFastReverse::on_inactivation()
+{
+	MissionBase::on_inactivation();
+	_in_landing_phase = false;
+}
+
 void RtlMissionFastReverse::on_activation()
 {
 	_home_pos_sub.update();
@@ -120,6 +126,7 @@ void RtlMissionFastReverse::setActiveMissionItems()
 		    _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
 		    num_found_items == 0) {
 			handleLanding(new_work_item_type);
+			_in_landing_phase = true;
 
 		} else {
 			// convert mission item to a simple waypoint, keep loiter to alt
@@ -131,6 +138,8 @@ void RtlMissionFastReverse::setActiveMissionItems()
 			_mission_item.time_inside = 0.0f;
 
 			pos_sp_triplet->previous = pos_sp_triplet->current;
+
+			_in_landing_phase = false;
 		}
 
 		if (num_found_items > 0) {

src/modules/navigator/rtl_mission_fast_reverse.h

@@ -58,6 +58,9 @@ public:
 	void on_activation() override;
 	void on_active() override;
 	void on_inactive() override;
+	void on_inactivation() override;
+
+	bool isLanding() override {return _in_landing_phase;};
 
 	rtl_time_estimate_s calc_rtl_time_estimate() override;
 
@@ -68,5 +71,7 @@ private:
 
 	int _mission_index_prior_rtl{-1};
 
+	bool _in_landing_phase{false};
+
 	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
 };