add fallback landing

TODO: Actually it would make more sense to transition to
LAND flight mode instead.

boards/px4/fmu-v5/init/rc.board_extras

@@ -0,0 +1,7 @@
+#!/bin/sh
+#
+# PX4 FMUv5 specific board extras init
+#------------------------------------------------------------------------------
+
+# Target estimator for precision landing
+landing_target_estimator start

boards/px4/fmu-v5x/init/rc.board_extras

@@ -0,0 +1,7 @@
+#!/bin/sh
+#
+# PX4 FMUv5x specific board extras init
+#------------------------------------------------------------------------------
+
+# Target estimator for precision landing
+landing_target_estimator start

boards/px4/fmu-v6x/init/rc.board_extras

@@ -0,0 +1,7 @@
+#!/bin/sh
+#
+# PX4 FMUv6x specific board extras init
+#------------------------------------------------------------------------------
+
+# Target estimator for precision landing
+landing_target_estimator start

msg/CMakeLists.txt

@@ -146,6 +146,7 @@ set(msg_files
 	PowerButtonState.msg
 	PowerMonitor.msg
 	PpsCapture.msg
+	PrecisionLandingStatus.msg
 	PwmInput.msg
 	Px4ioStatus.msg
 	QshellReq.msg

msg/NavigatorMissionItem.msg

@@ -6,6 +6,18 @@ uint16 sequence_current          # Sequence of the current mission item
 
 uint16 nav_cmd
 
+# Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message
+uint8 WORK_ITEM_TYPE_DEFAULT = 0                        # Default mission item
+uint8 WORK_ITEM_TYPE_TAKEOFF = 1                        # Takeoff before moving to waypoint
+uint8 WORK_ITEM_TYPE_MOVE_TO_LAND = 2                   # Move to land waypoint before descent
+uint8 WORK_ITEM_TYPE_ALIGN = 3                          # Align for next waypoint
+uint8 WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF = 4        # VTOL specific
+uint8 WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION = 5  # VTOL specific
+uint8 WORK_ITEM_TYPE_PRECISION_LAND = 6                 # Precision Landing for Multicopters using external aid
+
+uint8 nav_sub_cmd                # Synonymous to "work_item_type" in Navigator. Will be renamed eventually in navigator.
+                                 # sub-command, for example AUTO_PRECLAND in main command AUTO_MISSION
+
 float32 latitude
 float32 longitude
 

msg/PrecisionLandingStatus.msg

@@ -0,0 +1,9 @@
+uint8 PRECLAND_STATE_AUTORTL = 0               # Execute normal RTL logic until the target is spotted
+uint8 PRECLAND_STATE_SEARCH = 1                # Drone is supposed to land at current location, but has not spotted the target yet. Wait at current location in hopes of spotting the target.
+uint8 PRECLAND_STATE_MOVE_ABOVE_TARGET = 2     # Fly above target while maintaining RTL altitude
+uint8 PRECLAND_STATE_DESCEND_ABOVE_TARGET = 3  # Drone is above target and descending. This state still expects to see the target.
+uint8 PRECLAND_STATE_TOUCHING_DOWN = 4         # Same as descend, but if sight of target is lost, drone continues to land at last known location.
+uint8 PRECLAND_STATE_FALLBACK = 5              # Fallback to conventional landing, either because of opportunistic precision landing or because maximum number of precland attempts were reached
+
+uint64 timestamp        # time since system start (microseconds)
+uint8 state    # this flight-task's internal state

src/modules/flight_mode_manager/CMakeLists.txt

@@ -40,6 +40,7 @@
 set(flight_tasks_all)
 list(APPEND flight_tasks_all
 	Auto
+	PrecisionLanding
 	Descend
 	Failsafe
 	ManualAcceleration

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -107,6 +107,7 @@ void FlightModeManager::Run()
 		_vehicle_control_mode_sub.update();
 		_vehicle_land_detected_sub.update();
 		_vehicle_status_sub.update();
+		_navigator_mission_item_sub.update();
 
 		start_flight_task();
 
@@ -139,6 +140,16 @@ void FlightModeManager::start_flight_task()
 	bool task_failure = false;
 	bool should_disable_task = true;
 
+	const bool land_should_be_precland = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
+					      ||
+					      _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)
+					     && _param_rtl_pld_md.get() > 0;
+	const bool precland_mission_item_active = _vehicle_status_sub.get().nav_state ==
+			vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION &&
+			_navigator_mission_item_sub.get().nav_sub_cmd == navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND;
+	const bool precland_flight_mode = _vehicle_status_sub.get().nav_state ==
+					  vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
+
 	// Do not run any flight task for VTOLs in fixed-wing mode
 	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 		switchTask(FlightTaskIndex::None);
@@ -164,6 +175,15 @@ void FlightModeManager::start_flight_task()
 			task_failure = true;
 		}
 
+	} else if (land_should_be_precland || precland_mission_item_active || precland_flight_mode) {
+		// Take-over landing from navigator if precision landing is enabled
+		should_disable_task = false;
+
+		if (switchTask(FlightTaskIndex::PrecisionLanding) != FlightTaskError::NoError) {
+			PX4_INFO("Switching to precland flight task");
+			task_failure = true;
+		}
+
 	} else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
 
 		// Emergency descend

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -44,6 +44,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/topics/navigator_mission_item.h>
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/takeoff_status.h>
@@ -150,6 +151,7 @@ private:
 	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
 
 	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionData<navigator_mission_item_s> _navigator_mission_item_sub{ORB_ID(navigator_mission_item)};
 
 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
 	uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
@@ -157,6 +159,7 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max,
-		(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode
+		(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
+		(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md
 	);
 };

src/modules/flight_mode_manager/tasks/PrecisionLanding/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2023 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.
+#
+############################################################################
+
+px4_add_library(FlightTaskPrecisionLanding
+FlightTaskPrecisionLanding.cpp
+)
+
+target_link_libraries(FlightTaskPrecisionLanding PUBLIC FlightTask)
+target_include_directories(FlightTaskPrecisionLanding PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.cpp

@@ -0,0 +1,317 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskPrecisionLanding.cpp
+ *
+ */
+
+#include "FlightTaskPrecisionLanding.hpp"
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+bool FlightTaskPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTask::activate(last_setpoint);
+	_precland_state = PRECLAND_STATE::AUTORTL_CLIMB;
+
+	_position_setpoint = _position;
+	_search_count = 0;
+
+	_initial_yaw = _yaw;
+	_initial_position = _position;
+
+	return ret;
+}
+
+void FlightTaskPrecisionLanding::do_state_transition(PRECLAND_STATE new_state)
+{
+	_initial_yaw = _yaw;
+	_precland_state = new_state;
+	_state_start_time = hrt_absolute_time();
+}
+
+bool FlightTaskPrecisionLanding::inside_acceptance_radius()
+{
+	// TODO: Reuse what FlightTask has...
+	return matrix::Vector3f(_position_setpoint - _position).norm() <= _param_pld_hacc_rad.get();
+}
+
+bool FlightTaskPrecisionLanding::precision_target_available()
+{
+	const bool ever_received = _landing_target_pose.timestamp != 0;
+	const bool timed_out =  hrt_absolute_time() - _landing_target_pose.timestamp > _param_pld_btout.get() * SEC2USEC;
+	return ever_received && !timed_out;
+}
+
+void FlightTaskPrecisionLanding::generate_pos_xy_setpoints()
+{
+	switch (_precland_state) {
+	case PRECLAND_STATE::AUTORTL_CLIMB:
+		_position_setpoint(0) = _initial_position(0);
+		_position_setpoint(1) = _initial_position(1);
+		break;
+
+	case PRECLAND_STATE::AUTORTL_APPROACH:
+	case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
+
+		// Horizontal approach to home or precision target
+		if (precision_target_available()) {
+			_position_setpoint(0) = _landing_target_pose.x_abs;
+			_position_setpoint(1) = _landing_target_pose.y_abs;
+
+		} else {
+			_position_setpoint(0) = _sub_home_position.get().x;
+			_position_setpoint(1) = _sub_home_position.get().y;
+		}
+
+		break;
+
+	case PRECLAND_STATE::SEARCHING_TARGET:
+	case PRECLAND_STATE::MOVING_ABOVE_TARGET:
+	case PRECLAND_STATE::LANDING_ON_TARGET:
+		_position_setpoint(0) = _landing_target_pose.x_abs;
+		_position_setpoint(1) = _landing_target_pose.y_abs;
+		break;
+
+	case PRECLAND_STATE::FALLBACK_LAND:
+               // TODO: might be safer to set only once during state change
+		_position_setpoint(0) = _sub_home_position.get().x;
+		_position_setpoint(1) = _sub_home_position.get().y;
+		break;
+	}
+
+}
+
+void FlightTaskPrecisionLanding::generate_pos_z_setpoints()
+{
+	const float search_rel_altitude = _param_pld_srch_alt.get();
+
+	switch (_precland_state) {
+	case PRECLAND_STATE::AUTORTL_CLIMB:
+	case PRECLAND_STATE::AUTORTL_APPROACH:
+		// Horizontal approach to home or precision target
+		_position_setpoint(2) = _sub_home_position.get().z - _param_rtl_return_alt.get();
+		break;
+
+	case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
+	case PRECLAND_STATE::SEARCHING_TARGET:
+	case PRECLAND_STATE::MOVING_ABOVE_TARGET:
+
+		// TODO: Should use current altitude when state was entered as the altitude setpoint
+		if (PX4_ISFINITE(_landing_target_pose.z_abs)) {
+			// For safety reasons take whichever reference altitude is higher up (lower in NED)
+			_position_setpoint(2) = math::min(_sub_home_position.get().z, _landing_target_pose.z_abs) - search_rel_altitude;
+
+		} else {
+			_position_setpoint(2) = _sub_home_position.get().z - search_rel_altitude;
+		}
+
+		break;
+
+	case PRECLAND_STATE::LANDING_ON_TARGET:
+	case PRECLAND_STATE::FALLBACK_LAND:
+		_position_setpoint(2) = NAN;
+		break;
+	}
+}
+
+void FlightTaskPrecisionLanding::generate_vel_setpoints()
+{
+	switch (_precland_state) {
+	case PRECLAND_STATE::AUTORTL_CLIMB:
+	case PRECLAND_STATE::AUTORTL_APPROACH:
+	case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
+	case PRECLAND_STATE::SEARCHING_TARGET:
+	case PRECLAND_STATE::MOVING_ABOVE_TARGET:
+		_velocity_setpoint.setNaN();
+		break;
+
+	case PRECLAND_STATE::LANDING_ON_TARGET:
+	case PRECLAND_STATE::FALLBACK_LAND:
+		_velocity_setpoint(0) = 0;
+		_velocity_setpoint(1) = 0;
+		_velocity_setpoint(2) = _param_mpc_land_speed.get();
+		break;
+	}
+}
+
+void FlightTaskPrecisionLanding::generate_acc_setpoints()
+{
+	_acceleration_setpoint.setNaN();
+}
+
+void FlightTaskPrecisionLanding::generate_yaw_setpoint()
+{
+	switch (_precland_state) {
+	case PRECLAND_STATE::AUTORTL_CLIMB:
+		_yaw_setpoint = _initial_yaw;
+		break;
+
+	case PRECLAND_STATE::AUTORTL_APPROACH:
+
+	// TODO: Point in direction of flight
+	case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
+	case PRECLAND_STATE::SEARCHING_TARGET:
+	case PRECLAND_STATE::MOVING_ABOVE_TARGET:
+	case PRECLAND_STATE::LANDING_ON_TARGET:
+		_yaw_setpoint = _target_yaw;
+		break;
+
+	case PRECLAND_STATE::FALLBACK_LAND:
+		// TODO: Keep yaw constant
+		break;
+	}
+}
+
+void FlightTaskPrecisionLanding::check_state_transitions()
+{
+	switch (_precland_state) {
+	case PRECLAND_STATE::AUTORTL_CLIMB:
+
+		// transition condition
+		// Wait for drone to reach z position setpoint or be higher
+		if (_position(2) <= _position_setpoint(2) + _param_nav_mc_alt_rad.get()) {
+			mavlink_log_info(&_mavlink_log_pub, "Horizontal approach");
+			do_state_transition(PRECLAND_STATE::AUTORTL_APPROACH);
+		}
+
+		break;
+
+	case PRECLAND_STATE::AUTORTL_APPROACH: {
+			// transition condition
+			// Wait for drone to reach position setpoint
+			if (inside_acceptance_radius()) {
+				if (precision_target_available()) {
+					mavlink_log_info(&_mavlink_log_pub, "Moving above target");
+					do_state_transition(PRECLAND_STATE::MOVING_ABOVE_TARGET);
+
+				} else {
+					mavlink_log_info(&_mavlink_log_pub, "Moving to search center");
+					do_state_transition(PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE);
+				}
+
+				_target_yaw = _yaw_setpoint;
+			}
+
+			break;
+		}
+
+	case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
+
+		// transition condition
+		if (inside_acceptance_radius()) {
+			mavlink_log_info(&_mavlink_log_pub, "Starting search");
+			do_state_transition(PRECLAND_STATE::SEARCHING_TARGET);
+		}
+
+		break;
+
+	case PRECLAND_STATE::SEARCHING_TARGET: {
+			const float max_search_duration = _param_pld_srch_tout.get();
+
+			// Currently no search pattern is implemented. Just wait for target...
+			// Workaround for Orion app making unwanted changes to my parameters
+			if ((hrt_absolute_time() - _state_start_time) > max_search_duration * SEC2USEC) {
+				if (++_search_count >= _param_pld_max_srch.get()) {
+					mavlink_log_info(&_mavlink_log_pub, "Abandonning search and landing immediately");
+					do_state_transition(PRECLAND_STATE::FALLBACK_LAND);
+
+				} else {
+					mavlink_log_info(&_mavlink_log_pub, "Moving to search center");
+					do_state_transition(PRECLAND_STATE::SEARCHING_TARGET);
+				}
+			}
+
+			// transition condition
+			if (precision_target_available()) {
+				mavlink_log_info(&_mavlink_log_pub, "Moving above target");
+				do_state_transition(PRECLAND_STATE::MOVING_ABOVE_TARGET);
+			}
+
+			break;
+		}
+
+	case PRECLAND_STATE::MOVING_ABOVE_TARGET:
+
+		// Transition condition
+		// - Check acceptance radius and
+		// - wait long enough to observe if the landing_target_pose timed out before proceeding
+		if (abs(_position(0) - _position_setpoint(0)) <= _param_pld_hacc_rad.get()
+		    && abs(_position(1) - _position_setpoint(1)) <= _param_pld_hacc_rad.get()
+		    && _velocity.norm() <= HOVER_SPEED_THRESHOLD
+		    && (hrt_absolute_time() - _state_start_time) > _param_pld_btout.get() * SEC2USEC) {
+
+			if (hrt_absolute_time() - _landing_target_pose.timestamp < _param_pld_btout.get() * SEC2USEC) {
+				mavlink_log_info(&_mavlink_log_pub, "Landing on target...");
+				do_state_transition(PRECLAND_STATE::LANDING_ON_TARGET);
+
+			} else {
+				mavlink_log_info(&_mavlink_log_pub, "Lost target, back to searching");
+				do_state_transition(PRECLAND_STATE::SEARCHING_TARGET);
+			}
+		}
+
+		break;
+
+	case PRECLAND_STATE::LANDING_ON_TARGET:
+	case PRECLAND_STATE::FALLBACK_LAND:
+		break;
+	}
+}
+
+bool FlightTaskPrecisionLanding::update()
+{
+	// Get setpoints from FlightTask and later override if necessary
+	bool ret = FlightTask::update();
+
+	// Fetch uorb
+	if (_landing_target_pose_sub.updated()) {
+		_landing_target_pose_sub.copy(&_landing_target_pose);
+	}
+
+	generate_pos_xy_setpoints();
+	generate_pos_z_setpoints();
+	generate_vel_setpoints();
+	generate_acc_setpoints();
+	generate_yaw_setpoint();
+	check_state_transitions();
+
+	precision_landing_status_s precision_landing_status{};
+	precision_landing_status.timestamp = hrt_absolute_time();
+	precision_landing_status.state = _precland_state;
+	_precision_landing_status_pub.publish(precision_landing_status);
+
+	return ret;
+}

src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.hpp

@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskPrecisionLanding.hpp
+ *
+ * Flight task for better precision landing
+ */
+
+#pragma once
+
+#include "FlightTask.hpp"
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/landing_target_pose.h>
+#include <uORB/topics/precision_landing_status.h>
+// #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/topics/follow_target_estimator.h>
+// #include <lib/ecl/geo/geo.h>
+#include <mathlib/mathlib.h>
+#include <lib/matrix/matrix/math.hpp>
+
+// TODO: Get the correct define from some header
+#define SEC2USEC 1000000.0f
+#define HOVER_SPEED_THRESHOLD 0.20f	// If drone has speed under this value it is considered to be hovering in place
+
+class FlightTaskPrecisionLanding : public FlightTask
+{
+public:
+	FlightTaskPrecisionLanding() = default;
+	virtual ~FlightTaskPrecisionLanding() = default;
+
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+
+	bool update() override;
+
+private:
+	// TODO: Use ModuleParams
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed, ///< velocity for controlled descend
+					(ParamFloat<px4::params::PLD_SRCH_ALT>) _param_pld_srch_alt,
+					(ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_pld_srch_tout,
+					(ParamInt<px4::params::PLD_MAX_SRCH>) _param_pld_max_srch,
+					(ParamFloat<px4::params::PLD_HACC_RAD>) _param_pld_hacc_rad,
+					(ParamFloat<px4::params::PLD_BTOUT>) _param_pld_btout,
+					(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt,
+					(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad
+				       )
+
+	enum PRECLAND_STATE {
+		AUTORTL_CLIMB,
+		AUTORTL_APPROACH,
+		MOVE_TO_SEARCH_ALTITUDE,
+		SEARCHING_TARGET,
+		MOVING_ABOVE_TARGET,
+		LANDING_ON_TARGET,
+		FALLBACK_LAND
+	};
+
+	void do_state_transition(PRECLAND_STATE new_state);
+
+	bool inside_acceptance_radius();
+
+	bool precision_target_available();
+
+	void generate_setpoints();
+	void generate_pos_xy_setpoints();
+	void generate_pos_z_setpoints();
+	void generate_vel_setpoints();
+	void generate_acc_setpoints();
+	void generate_yaw_setpoint();
+
+	void check_state_transitions();
+
+	PRECLAND_STATE _precland_state;
+
+	uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
+	landing_target_pose_s _landing_target_pose{}; /**< precision landing target position */
+
+	uORB::PublicationMulti<precision_landing_status_s> _precision_landing_status_pub{ORB_ID(precision_landing_status)};
+
+	uint64_t _state_start_time{0}; /**< time when entering search state */
+	int _search_count = 0;
+	float _initial_yaw;
+	float _target_yaw;
+	matrix::Vector3f _initial_position;
+
+	orb_advert_t	_mavlink_log_pub{nullptr};
+};

src/modules/flight_mode_manager/tasks/PrecisionLanding/precland_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2017-2023 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

src/modules/logger/logged_topics.cpp

@@ -94,6 +94,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("position_controller_status", 500);
 	add_topic("position_controller_landing_status", 100);
 	add_topic("position_setpoint_triplet", 200);
+	add_topic("precision_landing_status", 1000);
 	add_optional_topic("px4io_status");
 	add_topic("radio_status");
 	add_topic("rtl_time_estimate", 1000);

src/modules/navigator/CMakeLists.txt

@@ -46,7 +46,6 @@ px4_add_module(
 		rtl.cpp
 		takeoff.cpp
 		land.cpp
-		precland.cpp
 		mission_feasibility_checker.cpp
 		geofence.cpp
 		vtol_takeoff.cpp

src/modules/navigator/mission.cpp

@@ -156,7 +156,7 @@ Mission::on_inactive()
 	}
 
 	/* reset so current mission item gets restarted if mission was paused */
-	_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+	_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT;
 
 	/* reset so MISSION_ITEM_REACHED isn't published */
 	_navigator->get_mission_result()->seq_reached = -1;
@@ -176,14 +176,10 @@ Mission::on_inactivation()
 	_navigator->stop_capturing_images();
 	_navigator->release_gimbal_control();
 
-	if (_navigator->get_precland()->is_activated()) {
-		_navigator->get_precland()->on_inactivation();
-	}
-
 	_time_mission_deactivated = hrt_absolute_time();
 
 	/* reset so current mission item gets restarted if mission was paused */
-	_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+	_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT;
 }
 
 void
@@ -249,7 +245,7 @@ Mission::on_active()
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) {
 		/* If we just completed a takeoff which was inserted before the right waypoint,
 		   there is no need to report that we reached it because we didn't. */
-		if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
+		if (_work_item_type != navigator_mission_item_s::WORK_ITEM_TYPE_TAKEOFF) {
 			set_mission_item_reached();
 		}
 
@@ -280,7 +276,7 @@ Mission::on_active()
 		 || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
 		 || _mission_item.nav_cmd == NAV_CMD_LAND
 		 || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-		 || _work_item_type == WORK_ITEM_TYPE_ALIGN)) {
+		 || _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_ALIGN)) {
 		// Mount control is disabled If the vehicle is in ROI-mode, the vehicle
 		// needs to rotate such that ROI is in the field of view.
 		// ROI only makes sense for multicopters.
@@ -296,13 +292,6 @@ Mission::on_active()
 
 		do_abort_landing();
 	}
-
-	if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) {
-		_navigator->get_precland()->on_active();
-
-	} else if (_navigator->get_precland()->is_activated()) {
-		_navigator->get_precland()->on_inactivation();
-	}
 }
 
 bool
@@ -379,7 +368,7 @@ Mission::set_execution_mode(const uint8_t mode)
 					--_current_mission_index;
 				}
 
-				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+				_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT;
 			}
 
 			break;
@@ -395,7 +384,7 @@ Mission::set_execution_mode(const uint8_t mode)
 					++_current_mission_index;
 				}
 
-				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+				_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT;
 			}
 
 			break;
@@ -547,7 +536,7 @@ Mission::update_mission()
 			_navigator->get_mission_result()->seq_total = _mission.count;
 
 			/* reset work item if new mission has been accepted */
-			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+			_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT;
 			_mission_changed = true;
 		}
 
@@ -586,7 +575,7 @@ void
 Mission::advance_mission()
 {
 	/* do not advance mission item if we're processing sub mission work items */
-	if (_work_item_type != WORK_ITEM_TYPE_DEFAULT) {
+	if (_work_item_type != navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) {
 		return;
 	}
 
@@ -648,7 +637,7 @@ Mission::set_mission_items()
 	bool has_next_position_item = false;
 	bool has_after_next_position_item = false;
 
-	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+	uint8_t new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT;
 
 	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
 				  &mission_item_after_next_position, &has_after_next_position_item)) {
@@ -757,7 +746,7 @@ Mission::set_mission_items()
 			user_feedback_done = true;
 		}
 
-		publish_navigator_mission_item(); // for logging
+		publish_navigator_mission_item();
 		_navigator->set_position_setpoint_triplet_updated();
 
 		return;
@@ -783,10 +772,10 @@ Mission::set_mission_items()
 				/* do takeoff before going to setpoint if needed and not already in takeoff */
 				/* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */
 				if (do_need_vertical_takeoff() &&
-				    _work_item_type == WORK_ITEM_TYPE_DEFAULT &&
-				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+				    _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT &&
+				    new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) {
 
-					new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
+					new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_TAKEOFF;
 
 					/* use current mission item as next position item */
 					mission_item_next_position = _mission_item;
@@ -811,8 +800,8 @@ Mission::set_mission_items()
 					_mission_item.time_inside = 0.0f;
 
 				} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
-					   && _work_item_type == WORK_ITEM_TYPE_DEFAULT
-					   && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
+					   && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT
+					   && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT
 					   && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 
 					/* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */
@@ -821,11 +810,11 @@ Mission::set_mission_items()
 					_mission_item.yaw = NAN;
 
 				} else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
-					   && _work_item_type == WORK_ITEM_TYPE_DEFAULT
-					   && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+					   && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT
+					   && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) {
 					// if the vehicle is already in fixed wing mode then the current mission item
 					// will be accepted immediately and the work items will be skipped
-					_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
+					_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_TAKEOFF;
 
 
 					/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
@@ -834,8 +823,8 @@ Mission::set_mission_items()
 
 				/* if we just did a normal takeoff navigate to the actual waypoint now */
 				if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF &&
-				    _work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
-				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+				    _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_TAKEOFF &&
+				    new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) {
 
 					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 					/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
@@ -844,8 +833,8 @@ Mission::set_mission_items()
 
 				/* if we just did a VTOL takeoff, prepare transition */
 				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
-				    _work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
-				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT &&
+				    _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_TAKEOFF &&
+				    new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT &&
 				    _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
 				    !_navigator->get_land_detected()->landed) {
 
@@ -859,7 +848,7 @@ Mission::set_mission_items()
 
 					_mission_item.force_heading = true;
 
-					new_work_item_type = WORK_ITEM_TYPE_ALIGN;
+					new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_ALIGN;
 
 					/* set position setpoint to current while aligning */
 					_mission_item.lat = _navigator->get_global_position()->lat;
@@ -868,7 +857,7 @@ Mission::set_mission_items()
 
 				/* heading is aligned now, prepare transition */
 				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
-				    _work_item_type == WORK_ITEM_TYPE_ALIGN &&
+				    _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_ALIGN &&
 				    _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
 				    !_navigator->get_land_detected()->landed) {
 
@@ -877,7 +866,7 @@ Mission::set_mission_items()
 
 					/* check if the vtol_takeoff waypoint is on top of us */
 					if (do_need_move_to_takeoff()) {
-						new_work_item_type = WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
+						new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
 					}
 
 					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
@@ -889,10 +878,10 @@ Mission::set_mission_items()
 
 				/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
 				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
-				    && _work_item_type == WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF
-				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+				    && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF
+				    && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) {
 
-					new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+					new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT;
 					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 					_mission_item.autocontinue = true;
 					_mission_item.time_inside = 0.0f;
@@ -900,11 +889,12 @@ Mission::set_mission_items()
 
 				/* move to land wp as fixed wing */
 				if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-				    && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF)
-				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
+				    && (_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT
+					|| _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF)
+				    && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT
 				    && !_navigator->get_land_detected()->landed) {
 
-					new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
+					new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND;
 
 					/* use current mission item as next position item */
 					mission_item_next_position = _mission_item;
@@ -926,8 +916,8 @@ Mission::set_mission_items()
 
 				/* transition to MC */
 				if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-				    && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
-				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
+				    && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND
+				    && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT
 				    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
 				    && !_navigator->get_land_detected()->landed) {
 
@@ -935,7 +925,7 @@ Mission::set_mission_items()
 					_mission_item.altitude = _navigator->get_global_position()->alt;
 					_mission_item.altitude_is_relative = false;
 
-					new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
+					new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
 
 					// make previous setpoint invalid, such that there will be no prev-current line following
 					// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
@@ -944,11 +934,11 @@ Mission::set_mission_items()
 
 				/* move to landing waypoint before descent if necessary */
 				if (do_need_move_to_land() &&
-				    (_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
-				     _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION) &&
-				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+				    (_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT ||
+				     _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION) &&
+				    new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) {
 
-					new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
+					new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND;
 
 					/* use current mission item as next position item */
 					mission_item_next_position = _mission_item;
@@ -978,38 +968,19 @@ Mission::set_mission_items()
 					// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
 					pos_sp_triplet->previous.valid = false;
 
-				} else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+				} else if (_mission_item.nav_cmd == NAV_CMD_LAND
+					   && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) {
 					if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
-						new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND;
-
-						if (_mission_item.land_precision == 1) {
-							_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
-
-						} else { //_mission_item.land_precision == 2
-							_navigator->get_precland()->set_mode(PrecLandMode::Required);
-						}
-
-						_navigator->get_precland()->on_activation();
-
+						new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND;
 					}
 				}
 
 				/* we just moved to the landing waypoint, now descend */
-				if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND &&
-				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+				if (_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND &&
+				    new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) {
 
 					if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
-						new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND;
-
-						if (_mission_item.land_precision == 1) {
-							_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
-
-						} else { //_mission_item.land_precision == 2
-							_navigator->get_precland()->set_mode(PrecLandMode::Required);
-						}
-
-						_navigator->get_precland()->on_activation();
-
+						new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND;
 					}
 
 				}
@@ -1062,8 +1033,8 @@ Mission::set_mission_items()
 
 				/* turn towards next waypoint before MC to FW transition */
 				if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
-				    && _work_item_type == WORK_ITEM_TYPE_DEFAULT
-				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
+				    && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT
+				    && new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT
 				    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 				    && !_navigator->get_land_detected()->landed
 				    && has_next_position_item) {
@@ -1071,7 +1042,7 @@ Mission::set_mission_items()
 					/* disable weathervane before front transition for allowing yaw to align */
 					pos_sp_triplet->current.disable_weather_vane = true;
 
-					new_work_item_type = WORK_ITEM_TYPE_ALIGN;
+					new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_ALIGN;
 
 					set_align_mission_item(&_mission_item, &mission_item_next_position);
 
@@ -1081,10 +1052,10 @@ Mission::set_mission_items()
 				}
 
 				/* yaw is aligned now */
-				if (_work_item_type == WORK_ITEM_TYPE_ALIGN &&
-				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+				if (_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_ALIGN &&
+				    new_work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT) {
 
-					new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+					new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT;
 
 					/* re-enable weather vane again after alignment */
 					pos_sp_triplet->current.disable_weather_vane = false;
@@ -1145,7 +1116,7 @@ Mission::set_mission_items()
 	} else {
 		// The mission item is not a gate, set the current position setpoint from mission item (is protected against non-position items)
 		// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
-		if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) {
+		if (new_work_item_type != navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND) {
 			mission_apply_limitation(_mission_item);
 			mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		}
@@ -1155,7 +1126,7 @@ Mission::set_mission_items()
 
 	// Only set the previous position item if the current one really changed
 	// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
-	if ((_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) &&
+	if ((_work_item_type != navigator_mission_item_s::WORK_ITEM_TYPE_MOVE_TO_LAND) &&
 	    !position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
 		pos_sp_triplet->previous = current_setpoint_copy;
 	}
@@ -1218,7 +1189,7 @@ Mission::set_mission_items()
 		}
 	}
 
-	publish_navigator_mission_item(); // for logging
+	publish_navigator_mission_item();
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
@@ -1465,7 +1436,7 @@ Mission::do_abort_landing()
 	_navigator->get_position_setpoint_triplet()->next.lon = (double)NAN;
 	_navigator->get_position_setpoint_triplet()->next.alt = NAN;
 
-	publish_navigator_mission_item(); // for logging
+	publish_navigator_mission_item();
 	_navigator->set_position_setpoint_triplet_updated();
 
 	mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing waypoint.\t",
@@ -1909,6 +1880,7 @@ void Mission::publish_navigator_mission_item()
 	navigator_mission_item.instance_count = _navigator->mission_instance_count();
 	navigator_mission_item.sequence_current = _current_mission_index;
 	navigator_mission_item.nav_cmd = _mission_item.nav_cmd;
+	navigator_mission_item.nav_sub_cmd = _work_item_type;
 	navigator_mission_item.latitude = _mission_item.lat;
 	navigator_mission_item.longitude = _mission_item.lon;
 	navigator_mission_item.altitude = _mission_item.altitude;

src/modules/navigator/mission.h

@@ -280,16 +280,7 @@ private:
 	bool _mission_waypoints_changed{false};
 	bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */
 
-	// Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message
-	enum work_item_type {
-		WORK_ITEM_TYPE_DEFAULT,		/**< default mission item */
-		WORK_ITEM_TYPE_TAKEOFF,		/**< takeoff before moving to waypoint */
-		WORK_ITEM_TYPE_MOVE_TO_LAND,	/**< move to land waypoint before descent */
-		WORK_ITEM_TYPE_ALIGN,		/**< align for next waypoint */
-		WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF,
-		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
-		WORK_ITEM_TYPE_PRECISION_LAND
-	} _work_item_type{WORK_ITEM_TYPE_DEFAULT};	/**< current type of work to do (sub mission item) */
+	uint8_t _work_item_type{navigator_mission_item_s::WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do during mission (sub mission item) */
 
 	uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL};	/**< the current mode of how the mission is executed,look at mission_result.msg for the definition */
 	bool _execution_mode_changed{false};

src/modules/navigator/navigator.h

@@ -43,7 +43,6 @@
 
 #include "geofence.h"
 #include "land.h"
-#include "precland.h"
 #include "loiter.h"
 #include "mission.h"
 #include "navigator_mode.h"
@@ -86,7 +85,7 @@ using namespace time_literals;
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 8
+#define NAVIGATOR_MODE_ARRAY_SIZE 6
 
 class Navigator : public ModuleBase<Navigator>, public ModuleParams
 {
@@ -159,8 +158,6 @@ public:
 	vehicle_local_position_s    *get_local_position() { return &_local_pos; }
 	vehicle_status_s            *get_vstatus() { return &_vstatus; }
 
-	PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */
-
 	const vehicle_roi_s &get_vroi() { return _vroi; }
 
 	void reset_vroi() { _vroi = {}; }
@@ -374,7 +371,6 @@ private:
 	Takeoff		_takeoff;			/**< class for handling takeoff commands */
 	VtolTakeoff	_vtol_takeoff;			/**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */
 	Land		_land;			/**< class for handling land commands */
-	PrecLand	_precland;			/**< class for handling precision land commands */
 	RTL 		_rtl;				/**< class that handles RTL */
 	AdsbConflict 	_adsb_conflict;			/**< class that handles ADSB conflict avoidance */
 

src/modules/navigator/navigator_main.cpp

@@ -77,7 +77,6 @@ Navigator::Navigator() :
 	_takeoff(this),
 	_vtol_takeoff(this),
 	_land(this),
-	_precland(this),
 	_rtl(this)
 {
 	/* Create a list of our possible navigation types */
@@ -86,8 +85,7 @@ Navigator::Navigator() :
 	_navigation_mode_array[2] = &_rtl;
 	_navigation_mode_array[3] = &_takeoff;
 	_navigation_mode_array[4] = &_land;
-	_navigation_mode_array[5] = &_precland;
-	_navigation_mode_array[6] = &_vtol_takeoff;
+	_navigation_mode_array[5] = &_vtol_takeoff;
 
 	/* iterate through navigation modes and initialize _mission_item for each */
 	for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
@@ -780,12 +778,6 @@ void Navigator::run()
 			navigation_mode_new = &_land;
 			break;
 
-		case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
-			_pos_sp_triplet_published_invalid_once = false;
-			navigation_mode_new = &_precland;
-			_precland.set_mode(PrecLandMode::Required);
-			break;
-
 		case vehicle_status_s::NAVIGATION_STATE_MANUAL:
 		case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		case vehicle_status_s::NAVIGATION_STATE_ALTCTL:

src/modules/navigator/precland.cpp

@@ -1,579 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2017 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.
- *
- ****************************************************************************/
-/**
- * @file precland.cpp
- *
- * Helper class to do precision landing with a landing target
- *
- * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
- */
-
-#include "precland.h"
-#include "navigator.h"
-
-#include <string.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <math.h>
-#include <fcntl.h>
-
-#include <systemlib/err.h>
-#include <systemlib/mavlink_log.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_command.h>
-
-#define SEC2USEC 1000000.0f
-
-#define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state
-
-static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing";
-
-PrecLand::PrecLand(Navigator *navigator) :
-	MissionBlock(navigator),
-	ModuleParams(navigator)
-{
-	_handle_param_acceleration_hor = param_find("MPC_ACC_HOR");
-	_handle_param_xy_vel_cruise = param_find("MPC_XY_CRUISE");
-
-	updateParams();
-}
-
-void
-PrecLand::on_activation()
-{
-	_state = PrecLandState::Start;
-	_search_cnt = 0;
-	_last_slewrate_time = 0;
-
-	vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
-
-	if (!_map_ref.isInitialized()) {
-		_map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
-	}
-
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	pos_sp_triplet->next.valid = false;
-	pos_sp_triplet->previous.valid = false;
-
-	// Check that the current position setpoint is valid, otherwise land at current position
-	if (!pos_sp_triplet->current.valid) {
-		PX4_WARN("Reset");
-		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
-		pos_sp_triplet->current.valid = true;
-		pos_sp_triplet->current.timestamp = hrt_absolute_time();
-	}
-
-	_sp_pev = matrix::Vector2f(0, 0);
-	_sp_pev_prev = matrix::Vector2f(0, 0);
-	_last_slewrate_time = 0;
-
-	switch_to_state_start();
-
-	_is_activated = true;
-}
-
-void
-PrecLand::on_active()
-{
-	// get new target measurement
-	_target_pose_updated = _target_pose_sub.update(&_target_pose);
-
-	if (_target_pose_updated) {
-		_target_pose_valid = true;
-	}
-
-	if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) > _param_pld_btout.get()) {
-		_target_pose_valid = false;
-	}
-
-	// stop if we are landed
-	if (_navigator->get_land_detected()->landed) {
-		switch_to_state_done();
-	}
-
-	switch (_state) {
-	case PrecLandState::Start:
-		run_state_start();
-		break;
-
-	case PrecLandState::HorizontalApproach:
-		run_state_horizontal_approach();
-		break;
-
-	case PrecLandState::DescendAboveTarget:
-		run_state_descend_above_target();
-		break;
-
-	case PrecLandState::FinalApproach:
-		run_state_final_approach();
-		break;
-
-	case PrecLandState::Search:
-		run_state_search();
-		break;
-
-	case PrecLandState::Fallback:
-		run_state_fallback();
-		break;
-
-	case PrecLandState::Done:
-		// nothing to do
-		break;
-
-	default:
-		// unknown state
-		break;
-	}
-
-}
-
-void
-PrecLand::on_inactivation()
-{
-	_is_activated = false;
-}
-
-void
-PrecLand::updateParams()
-{
-	ModuleParams::updateParams();
-
-	if (_handle_param_acceleration_hor != PARAM_INVALID) {
-		param_get(_handle_param_acceleration_hor, &_param_acceleration_hor);
-	}
-
-	if (_handle_param_xy_vel_cruise != PARAM_INVALID) {
-		param_get(_handle_param_xy_vel_cruise, &_param_xy_vel_cruise);
-	}
-}
-
-void
-PrecLand::run_state_start()
-{
-	// check if target visible and go to horizontal approach
-	if (switch_to_state_horizontal_approach()) {
-		return;
-	}
-
-	if (_mode == PrecLandMode::Opportunistic) {
-		// could not see the target immediately, so just fall back to normal landing
-		switch_to_state_fallback();
-	}
-
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	float dist = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
-			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-
-	// check if we've reached the start point
-	if (dist < _navigator->get_acceptance_radius()) {
-		if (!_point_reached_time) {
-			_point_reached_time = hrt_absolute_time();
-		}
-
-		// if we don't see the target after 1 second, search for it
-		if (_param_pld_srch_tout.get() > 0) {
-
-			if (hrt_absolute_time() - _point_reached_time > 2000000) {
-				if (!switch_to_state_search()) {
-					switch_to_state_fallback();
-				}
-			}
-
-		} else {
-			switch_to_state_fallback();
-		}
-	}
-}
-
-void
-PrecLand::run_state_horizontal_approach()
-{
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	// check if target visible, if not go to start
-	if (!check_state_conditions(PrecLandState::HorizontalApproach)) {
-		PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
-
-		// Stay at current position for searching for the landing target
-		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
-
-		if (!switch_to_state_start()) {
-			switch_to_state_fallback();
-		}
-
-		return;
-	}
-
-	if (check_state_conditions(PrecLandState::DescendAboveTarget)) {
-		if (!_point_reached_time) {
-			_point_reached_time = hrt_absolute_time();
-		}
-
-		if (hrt_absolute_time() - _point_reached_time > 2000000) {
-			// if close enough for descent above target go to descend above target
-			if (switch_to_state_descend_above_target()) {
-
-				return;
-			}
-		}
-
-	}
-
-	float x = _target_pose.x_abs;
-	float y = _target_pose.y_abs;
-
-	slewrate(x, y);
-
-	// XXX need to transform to GPS coords because mc_pos_control only looks at that
-	_map_ref.reproject(x, y, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
-
-	pos_sp_triplet->current.alt = _approach_alt;
-	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-
-	_navigator->set_position_setpoint_triplet_updated();
-}
-
-void
-PrecLand::run_state_descend_above_target()
-{
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	// check if target visible
-	if (!check_state_conditions(PrecLandState::DescendAboveTarget)) {
-		if (!switch_to_state_final_approach()) {
-			PX4_WARN("%s, state: %i", LOST_TARGET_ERROR_MESSAGE, (int) _state);
-
-			// Stay at current position for searching for the target
-			pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-			pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-			pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
-
-			if (!switch_to_state_start()) {
-				switch_to_state_fallback();
-			}
-		}
-
-		return;
-	}
-
-	// XXX need to transform to GPS coords because mc_pos_control only looks at that
-	_map_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
-
-	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
-
-	_navigator->set_position_setpoint_triplet_updated();
-}
-
-void
-PrecLand::run_state_final_approach()
-{
-	// nothing to do, will land
-}
-
-void
-PrecLand::run_state_search()
-{
-	// check if we can see the target
-	if (check_state_conditions(PrecLandState::HorizontalApproach)) {
-		if (!_target_acquired_time) {
-			// target just became visible. Stop climbing, but give it some margin so we don't stop too apruptly
-			_target_acquired_time = hrt_absolute_time();
-			position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-			float new_alt = _navigator->get_global_position()->alt + 1.0f;
-			pos_sp_triplet->current.alt = new_alt < pos_sp_triplet->current.alt ? new_alt : pos_sp_triplet->current.alt;
-			_navigator->set_position_setpoint_triplet_updated();
-		}
-
-	}
-
-	// stay at that height for a second to allow the vehicle to settle
-	if (_target_acquired_time && (hrt_absolute_time() - _target_acquired_time) > 1000000) {
-		// try to switch to horizontal approach
-		if (switch_to_state_horizontal_approach()) {
-			return;
-		}
-	}
-
-	// check if search timed out and go to fallback
-	if (hrt_absolute_time() - _state_start_time > _param_pld_srch_tout.get()*SEC2USEC) {
-		PX4_WARN("Search timed out");
-
-		switch_to_state_fallback();
-	}
-}
-
-void
-PrecLand::run_state_fallback()
-{
-	// nothing to do, will land
-}
-
-bool
-PrecLand::switch_to_state_start()
-{
-	if (check_state_conditions(PrecLandState::Start)) {
-		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-		_navigator->set_position_setpoint_triplet_updated();
-		_search_cnt++;
-
-		_point_reached_time = 0;
-
-		_state = PrecLandState::Start;
-		_state_start_time = hrt_absolute_time();
-		return true;
-	}
-
-	return false;
-}
-
-bool
-PrecLand::switch_to_state_horizontal_approach()
-{
-	if (check_state_conditions(PrecLandState::HorizontalApproach)) {
-		print_state_switch_message("horizontal approach");
-		_approach_alt = _navigator->get_global_position()->alt;
-
-		_point_reached_time = 0;
-
-		_state = PrecLandState::HorizontalApproach;
-		_state_start_time = hrt_absolute_time();
-		return true;
-	}
-
-	return false;
-}
-
-bool
-PrecLand::switch_to_state_descend_above_target()
-{
-	if (check_state_conditions(PrecLandState::DescendAboveTarget)) {
-		print_state_switch_message("descend");
-		_state = PrecLandState::DescendAboveTarget;
-		_state_start_time = hrt_absolute_time();
-		return true;
-	}
-
-	return false;
-}
-
-bool
-PrecLand::switch_to_state_final_approach()
-{
-	if (check_state_conditions(PrecLandState::FinalApproach)) {
-		print_state_switch_message("final approach");
-		_state = PrecLandState::FinalApproach;
-		_state_start_time = hrt_absolute_time();
-		return true;
-	}
-
-	return false;
-}
-
-bool
-PrecLand::switch_to_state_search()
-{
-	PX4_INFO("Climbing to search altitude.");
-	vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
-
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	pos_sp_triplet->current.alt = vehicle_local_position->ref_alt + _param_pld_srch_alt.get();
-	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-	_navigator->set_position_setpoint_triplet_updated();
-
-	_target_acquired_time = 0;
-
-	_state = PrecLandState::Search;
-	_state_start_time = hrt_absolute_time();
-	return true;
-}
-
-bool
-PrecLand::switch_to_state_fallback()
-{
-	print_state_switch_message("fallback");
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-	pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-	pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
-	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
-	_navigator->set_position_setpoint_triplet_updated();
-
-	_state = PrecLandState::Fallback;
-	_state_start_time = hrt_absolute_time();
-	return true;
-}
-
-bool
-PrecLand::switch_to_state_done()
-{
-	_state = PrecLandState::Done;
-	_state_start_time = hrt_absolute_time();
-	return true;
-}
-
-void PrecLand::print_state_switch_message(const char *state_name)
-{
-	PX4_INFO("Precland: switching to %s", state_name);
-}
-
-bool PrecLand::check_state_conditions(PrecLandState state)
-{
-	vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
-
-	switch (state) {
-	case PrecLandState::Start:
-		return _search_cnt <= _param_pld_max_srch.get();
-
-	case PrecLandState::HorizontalApproach:
-
-		// if we're already in this state, only want to make it invalid if we reached the target but can't see it anymore
-		if (_state == PrecLandState::HorizontalApproach) {
-			if (fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get()
-			    && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get()) {
-				// we've reached the position where we last saw the target. If we don't see it now, we need to do something
-				return _target_pose_valid && _target_pose.abs_pos_valid;
-
-			} else {
-				// We've seen the target sometime during horizontal approach.
-				// Even if we don't see it as we're moving towards it, continue approaching last known location
-				return true;
-			}
-		}
-
-		// If we're trying to switch to this state, the target needs to be visible
-		return _target_pose_updated && _target_pose_valid && _target_pose.abs_pos_valid;
-
-	case PrecLandState::DescendAboveTarget:
-
-		// if we're already in this state, only leave it if target becomes unusable, don't care about horizontall offset to target
-		if (_state == PrecLandState::DescendAboveTarget) {
-			// if we're close to the ground, we're more critical of target timeouts so we quickly go into descend
-			if (check_state_conditions(PrecLandState::FinalApproach)) {
-				return hrt_absolute_time() - _target_pose.timestamp < 500000; // 0.5s
-
-			} else {
-				return _target_pose_valid && _target_pose.abs_pos_valid;
-			}
-
-		} else {
-			// if not already in this state, need to be above target to enter it
-			return _target_pose_updated && _target_pose.abs_pos_valid
-			       && fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get()
-			       && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get();
-		}
-
-	case PrecLandState::FinalApproach:
-		return _target_pose_valid && _target_pose.abs_pos_valid
-		       && (_target_pose.z_abs - vehicle_local_position->z) < _param_pld_fappr_alt.get();
-
-	case PrecLandState::Search:
-		return true;
-
-	case PrecLandState::Fallback:
-		return true;
-
-	default:
-		return false;
-	}
-}
-
-void PrecLand::slewrate(float &sp_x, float &sp_y)
-{
-	matrix::Vector2f sp_curr(sp_x, sp_y);
-	uint64_t now = hrt_absolute_time();
-
-	float dt = (now - _last_slewrate_time);
-
-	if (dt < 1) {
-		// bad dt, can't divide by it
-		return;
-	}
-
-	dt /= SEC2USEC;
-
-	if (!_last_slewrate_time) {
-		// running the first time since switching to precland
-
-		// assume dt will be about 50000us
-		dt = 50000 / SEC2USEC;
-
-		// set a best guess for previous setpoints for smooth transition
-		_sp_pev = _map_ref.project(_navigator->get_position_setpoint_triplet()->current.lat,
-					   _navigator->get_position_setpoint_triplet()->current.lon);
-		_sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt;
-		_sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt;
-	}
-
-	_last_slewrate_time = now;
-
-	// limit the setpoint speed to the maximum cruise speed
-	matrix::Vector2f sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
-
-	if (sp_vel.length() > _param_xy_vel_cruise) {
-		sp_vel = sp_vel.normalized() * _param_xy_vel_cruise;
-		sp_curr = _sp_pev + sp_vel * dt;
-	}
-
-	// limit the setpoint acceleration to the maximum acceleration
-	matrix::Vector2f sp_acc = (sp_curr - _sp_pev * 2 + _sp_pev_prev) / (dt * dt); // acceleration of the setpoints
-
-	if (sp_acc.length() > _param_acceleration_hor) {
-		sp_acc = sp_acc.normalized() * _param_acceleration_hor;
-		sp_curr = _sp_pev * 2 - _sp_pev_prev + sp_acc * (dt * dt);
-	}
-
-	// limit the setpoint speed such that we can stop at the setpoint given the maximum acceleration/deceleration
-	float max_spd = sqrtf(_param_acceleration_hor * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x,
-			      sp_y))).length());
-	sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
-
-	if (sp_vel.length() > max_spd) {
-		sp_vel = sp_vel.normalized() * max_spd;
-		sp_curr = _sp_pev + sp_vel * dt;
-	}
-
-	_sp_pev_prev = _sp_pev;
-	_sp_pev = sp_curr;
-
-	sp_x = sp_curr(0);
-	sp_y = sp_curr(1);
-}

src/modules/navigator/precland.h

@@ -1,150 +0,0 @@
-/***************************************************************************
- *
- *   Copyright (c) 2017 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.
- *
- ****************************************************************************/
-/**
- * @file precland.h
- *
- * Helper class to do precision landing with a landing target
- *
- * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
- */
-
-#pragma once
-
-#include <matrix/math.hpp>
-#include <lib/geo/geo.h>
-#include <px4_platform_common/module_params.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/landing_target_pose.h>
-
-#include "navigator_mode.h"
-#include "mission_block.h"
-
-enum class PrecLandState {
-	Start, // Starting state
-	HorizontalApproach, // Positioning over landing target while maintaining altitude
-	DescendAboveTarget, // Stay over landing target while descending
-	FinalApproach, // Final landing approach, even without landing target
-	Search, // Search for landing target
-	Fallback, // Fallback landing method
-	Done // Done landing
-};
-
-enum class PrecLandMode {
-	Opportunistic = 1, // only do precision landing if landing target visible at the beginning
-	Required = 2 // try to find landing target if not visible at the beginning
-};
-
-class PrecLand : public MissionBlock, public ModuleParams
-{
-public:
-	PrecLand(Navigator *navigator);
-	~PrecLand() override = default;
-
-	void on_activation() override;
-	void on_active() override;
-	void on_inactivation() override;
-
-	void set_mode(PrecLandMode mode) { _mode = mode; };
-
-	PrecLandMode get_mode() { return _mode; };
-
-	bool is_activated() { return _is_activated; };
-
-private:
-
-	void updateParams() override;
-
-	// run the control loop for each state
-	void run_state_start();
-	void run_state_horizontal_approach();
-	void run_state_descend_above_target();
-	void run_state_final_approach();
-	void run_state_search();
-	void run_state_fallback();
-
-	// attempt to switch to a different state. Returns true if state change was successful, false otherwise
-	bool switch_to_state_start();
-	bool switch_to_state_horizontal_approach();
-	bool switch_to_state_descend_above_target();
-	bool switch_to_state_final_approach();
-	bool switch_to_state_search();
-	bool switch_to_state_fallback();
-	bool switch_to_state_done();
-
-	void print_state_switch_message(const char *state_name);
-
-	// check if a given state could be changed into. Return true if possible to transition to state, false otherwise
-	bool check_state_conditions(PrecLandState state);
-	void slewrate(float &sp_x, float &sp_y);
-
-	landing_target_pose_s _target_pose{}; /**< precision landing target position */
-
-	uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)};
-	bool _target_pose_valid{false}; /**< whether we have received a landing target position message */
-	bool _target_pose_updated{false}; /**< wether the landing target position message is updated */
-
-	MapProjection _map_ref{}; /**< class for local/global projections */
-
-	uint64_t _state_start_time{0}; /**< time when we entered current state */
-	uint64_t _last_slewrate_time{0}; /**< time when we last limited setpoint changes */
-	uint64_t _target_acquired_time{0}; /**< time when we first saw the landing target during search */
-	uint64_t _point_reached_time{0}; /**< time when we reached a setpoint */
-
-	int _search_cnt{0}; /**< counter of how many times we had to search for the landing target */
-	float _approach_alt{0.0f}; /**< altitude at which to stay during horizontal approach */
-
-	matrix::Vector2f _sp_pev;
-	matrix::Vector2f _sp_pev_prev;
-
-	PrecLandState _state{PrecLandState::Start};
-
-	PrecLandMode _mode{PrecLandMode::Opportunistic};
-
-	bool _is_activated {false}; /**< indicates if precland is activated */
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::PLD_BTOUT>) _param_pld_btout,
-		(ParamFloat<px4::params::PLD_HACC_RAD>) _param_pld_hacc_rad,
-		(ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_pld_fappr_alt,
-		(ParamFloat<px4::params::PLD_SRCH_ALT>) _param_pld_srch_alt,
-		(ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_pld_srch_tout,
-		(ParamInt<px4::params::PLD_MAX_SRCH>) _param_pld_max_srch
-	)
-
-	// non-navigator parameters
-	param_t	_handle_param_acceleration_hor{PARAM_INVALID};
-	param_t	_handle_param_xy_vel_cruise{PARAM_INVALID};
-	float	_param_acceleration_hor{0.0f};
-	float	_param_xy_vel_cruise{0.0f};
-
-};

src/modules/navigator/rtl.cpp

@@ -69,9 +69,7 @@ RTL::RTL(Navigator *navigator) :
 
 void RTL::on_inactivation()
 {
-	if (_navigator->get_precland()->is_activated()) {
-		_navigator->get_precland()->on_inactivation();
-	}
+
 }
 
 void RTL::on_inactive()
@@ -314,13 +312,6 @@ void RTL::on_active()
 		set_rtl_item();
 	}
 
-	if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) {
-		_navigator->get_precland()->on_active();
-
-	} else if (_navigator->get_precland()->is_activated()) {
-		_navigator->get_precland()->on_inactivation();
-	}
-
 	// Limit rtl time calculation to 1Hz
 	if ((hrt_absolute_time() - _destination_check_time) > 1_s) {
 		_destination_check_time = hrt_absolute_time();
@@ -589,16 +580,6 @@ void RTL::set_rtl_item()
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;
 			_mission_item.origin = ORIGIN_ONBOARD;
-			_mission_item.land_precision = _param_rtl_pld_md.get();
-
-			if (_mission_item.land_precision == 1) {
-				_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
-				_navigator->get_precland()->on_activation();
-
-			} else if (_mission_item.land_precision == 2) {
-				_navigator->get_precland()->set_mode(PrecLandMode::Required);
-				_navigator->get_precland()->on_activation();
-			}
 
 			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t");
 			events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination");

src/modules/navigator/rtl.h

@@ -170,7 +170,6 @@ private:
 		(ParamFloat<px4::params::RTL_MIN_DIST>)    _param_rtl_min_dist,
 		(ParamInt<px4::params::RTL_TYPE>)          _param_rtl_type,
 		(ParamInt<px4::params::RTL_CONE_ANG>)      _param_rtl_cone_half_angle_deg,
-		(ParamInt<px4::params::RTL_PLD_MD>)        _param_rtl_pld_md,
 		(ParamFloat<px4::params::RTL_LOITER_RAD>)  _param_rtl_loiter_rad,
 		(ParamInt<px4::params::RTL_HDG_MD>)        _param_rtl_hdg_md,
 		(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,

src/modules/navigator/rtl_params.c

@@ -135,6 +135,8 @@ PARAM_DEFINE_INT32(RTL_TYPE, 0);
  */
 PARAM_DEFINE_INT32(RTL_CONE_ANG, 45);
 
+
+// TODO: Rename to NAV_PLD_MD if it also applies to landing rather than only RTL
 /**
  * RTL precision land mode
  *