navigator : add support for precision landing

src/modules/navigator/CMakeLists.txt

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

src/modules/navigator/mission.cpp

@@ -188,7 +188,6 @@ Mission::on_active()
 
 	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
-
 		/* 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) {
@@ -232,6 +231,17 @@ Mission::on_active()
 
 		do_abort_landing();
 	}
+
+	if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) {
+		// switch out of precision land once landed
+		if (_navigator->get_land_detected()->landed) {
+			_navigator->get_precland()->on_inactivation();
+			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+
+		} else {
+			_navigator->get_precland()->on_active();
+		}
+	}
 }
 
 bool
@@ -501,9 +511,12 @@ Mission::set_mission_items()
 			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
 		}
 
-		/* we have a new position item so set previous position setpoint to current */
 		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		pos_sp_triplet->previous = pos_sp_triplet->current;
+
+		/* we have a new position item so set previous position setpoint to current */
+		if (_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) {
+			pos_sp_triplet->previous = pos_sp_triplet->current;
+		}
 
 		/* 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 */
@@ -681,14 +694,41 @@ Mission::set_mission_items()
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.autocontinue = true;
 			_mission_item.time_inside = 0.0f;
+
+		} else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == 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();
+
+			}
 		}
 
 		/* 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) {
 
-			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
-			/* XXX: noop */
+			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();
+
+			}
+
 		}
 
 		/* ignore yaw for landing items */
@@ -760,8 +800,10 @@ Mission::set_mission_items()
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
 	/* set current position setpoint from mission item (is protected against non-position items) */
-	mission_apply_limitation(_mission_item);
-	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+	if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) {
+		mission_apply_limitation(_mission_item);
+		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+	}
 
 	/* issue command if ready (will do nothing for position mission items) */
 	issue_command(_mission_item);

src/modules/navigator/mission.h

@@ -264,7 +264,8 @@ private:
 		WORK_ITEM_TYPE_ALIGN,		/**< align for next waypoint */
 		WORK_ITEM_TYPE_CMD_BEFORE_MOVE,
 		WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,
-		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION
+		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) */
 };
 

src/modules/navigator/navigation.h

@@ -134,6 +134,7 @@ struct mission_item_s {
 	union {
 		uint16_t do_jump_current_count;		/**< count how many times the jump has been done	*/
 		uint16_t vertex_count;			/**< Polygon vertex count (geofence)	*/
+		uint16_t land_precision;		/**< Defines if landing should be precise: 0 = normal landing, 1 = opportunistic precision landing, 2 = required precision landing (with search)	*/
 	};
 	struct {
 		uint16_t frame : 4,					/**< mission frame */

src/modules/navigator/navigator.h

@@ -48,6 +48,7 @@
 #include "geofence.h"
 #include "gpsfailure.h"
 #include "land.h"
+#include "precland.h"
 #include "loiter.h"
 #include "mission.h"
 #include "navigator_mode.h"
@@ -76,7 +77,7 @@
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 10
+#define NAVIGATOR_MODE_ARRAY_SIZE 11
 
 class Navigator : public control::SuperBlock
 {
@@ -148,6 +149,7 @@ public:
 	struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
 	struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
 	struct 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; }
 
@@ -318,6 +320,7 @@ private:
 	Loiter		_loiter;			/**< class that handles loiter */
 	Takeoff		_takeoff;			/**< class for handling takeoff commands */
 	Land		_land;			/**< class for handling land commands */
+	PrecLand	_precland;			/**< class for handling precision land commands */
 	RTL 		_rtl;				/**< class that handles RTL */
 	RCLoss 		_rcLoss;				/**< class that handles RTL according to OBC rules (rc loss mode) */
 	DataLinkLoss	_dataLinkLoss;			/**< class that handles the OBC datalink loss mode */

src/modules/navigator/navigator_main.cpp

@@ -91,6 +91,7 @@ Navigator::Navigator() :
 	_loiter(this, "LOI"),
 	_takeoff(this, "TKF"),
 	_land(this, "LND"),
+	_precland(this, "PLD"),
 	_rtl(this, "RTL"),
 	_rcLoss(this, "RCL"),
 	_dataLinkLoss(this, "DLL"),
@@ -122,7 +123,9 @@ Navigator::Navigator() :
 	_navigation_mode_array[6] = &_rcLoss;
 	_navigation_mode_array[7] = &_takeoff;
 	_navigation_mode_array[8] = &_land;
-	_navigation_mode_array[9] = &_follow_target;
+	_navigation_mode_array[9] = &_precland;
+	_navigation_mode_array[10] = &_follow_target;
+
 }
 
 Navigator::~Navigator()
@@ -648,6 +651,12 @@ Navigator::task_main()
 			navigation_mode_new = &_land;
 			break;
 
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
+			_pos_sp_triplet_published_invalid_once = false;
+			navigation_mode_new = &_precland;
+			_precland.set_mode(PrecLandMode::Required);
+			break;
+
 		case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 			_pos_sp_triplet_published_invalid_once = false;
 			navigation_mode_new = &_land;

src/modules/navigator/precland.cpp

@@ -0,0 +1,602 @@
+/****************************************************************************
+ *
+ *   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
+
+PrecLand::PrecLand(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_targetPoseSub(0),
+	_target_pose_valid(false),
+	_state_start_time(0),
+	_search_cnt(0),
+	_approach_alt(0),
+	_param_timeout(this, "PLD_BTOUT", false),
+	_param_hacc_rad(this, "PLD_HACC_RAD", false),
+	_param_final_approach_alt(this, "PLD_FAPPR_ALT", false),
+	_param_search_alt(this, "PLD_SRCH_ALT", false),
+	_param_search_timeout(this, "PLD_SRCH_TOUT", false),
+	_param_max_searches(this, "PLD_MAX_SRCH", false),
+	_param_acceleration_hor(this, "MPC_ACC_HOR", false),
+	_param_xy_vel_cruise(this, "MPC_XY_CRUISE", false)
+
+{
+	/* load initial params */
+	updateParams();
+
+}
+
+PrecLand::~PrecLand()
+{
+}
+
+void
+PrecLand::on_inactive()
+{
+}
+
+void
+PrecLand::on_activation()
+{
+	// We need to subscribe here and not in the constructor because constructor is called before the navigator task is spawned
+	if (!_targetPoseSub) {
+		_targetPoseSub = orb_subscribe(ORB_ID(landing_target_pose));
+	}
+
+	_state = PrecLandState::Start;
+	_search_cnt = 0;
+	_last_slewrate_time = 0;
+
+	vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
+
+	if (!map_projection_initialized(&_map_ref)) {
+		map_projection_init(&_map_ref, 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;
+
+	// Check that the current position setpoint is valid, otherwise land at current position
+	if (!pos_sp_triplet->current.valid) {
+		PX4_WARN("Resetting landing position to current position");
+		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;
+	}
+
+	_sp_pev = matrix::Vector2f(0, 0);
+	_sp_pev_prev = matrix::Vector2f(0, 0);
+	_last_slewrate_time = 0;
+
+	switch_to_state_start();
+
+}
+
+void
+PrecLand::on_active()
+{
+	// get new target measurement
+	bool updated = false;
+	orb_check(_targetPoseSub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(landing_target_pose), _targetPoseSub, &_target_pose);
+		_target_pose_valid = true;
+	}
+
+	if (hrt_absolute_time() - _target_pose.timestamp > (uint64_t)(_param_timeout.get()*SEC2USEC)) {
+		_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::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
+		if (!switch_to_state_fallback()) {
+			PX4_ERR("Can't switch to search or fallback landing");
+		}
+	}
+
+	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_search_timeout.get() > 0) {
+
+			if (hrt_absolute_time() - _point_reached_time > 2000000) {
+				if (!switch_to_state_search()) {
+					if (!switch_to_state_fallback()) {
+						PX4_ERR("Can't switch to search or fallback landing");
+					}
+				}
+			}
+
+		} else {
+			if (!switch_to_state_fallback()) {
+				PX4_ERR("Can't switch to search or fallback landing");
+			}
+		}
+	}
+}
+
+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("Lost landing target while landig (horizontal approach).");
+
+		// 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()) {
+			if (!switch_to_state_fallback()) {
+				PX4_ERR("Can't switch to fallback landing");
+			}
+		}
+
+		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;
+			}
+		}
+
+	}
+
+	if (hrt_absolute_time() - _state_start_time > STATE_TIMEOUT) {
+		PX4_ERR("Precision landing took too long during horizontal approach phase.");
+
+		if (switch_to_state_fallback()) {
+			return;
+		}
+
+		PX4_ERR("Can't switch to fallback landing");
+	}
+
+	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
+	double lat, lon;
+	map_projection_reproject(&_map_ref, x, y, &lat, &lon);
+
+	pos_sp_triplet->current.lat = lat;
+	pos_sp_triplet->current.lon = 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("Lost landing target while landing (descending).");
+
+			// 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()) {
+				if (!switch_to_state_fallback()) {
+					PX4_ERR("Can't switch to fallback landing");
+				}
+			}
+		}
+
+		return;
+	}
+
+	// XXX need to transform to GPS coords because mc_pos_control only looks at that
+	double lat, lon;
+	map_projection_reproject(&_map_ref, _target_pose.x_abs, _target_pose.y_abs, &lat, &lon);
+
+	pos_sp_triplet->current.lat = lat;
+	pos_sp_triplet->current.lon = 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_search_timeout.get()*SEC2USEC) {
+		PX4_WARN("Search timed out");
+
+		if (!switch_to_state_fallback()) {
+			PX4_ERR("Can't switch to fallback landing");
+		}
+	}
+}
+
+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)) {
+		_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)) {
+		_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)) {
+		_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_search_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()
+{
+	PX4_WARN("Falling back to normal land.");
+	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;
+}
+
+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_max_searches.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_hacc_rad.get()
+			    && fabsf(_target_pose.y_rel - vehicle_local_position->y) < _param_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_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_valid && _target_pose.abs_pos_valid
+			       && fabsf(_target_pose.x_rel) < _param_hacc_rad.get() && fabsf(_target_pose.y_rel) < _param_hacc_rad.get();
+		}
+
+	case PrecLandState::FinalApproach:
+		return _target_pose_valid && _target_pose.rel_pos_valid && _target_pose.z_rel < _param_final_approach_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
+		map_projection_project(&_map_ref, _navigator->get_position_setpoint_triplet()->current.lat,
+				       _navigator->get_position_setpoint_triplet()->current.lon, &_sp_pev(0), &_sp_pev(1));
+		_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.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/navigator/precland.h

@@ -0,0 +1,135 @@
+/***************************************************************************
+ *
+ *   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>
+ */
+
+#ifndef NAVIGATOR_PRECLAND_H
+#define NAVIGATOR_PRECLAND_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+#include <uORB/topics/landing_target_pose.h>
+#include <geo/geo.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:
+	PrecLand(Navigator *navigator, const char *name);
+
+	~PrecLand();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+	void set_mode(PrecLandMode mode) { _mode = mode; };
+
+	PrecLandMode get_mode() { return _mode; };
+
+private:
+	// 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();
+
+	// 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 */
+	int _targetPoseSub;
+	bool _target_pose_valid; /**< wether we have received a landing target position message */
+	struct map_projection_reference_s _map_ref {}; /**< reference for local/global projections */
+	uint64_t _state_start_time; /**< time when we entered current state */
+	uint64_t _last_slewrate_time; /**< time when we last limited setpoint changes */
+	uint64_t _target_acquired_time; /**< time when we first saw the landing target during search */
+	uint64_t _point_reached_time; /**< time when we reached a setpoint */
+	int _search_cnt; /**< counter of how many times we had to search for the landing target */
+	float _approach_alt; /**< altitude at which to stay during horizontal approach */
+
+	matrix::Vector2f _sp_pev;
+	matrix::Vector2f _sp_pev_prev;
+
+	PrecLandState _state;
+
+	PrecLandMode _mode;
+
+	control::BlockParamFloat _param_timeout;
+	control::BlockParamFloat _param_hacc_rad;
+	control::BlockParamFloat _param_final_approach_alt;
+	control::BlockParamFloat _param_search_alt;
+	control::BlockParamFloat _param_search_timeout;
+	control::BlockParamInt   _param_max_searches;
+	control::BlockParamFloat _param_acceleration_hor;
+	control::BlockParamFloat _param_xy_vel_cruise;
+
+};
+
+#endif

src/modules/navigator/precland_params.c

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   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_params.c
+ *
+ * Parameters for precision landing.
+ *
+ * @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
+ */
+
+/**
+ * Landing Target Timeout
+ *
+ * Time after which the landing target is considered lost without any new measurements.
+ *
+ * @unit s
+ * @min 0.0
+ * @max 50
+ * @decimal 1
+ * @increment 0.5
+ * @group Precision Land
+ */
+PARAM_DEFINE_FLOAT(PLD_BTOUT, 5.0f);
+
+/**
+ * Horizontal acceptance radius
+ *
+ * Start descending if closer above landing target than this.
+ *
+ * @unit m
+ * @min 0.0
+ * @max 10
+ * @decimal 2
+ * @increment 0.1
+ * @group Precision Land
+ */
+PARAM_DEFINE_FLOAT(PLD_HACC_RAD, 0.2f);
+
+/**
+ * Final approach altitude
+ *
+ * Allow final approach (without horizontal positioning) if losing landing target closer than this to the ground.
+ *
+ * @unit m
+ * @min 0.0
+ * @max 10
+ * @decimal 2
+ * @increment 0.1
+ * @group Precision Land
+ */
+PARAM_DEFINE_FLOAT(PLD_FAPPR_ALT, 0.1f);
+
+/**
+ * Search altitude
+ *
+ * Altitude above home to which to climb when searching for the landing target.
+ *
+ * @unit m
+ * @min 0.0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group Precision Land
+ */
+PARAM_DEFINE_FLOAT(PLD_SRCH_ALT, 10.0f);
+
+/**
+ * Search timeout
+ *
+ * Time allowed to search for the landing target before falling back to normal landing.
+ *
+ * @unit s
+ * @min 0.0
+ * @max 100
+ * @decimal 1
+ * @increment 0.1
+ * @group Precision Land
+ */
+PARAM_DEFINE_FLOAT(PLD_SRCH_TOUT, 10.0f);
+
+/**
+ * Maximum number of search attempts
+ *
+ * Maximum number of times to seach for the landing target if it is lost during the precision landing.
+ *
+ * @min 0.0
+ * @max 100
+ * @group Precision Land
+ */
+PARAM_DEFINE_INT32(PLD_MAX_SRCH, 3);