convert precland to flight task

msg/CMakeLists.txt

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

msg/precision_landing_status.msg

@@ -0,0 +1,10 @@
+# ORBIT_YAW_BEHAVIOUR
+uint8 PRECLAND_STATE_START = 0
+uint8 PRECLAND_STATE_HORIZONTAL_APPROACH = 1
+uint8 PRECLAND_STATE_DESCEND_ABOVE_TARGET = 2
+uint8 PRECLAND_STATE_FINAL_APPROACH = 3
+uint8 PRECLAND_STATE_SEARCH = 4
+uint8 PRECLAND_STATE_FALLBACK = 5
+
+uint64 timestamp 	# time since system start (microseconds)
+uint8 precland_state    # Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. [m]

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

@@ -164,6 +164,16 @@ void FlightModeManager::start_flight_task()
 			task_failure = true;
 		}
 
+		// Take-over landing from navigator if precision landing is enabled
+
+	} else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
+		   && _param_rtl_pld_md.get() > 0) {
+		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

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -157,6 +157,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/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 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
@@ -31,101 +31,51 @@
  *
  ****************************************************************************/
 /**
- * @file precland.cpp
+ * @file FlightTaskAutoPrecisionLanding.cpp
- *
- * Helper class to do precision landing with a landing target
  *
  * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
  */
 
-#include "precland.h"
+#include "FlightTaskAutoPrecisionLanding.hpp"
-#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) :
+bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint)
-	MissionBlock(navigator),
-	ModuleParams(navigator)
 {
-	_handle_param_acceleration_hor = param_find("MPC_ACC_HOR");
+	bool ret = FlightTask::activate(last_setpoint);
-	_handle_param_xy_vel_cruise = param_find("MPC_XY_CRUISE");
 
-	updateParams();
+	// This looks wrong at first, but is a mean little trick to avoid discontinuous setpoints.
-}
+	// When this flight task is activated, _target might be outdated until the next navigator triplet
+	// comes in. Until then pretend that the current position setpoint is the navigator's setpoint:
+	_target = _position_setpoint;
 
-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();
+	_sp_pev = matrix::Vector2f(last_setpoint.x, last_setpoint.y);
-
+	_sp_pev_prev = matrix::Vector2f(last_setpoint.x, last_setpoint.y);
-	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;
+	return ret;
 }
 
-void
+bool FlightTaskAutoPrecisionLanding::update()
-PrecLand::on_active()
 {
-	// get new target measurement
+	bool ret = FlightTaskAuto::update();
-	_target_pose_updated = _target_pose_sub.update(&_target_pose);
 
-	if (_target_pose_updated) {
+	// Fetch uorb
-		_target_pose_valid = true;
+	if (_target_pose_sub.updated()) {
+		_target_pose_sub.copy(&_target_pose);
 	}
 
-	if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) > _param_pld_btout.get()) {
+	// target pose can become invalid when the message timed out
-		_target_pose_valid = false;
+	_target_pose_valid = (hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) <= _param_pld_btout.get();
-	}
-
-	// stop if we are landed
-	if (_navigator->get_land_detected()->landed) {
-		switch_to_state_done();
-	}
 
 	switch (_state) {
 	case PrecLandState::Start:
@@ -152,88 +102,66 @@ PrecLand::on_active()
 		run_state_fallback();
 		break;
 
-	case PrecLandState::Done:
-		// nothing to do
-		break;
-
 	default:
 		// unknown state
 		break;
 	}
 
+	// Publish status message for debugging
+	precision_landing_status_s precision_landing_status{};
+	precision_landing_status.timestamp = hrt_absolute_time();
+	precision_landing_status.precland_state = (uint8_t) _state;
+	_precision_landing_status_pub.publish(precision_landing_status);
+
+	_constraints.want_takeoff = _checkTakeoff();
+
+	return ret;
 }
 
 void
-PrecLand::on_inactivation()
+FlightTaskAutoPrecisionLanding::run_state_start()
 {
-	_is_activated = false;
+	_position_setpoint = _target; // Follow navigator triplet
-}
 
-void
+	// check if target visible and go to horizontal approach directly
-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) {
+	} else if ((PrecLandMode)_param_rtl_pld_md.get() == 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();
+	} else if (_type == WaypointType::land) {
-	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
+		// Navigator already entered land stage. Take over with precision landing
-	if (dist < _navigator->get_acceptance_radius()) {
+		switch_to_state_search();
-		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()
+FlightTaskAutoPrecisionLanding::run_state_horizontal_approach()
 {
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	float x = _target_pose.x_abs;
+	float y = _target_pose.y_abs;
+	slewrate(x, y);
+
+	// Fly to target XY position, but keep navigator's altitude setpoint
+	_position_setpoint(0) = x;
+	_position_setpoint(1) = y;
+	_position_setpoint(2) = _target(2);
+	_velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN;
 
 	// 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;
+		// TODO: This is not going to work after conversion to flight task, because
-		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+		// the position_setpoint will be overwritten at the next iteration!
-		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
+		// Solution: An additional wait state
+		_position_setpoint = _position;
+		_velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN;
 
 		if (!switch_to_state_start()) {
 			switch_to_state_fallback();
@@ -256,25 +184,18 @@ PrecLand::run_state_horizontal_approach()
 		}
 
 	}
-
-	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()
+FlightTaskAutoPrecisionLanding::run_state_descend_above_target()
 {
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	// Overwrite Auto setpoints in order to descend above target
+	_position_setpoint(0) = _target_pose.x_abs;
+	_position_setpoint(1) = _target_pose.y_abs;
+	_position_setpoint(2) = NAN;
+	_velocity_setpoint(0) = 0;
+	_velocity_setpoint(1) = 0;
+	_velocity_setpoint(2) = _param_mpc_land_speed.get();
 
 	// check if target visible
 	if (!check_state_conditions(PrecLandState::DescendAboveTarget)) {
@@ -282,9 +203,7 @@ PrecLand::run_state_descend_above_target()
 			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;
+			_position_setpoint = _position;
-			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();
@@ -293,33 +212,35 @@ PrecLand::run_state_descend_above_target()
 
 		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()
+FlightTaskAutoPrecisionLanding::run_state_final_approach()
 {
-	// nothing to do, will land
+	// Overwrite Auto setpoints in order to land at target's last known location
+	_position_setpoint(0) = _target_pose.x_abs;
+	_position_setpoint(1) = _target_pose.y_abs;
+	_position_setpoint(2) = NAN;
+	_velocity_setpoint(0) = 0;
+	_velocity_setpoint(1) = 0;
+	_velocity_setpoint(2) = _param_mpc_land_speed.get();
 }
 
 void
-PrecLand::run_state_search()
+FlightTaskAutoPrecisionLanding::run_state_search()
 {
+	// Overwrite Auto setpoints in order to hover at search altitude
+	_position_setpoint = _target;
+	_position_setpoint(2) = _sub_home_position.get().z - _param_pld_srch_alt.get();
+	_velocity_setpoint(0) = _velocity_setpoint(1) = _velocity_setpoint(2) = NAN;
+
 	// 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();
+			_position_setpoint = _position;
-			float new_alt = _navigator->get_global_position()->alt + 1.0f;
+			_position_setpoint(2) += 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();
 		}
 
 	}
@@ -341,18 +262,19 @@ PrecLand::run_state_search()
 }
 
 void
-PrecLand::run_state_fallback()
+FlightTaskAutoPrecisionLanding::run_state_fallback()
 {
-	// nothing to do, will land
+	// nothing to do, just listen to navigator
+	_position_setpoint = _target;
+	_velocity_setpoint(0) = 0;
+	_velocity_setpoint(1) = 0;
+	_velocity_setpoint(2) = _param_mpc_land_speed.get();
 }
 
 bool
-PrecLand::switch_to_state_start()
+FlightTaskAutoPrecisionLanding::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;
@@ -366,11 +288,10 @@ PrecLand::switch_to_state_start()
 }
 
 bool
-PrecLand::switch_to_state_horizontal_approach()
+FlightTaskAutoPrecisionLanding::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;
 
@@ -383,7 +304,7 @@ PrecLand::switch_to_state_horizontal_approach()
 }
 
 bool
-PrecLand::switch_to_state_descend_above_target()
+FlightTaskAutoPrecisionLanding::switch_to_state_descend_above_target()
 {
 	if (check_state_conditions(PrecLandState::DescendAboveTarget)) {
 		print_state_switch_message("descend");
@@ -396,7 +317,7 @@ PrecLand::switch_to_state_descend_above_target()
 }
 
 bool
-PrecLand::switch_to_state_final_approach()
+FlightTaskAutoPrecisionLanding::switch_to_state_final_approach()
 {
 	if (check_state_conditions(PrecLandState::FinalApproach)) {
 		print_state_switch_message("final approach");
@@ -408,57 +329,33 @@ PrecLand::switch_to_state_final_approach()
 	return false;
 }
 
-bool
+void
-PrecLand::switch_to_state_search()
+FlightTaskAutoPrecisionLanding::switch_to_state_search()
 {
-	PX4_INFO("Climbing to search altitude.");
+	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
+void
-PrecLand::switch_to_state_fallback()
+FlightTaskAutoPrecisionLanding::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
+void FlightTaskAutoPrecisionLanding::print_state_switch_message(const char *state_name)
-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)
+bool FlightTaskAutoPrecisionLanding::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();
@@ -467,8 +364,7 @@ bool PrecLand::check_state_conditions(PrecLandState state)
 
 		// 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()
+			if (Vector2f(Vector2f(_target_pose.x_abs, _target_pose.y_abs) - _position.xy()).norm() <= _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;
 
@@ -480,15 +376,15 @@ bool PrecLand::check_state_conditions(PrecLandState state)
 		}
 
 		// 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;
+		return _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 we're already in this state, only leave it if target becomes unusable, don't care about horizontal 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
+				return hrt_absolute_time() - _target_pose.timestamp < 500000; // 0.5s  // TODO: Magic number!
 
 			} else {
 				return _target_pose_valid && _target_pose.abs_pos_valid;
@@ -496,14 +392,14 @@ bool PrecLand::check_state_conditions(PrecLandState state)
 
 		} else {
 			// if not already in this state, need to be above target to enter it
-			return _target_pose_updated && _target_pose.abs_pos_valid
+			return _target_pose.abs_pos_valid
-			       && fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_pld_hacc_rad.get()
+			       && fabsf(_target_pose.x_abs - _position(0)) < _param_pld_hacc_rad.get()
-			       && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_pld_hacc_rad.get();
+			       && fabsf(_target_pose.y_abs - _position(1)) < _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();
+		       && (_target_pose.z_abs - _position(2)) < _param_pld_fappr_alt.get();
 
 	case PrecLandState::Search:
 		return true;
@@ -516,7 +412,7 @@ bool PrecLand::check_state_conditions(PrecLandState state)
 	}
 }
 
-void PrecLand::slewrate(float &sp_x, float &sp_y)
+void FlightTaskAutoPrecisionLanding::slewrate(float &sp_x, float &sp_y)
 {
 	matrix::Vector2f sp_curr(sp_x, sp_y);
 	uint64_t now = hrt_absolute_time();
@@ -537,10 +433,8 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
 		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,
+		_sp_pev_prev(0) = _sp_pev(0) - _velocity(0) * dt;
-					   _navigator->get_position_setpoint_triplet()->current.lon);
+		_sp_pev_prev(1) = _sp_pev(1) - _velocity(1) * dt;
-		_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;
@@ -548,21 +442,21 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
 	// 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) {
+	if (sp_vel.length() > _param_xy_vel_cruise.get()) {
-		sp_vel = sp_vel.normalized() * _param_xy_vel_cruise;
+		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) {
+	if (sp_acc.length() > _param_acceleration_hor.get()) {
-		sp_acc = sp_acc.normalized() * _param_acceleration_hor;
+		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 * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x,
+	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
 

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

@@ -1,6 +1,6 @@
-/***************************************************************************
+/****************************************************************************
  *
- *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022 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
@@ -30,24 +30,32 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
+
 /**
- * @file precland.h
+ * @file FlightTaskAutoPrecisionLanding.hpp
  *
- * Helper class to do precision landing with a landing target
+ * 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>
 
-#include "navigator_mode.h"
+#define SEC2USEC 1000000.0f		// TODO: Get the correct define from some header
-#include "mission_block.h"
+#define STATE_TIMEOUT 10000000 		// [us] Maximum time to spend in any state
+#define ACCEPTANCE_RADIUS 0.20f		// Horizontal acceptance radius for the navigation to the landing target
+// TODO: Get ACCEPTANCE_RADIUS from NAV_ACC_RAD
 
 enum class PrecLandState {
 	Start, // Starting state
@@ -55,8 +63,7 @@ enum class PrecLandState {
 	DescendAboveTarget, // Stay over landing target while descending
 	FinalApproach, // Final landing approach, even without landing target
 	Search, // Search for landing target
-	Fallback, // Fallback landing method
+	Fallback // Fallback landing method
-	Done // Done landing
 };
 
 enum class PrecLandMode {
@@ -64,26 +71,17 @@ enum class PrecLandMode {
 	Required = 2 // try to find landing target if not visible at the beginning
 };
 
-class PrecLand : public MissionBlock, public ModuleParams
+class FlightTaskAutoPrecisionLanding : public FlightTaskAuto
 {
 public:
-	PrecLand(Navigator *navigator);
+	FlightTaskAutoPrecisionLanding() = default;
-	~PrecLand() override = default;
+	virtual ~FlightTaskAutoPrecisionLanding() = default;
 
-	void on_activation() override;
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
-	void on_active() override;
-	void on_inactivation() override;
 
-	void set_mode(PrecLandMode mode) { _mode = mode; };
+	bool update() override;
-
-	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();
@@ -97,9 +95,8 @@ private:
 	bool switch_to_state_horizontal_approach();
 	bool switch_to_state_descend_above_target();
 	bool switch_to_state_final_approach();
-	bool switch_to_state_search();
+	void switch_to_state_search();
-	bool switch_to_state_fallback();
+	void switch_to_state_fallback();
-	bool switch_to_state_done();
 
 	void print_state_switch_message(const char *state_name);
 
@@ -110,10 +107,9 @@ private:
 	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 */
+	uORB::PublicationMulti<precision_landing_status_s> _precision_landing_status_pub{ORB_ID(precision_landing_status)};
-	bool _target_pose_updated{false}; /**< wether the landing target position message is updated */
 
-	MapProjection _map_ref{}; /**< class for local/global projections */
+	bool _target_pose_valid{false}; /**< whether we have received a landing target position message */
 
 	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 */
@@ -121,30 +117,23 @@ private:
 	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};
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
-
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed, ///< velocity for controlled descend
-	bool _is_activated {false}; /**< indicates if precland is activated */
+					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_acceleration_hor,
-
+					(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_xy_vel_cruise,
-	DEFINE_PARAMETERS(
+					(ParamFloat<px4::params::PLD_BTOUT>) _param_pld_btout,
-		(ParamFloat<px4::params::PLD_BTOUT>) _param_pld_btout,
+					(ParamFloat<px4::params::PLD_HACC_RAD>) _param_pld_hacc_rad,
-		(ParamFloat<px4::params::PLD_HACC_RAD>) _param_pld_hacc_rad,
+					(ParamFloat<px4::params::PLD_FAPPR_ALT>) _param_pld_fappr_alt,
-		(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_ALT>) _param_pld_srch_alt,
+					(ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_pld_srch_tout,
-		(ParamFloat<px4::params::PLD_SRCH_TOUT>) _param_pld_srch_tout,
+					(ParamInt<px4::params::PLD_MAX_SRCH>) _param_pld_max_srch,
-		(ParamInt<px4::params::PLD_MAX_SRCH>) _param_pld_max_srch
+					(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md
-	)
+				       )
-
-	// 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/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);