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.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);