change land setpoint

The current name is confusing as it suggest an action,
which is switching to a new state. In reality these
functions first perform a check and then possibly
reject the state switch.

ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock

@@ -33,6 +33,8 @@ param set-default PWM_MAIN_FUNC4 104
 param set-default LTEST_MODE 1
 param set-default PLD_HACC_RAD 0.1
 param set-default RTL_PLD_MD 2
+param set-default MPC_LAND_SPEED 0.2
+param set-default PLD_SRCH_TOUT 30
 
 # Start up Landing Target Estimator module
 landing_target_estimator start

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_TRANSITON_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 precland_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
+	AutoPrecisionLanding
 	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,24 @@ void FlightModeManager::start_flight_task()
 	bool task_failure = false;
 	bool should_disable_task = true;
 
+	// land/rtl mode is precland
+	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;
+
+
+	// Mission item precland
+	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;
+
+	// When issuing the auto:precland mode
+	const bool precland_flight_mode = _vehicle_status_sub.get().nav_state ==
+					  vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
+
+	// PX4_INFO("start_flight_task: %d %d %d", land_should_be_precland, precland_mission_item_active, precland_flight_mode);
+
 	// 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 +183,14 @@ 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::AutoPrecisionLanding) != FlightTaskError::NoError) {
+			task_failure = true;
+		}
+
 	} else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
 
 		// Emergency descend
@@ -419,6 +446,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 
 	// activation failed
 	if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) {
+		PX4_INFO("activate failed");
 		_current_task.task->~FlightTask();
 		_current_task.task = nullptr;
 		_current_task.index = FlightTaskIndex::None;

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/Auto/FlightTaskAuto.cpp

@@ -104,6 +104,10 @@ bool FlightTaskAuto::updateInitialize()
 
 bool FlightTaskAuto::update()
 {
+	if (_type_previous != _type) {
+		PX4_INFO("_type: %d", (int)_type);
+	}
+
 	bool ret = FlightTask::update();
 	// always reset constraints because they might change depending on the type
 	_setDefaultConstraints();
@@ -215,6 +219,7 @@ void FlightTaskAuto::overrideCruiseSpeed(const float cruise_speed_m_s)
 
 void FlightTaskAuto::_prepareLandSetpoints()
 {
+	// PX4_INFO("FlightTaskAuto::_prepareLandSetpoints");
 	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
 
 	// Slow down automatic descend close to ground
@@ -235,7 +240,9 @@ void FlightTaskAuto::_prepareLandSetpoints()
 	}
 
 	// Update xy-position in case of landing position changes (etc. precision landing)
-	_land_position = Vector3f(_target(0), _target(1), NAN);
+	_position_setpoint(2) = NAN;
+	_land_position = _position_setpoint;
+	// _land_position = Vector3f(_target(0), _target(1), NAN);
 
 	// User input assisted landing
 	if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {

src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/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(FlightTaskAutoPrecisionLanding
+FlightTaskAutoPrecisionLanding.cpp
+)
+
+target_link_libraries(FlightTaskAutoPrecisionLanding PUBLIC FlightTaskAuto)
+target_include_directories(FlightTaskAutoPrecisionLanding PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp

@@ -0,0 +1,361 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 FlightTaskAutoPrecisionLanding.cpp
+ *
+ * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
+ */
+
+#include "FlightTaskAutoPrecisionLanding.hpp"
+
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <fcntl.h>
+
+const char* STATE_STRINGS[] = {"Idle", "Start", "HorizontalApproach", "DescendAboveTarget", "Search", "NormalLand", "Finished"};
+static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing";
+
+static constexpr int32_t RTL_PREC_LAND_OPPORTUNISTIC = 1;
+static constexpr int32_t RTL_PREC_LAND_REQUIRED = 2;
+
+
+bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint)
+{
+	PX4_INFO("FlightTaskAutoPrecisionLanding::activate");
+	bool ret = FlightTaskAuto::activate(last_setpoint);
+
+	_search_count = 0;
+	_last_slewrate_time = 0;
+	_landing_target_valid = false;
+	_state = PrecLandState::Idle;
+	_fix_this_activate_update_loop = true;
+
+	_sub_vehicle_status.update();
+
+	uint8_t nav_state = _sub_vehicle_status.get().nav_state;
+
+	// TODO: use mode information to determine behaviors?
+	// Regular precision land mode and mission precision land mode have the same behavior
+	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
+		_mode = PrecLandMode::RegularPrecisionLand;
+		PX4_INFO("mode: %d", (int)_mode);
+
+	} else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND && _param_rtl_pld_md.get() >= RTL_PREC_LAND_OPPORTUNISTIC) {
+		_mode = PrecLandMode::RegularLandOpportunistic;
+		PX4_INFO("mode: %d", (int)_mode);
+
+	} else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && _param_rtl_pld_md.get() == RTL_PREC_LAND_OPPORTUNISTIC) {
+		_mode = PrecLandMode::ReturnToLaunchOpportunistic;
+		PX4_INFO("mode: %d", (int)_mode);
+
+	} else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && _param_rtl_pld_md.get() == RTL_PREC_LAND_REQUIRED) {
+		_mode = PrecLandMode::ReturnToLaunchRequired;
+		PX4_INFO("mode: %d", (int)_mode);
+	}
+
+	return ret;
+}
+
+bool FlightTaskAutoPrecisionLanding::updateInitialize()
+{
+	bool ret = FlightTaskAuto::updateInitialize();
+
+	_sub_home_position.update();
+	_sub_vehicle_status.update();
+	_landing_target_pose_sub.update();
+
+
+	uint8_t nav_state = _sub_vehicle_status.get().nav_state;
+
+	if (_nav_state != nav_state) {
+		PX4_INFO("nav_state: %u", nav_state);
+		_nav_state = nav_state;
+	}
+
+	// require valid position
+	ret = ret && _position.isAllFinite() && _velocity.isAllFinite();
+
+	return ret;
+}
+
+bool FlightTaskAutoPrecisionLanding::update()
+{
+	bool ret = FlightTaskAuto::update();
+
+	_landing_target_valid = (hrt_elapsed_time(&_landing_target_pose_sub.get().timestamp) / 1e6f) <= _param_pld_btout.get();
+
+	switch (_state) {
+
+	case PrecLandState::Idle:
+		run_state_idle();
+		break;
+
+	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::Search:
+		run_state_search();
+		break;
+
+	case PrecLandState::NormalLand:
+		run_state_normal_land();
+		break;
+
+	default:
+		break;
+	}
+
+	precision_landing_status_s status = {};
+	status.timestamp = hrt_absolute_time();
+	status.precland_state = (uint8_t) _state;
+	_precision_landing_status_pub.publish(status);
+
+	return ret;
+}
+
+void FlightTaskAutoPrecisionLanding::switch_state(PrecLandState state)
+{
+	state_on_exit(_state);
+	_state = state;
+	state_on_enter(_state);
+	PX4_INFO("Switching to %s", STATE_STRINGS[static_cast<int>(state)]);
+}
+
+void FlightTaskAutoPrecisionLanding::state_on_enter(PrecLandState state)
+{
+	switch (state) {
+	case PrecLandState::Search:
+		_search_count++;
+		_search_start_time = hrt_absolute_time();
+		break;
+	// case PrecLandState::HorizontalApproach:
+	// 	_horizontal_approach_alt = _position(2);
+	// 	break;
+	default:
+		break;
+	}
+}
+
+void FlightTaskAutoPrecisionLanding::state_on_exit(PrecLandState state)
+{
+	// TODO:
+	(void)state;
+}
+
+void FlightTaskAutoPrecisionLanding::run_state_idle()
+{
+	bool is_rtl = _mode == PrecLandMode::ReturnToLaunchRequired || _mode == PrecLandMode::ReturnToLaunchOpportunistic;
+	bool within_home_acceptance = Vector2f(Vector2f(_sub_home_position.get().x,
+				 _sub_home_position.get().y) - _position.xy()).norm() <= _target_acceptance_radius;
+
+	// We must wait for RTL to get home before starting
+	if (is_rtl && !within_home_acceptance) {
+		return;
+	} else if (is_rtl && within_home_acceptance) {
+		PX4_INFO("RTL reached home point");
+	}
+
+	switch_state(PrecLandState::Start);
+}
+
+void FlightTaskAutoPrecisionLanding::run_state_start()
+{
+	if (_landing_target_valid) {
+		switch_state(PrecLandState::HorizontalApproach);
+
+	} else {
+
+		PX4_INFO("Target not seen");
+
+		bool required = _mode == PrecLandMode::RegularPrecisionLand ||
+								 _mode == PrecLandMode::MissionPrecisionLand ||
+								 _mode == PrecLandMode::ReturnToLaunchRequired;
+
+		if (required) {
+			switch_state(PrecLandState::Search);
+
+		} else {
+			switch_state(PrecLandState::NormalLand);
+		}
+	}
+}
+
+void FlightTaskAutoPrecisionLanding::run_state_normal_land()
+{
+	if (_landing_target_valid) {
+		switch_state(PrecLandState::HorizontalApproach);
+		return;
+	}
+}
+
+void FlightTaskAutoPrecisionLanding::run_state_search()
+{
+	_target(2) = _position_setpoint(2) = _sub_home_position.get().z - _param_pld_srch_alt.get();
+
+	// If target is seen run horizontal approach
+	if (_landing_target_valid) {
+		switch_state(PrecLandState::HorizontalApproach);
+		return;
+	}
+
+	// If we exceed PLD_SRCH_TOUT fallback to normal land
+	if ((hrt_elapsed_time(&_search_start_time) / 1e6f) >= _param_pld_srch_tout.get()) {
+		PX4_INFO("Search timed out");
+		switch_state(PrecLandState::NormalLand);
+	}
+
+	// Check if we have exceeded the maximum number of search attempts
+	if (_search_count > _param_pld_max_srch.get()) {
+		PX4_INFO("Maximum search attempts exceeded (%d)", _search_count);
+		switch_state(PrecLandState::NormalLand);
+	}
+}
+
+void FlightTaskAutoPrecisionLanding::run_state_horizontal_approach()
+{
+	if (!_landing_target_valid) {
+		switch_state(PrecLandState::Search);
+		return;
+	}
+
+	float x = _landing_target_pose_sub.get().x_abs;
+	float y = _landing_target_pose_sub.get().y_abs;
+	slewrate(x, y); // TODO: Replace this with PX4's AlphaFilter
+
+	// Fly to target XY position, maintain current altitude
+	_target(0) = _position_setpoint(0) = x;
+	_target(1) = _position_setpoint(1) = y;
+
+	// Check if it's time to descend
+	Vector2f target_position_vector = Vector2f(_landing_target_pose_sub.get().x_abs,_landing_target_pose_sub.get().y_abs);
+	Vector2f position_delta_vector = Vector2f(target_position_vector - _position.xy());
+
+	if (position_delta_vector.norm() <= _param_pld_hacc_rad.get() ) {
+		switch_state(PrecLandState::DescendAboveTarget);
+	}
+}
+
+void FlightTaskAutoPrecisionLanding::run_state_descend_above_target()
+{
+	if (!_landing_target_valid) {
+		switch_state(PrecLandState::Search);
+		return;
+	}
+
+	// FlightTask::update() function is preparing our land setpoints.
+	_target(0) = _position_setpoint(0) = _landing_target_pose_sub.get().x_abs;
+	_target(1) = _position_setpoint(1) = _landing_target_pose_sub.get().y_abs;
+
+	// Check if we're within our final approach altitude
+	if ((_landing_target_pose_sub.get().z_abs - _position(2)) < _param_pld_fappr_alt.get()) {
+		switch_state(PrecLandState::Finished);
+	}
+}
+
+void FlightTaskAutoPrecisionLanding::run_state_finished()
+{
+	// Nothing to do.
+}
+
+bool FlightTaskAutoPrecisionLanding::hor_acc_radius_check()
+{
+	return Vector2f(Vector2f(_landing_target_pose_sub.get().x_abs,
+				 _landing_target_pose_sub.get().y_abs) - _position.xy()).norm() <= _param_pld_hacc_rad.get();
+}
+
+void FlightTaskAutoPrecisionLanding::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 /= 1000000;
+
+	if (_last_slewrate_time == 0) {
+		// running the first time since switching to precland
+		_sp_pev_prev(0) = sp_x;
+		_sp_pev_prev(1) = sp_y;
+		_sp_pev(0) = sp_x;
+		_sp_pev(1) = sp_y;
+	}
+
+	_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.get()) {
+		sp_vel = sp_vel.normalized() * _param_xy_vel_cruise.get();
+		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.get()) {
+		sp_acc = sp_acc.normalized() * _param_acceleration_hor.get();
+		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.get() * ((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/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 FlightTaskAutoPrecisionLanding.hpp
+ *
+ * Flight task for better precision landing
+ *
+ * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
+ */
+
+#pragma once
+
+#include "FlightTaskAuto.hpp"
+
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <px4_platform_common/module_params.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/landing_target_pose.h>
+#include <uORB/topics/precision_landing_status.h>
+
+enum class PrecLandState {
+	Idle,
+	Start,
+	HorizontalApproach,
+	DescendAboveTarget,
+	Search,
+	NormalLand,
+	Finished,
+};
+
+enum class PrecLandMode {
+	RegularPrecisionLand,
+	RegularLandOpportunistic,
+	ReturnToLaunchOpportunistic,
+	ReturnToLaunchRequired,
+	MissionPrecisionLand
+};
+
+class FlightTaskAutoPrecisionLanding : public FlightTaskAuto
+{
+public:
+	FlightTaskAutoPrecisionLanding() = default;
+	virtual ~FlightTaskAutoPrecisionLanding() = default;
+
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+
+	bool update() override;
+
+	bool updateInitialize() override;
+
+private:
+
+	// Jake:
+	void run_state_idle();
+	void run_state_start();
+	void run_state_horizontal_approach();
+	void run_state_descend_above_target();
+	void run_state_search();
+	void run_state_normal_land();
+	void run_state_finished();
+
+	void switch_state(PrecLandState state);
+	void state_on_enter(PrecLandState state);
+	void state_on_exit(PrecLandState state);
+
+	void slewrate(float &sp_x, float &sp_y);
+	bool hor_acc_radius_check();
+
+	uORB::SubscriptionData<home_position_s>			_sub_home_position {ORB_ID(home_position)};
+	uORB::SubscriptionData<vehicle_status_s>		_sub_vehicle_status {ORB_ID(vehicle_status)};
+	uORB::SubscriptionData<landing_target_pose_s>	_landing_target_pose_sub {ORB_ID(landing_target_pose)};
+
+	uORB::PublicationMulti<precision_landing_status_s> _precision_landing_status_pub {ORB_ID(precision_landing_status)};
+
+	float _horizontal_approach_alt {};
+
+	// FIX THIS
+	// INFO  [FlightTaskAutoPrecisionLanding] Switching to Start
+	// INFO  [FlightTaskAutoPrecisionLanding] FlightTaskAutoPrecisionLanding::activate
+	// INFO  [FlightTaskAutoPrecisionLanding] Switching to Start
+	// INFO  [FlightTaskAutoPrecisionLanding] FlightTaskAutoPrecisionLanding::activate
+	// INFO  [FlightTaskAutoPrecisionLanding] Switching to Start
+	// INFO  [FlightTaskAutoPrecisionLanding] FlightTaskAutoPrecisionLanding::activate
+	// INFO  [FlightTaskAutoPrecisionLanding] Switching to Start
+	bool _fix_this_activate_update_loop {true};
+
+	uint8_t _nav_state {0};
+
+	bool _landing_target_valid {false}; /**< whether we have received a landing target position message */
+
+	hrt_abstime _search_start_time {0};
+
+	uint64_t _last_slewrate_time {0}; /**< time when we last limited setpoint changes */
+	int _search_count {0};
+
+	matrix::Vector2f _sp_pev;
+	matrix::Vector2f _sp_pev_prev;
+
+	PrecLandMode  _mode  {PrecLandMode::RegularPrecisionLand};
+	PrecLandState _state {PrecLandState::Idle};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+		(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed, ///< velocity for controlled descend
+		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_acceleration_hor,
+		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_xy_vel_cruise,
+		(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,
+		(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md
+	)
+
+};

src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/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

@@ -96,6 +96,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/land.cpp

@@ -49,6 +49,7 @@ Land::Land(Navigator *navigator) :
 void
 Land::on_activation()
 {
+	PX4_INFO("Land::on_activation");
 	/* set current mission item to Land */
 	set_land_item(&_mission_item);
 	_navigator->get_mission_result()->finished = false;

src/modules/navigator/loiter.cpp

@@ -57,6 +57,8 @@ Loiter::on_inactive()
 void
 Loiter::on_activation()
 {
+	PX4_INFO("Loiter::on_activation");
+
 	if (_navigator->get_reposition_triplet()->current.valid) {
 		reposition();
 

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_TRANSITON_AFTER_TAKEOFF;
+						new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_TRANSITON_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_TRANSITON_AFTER_TAKEOFF
-				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+				    && _work_item_type == navigator_mission_item_s::WORK_ITEM_TYPE_TRANSITON_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_TRANSITON_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_TRANSITON_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,21 @@ 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();
-
+						PX4_INFO("new work item type is PRECISION_LAND");
+						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();
-
+						PX4_INFO("we just moved to the landing waypoint, now descend");
+						new_work_item_type = navigator_mission_item_s::WORK_ITEM_TYPE_PRECISION_LAND;
 					}
 
 				}
@@ -1062,8 +1035,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 +1044,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 +1054,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 +1118,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 +1128,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 +1191,7 @@ Mission::set_mission_items()
 		}
 	}
 
-	publish_navigator_mission_item(); // for logging
+	publish_navigator_mission_item();
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
@@ -1465,7 +1438,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 +1882,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

@@ -281,16 +281,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_TRANSITON_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"
@@ -85,7 +84,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
 {
@@ -173,8 +172,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 = {}; }
@@ -393,7 +390,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 */
 
 	NavigatorMode *_navigation_mode{nullptr};	/**< abstract pointer to current navigation mode class */

src/modules/navigator/navigator_main.cpp

@@ -76,7 +76,6 @@ Navigator::Navigator() :
 	_takeoff(this),
 	_vtol_takeoff(this),
 	_land(this),
-	_precland(this),
 	_rtl(this)
 {
 	/* Create a list of our possible navigation types */
@@ -85,8 +84,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++) {
@@ -771,14 +769,9 @@ void Navigator::run()
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
-			_pos_sp_triplet_published_invalid_once = false;
-			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);
+			navigation_mode_new = &_land;
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_MANUAL:

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()
@@ -251,6 +249,8 @@ void RTL::find_RTL_destination()
 
 void RTL::on_activation()
 {
+	PX4_INFO("RTL::on_activation");
+
 	_rtl_state = RTL_STATE_NONE;
 
 	// if a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode
@@ -314,13 +314,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 +582,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
  *