From 63e741824cfde42ac563c457e615cf58f10a2529 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 1 Sep 2014 11:27:28 +0200
Subject: [PATCH 01/10] fw landing: use terrain estimate from global pos

---
 .../fw_pos_control_l1_main.cpp                | 82 ++++++-------------
 1 file changed, 24 insertions(+), 58 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index deccab4820..064b8a6fae 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -88,7 +88,6 @@
 #include <launchdetection/LaunchDetector.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
-#include <drivers/drv_range_finder.h>
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
 
@@ -143,7 +142,6 @@ private:
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
 	int		_sensor_combined_sub;		/**< for body frame accelerations */
-	int		_range_finder_sub;		/**< range finder subscription */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
 	orb_advert_t	_tecs_status_pub;		/**< TECS status publication */
@@ -158,7 +156,6 @@ private:
 	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;		/**< triplet of mission items */
 	struct sensor_combined_s			_sensor_combined;		/**< for body frame accelerations */
-	struct range_finder_report 			_range_finder;			/**< range finder report */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -239,7 +236,6 @@ private:
 		float land_flare_alt_relative;
 		float land_thrust_lim_alt_relative;
 		float land_heading_hold_horizontal_distance;
-		float range_finder_rel_alt;
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -284,7 +280,6 @@ private:
 		param_t land_flare_alt_relative;
 		param_t land_thrust_lim_alt_relative;
 		param_t land_heading_hold_horizontal_distance;
-		param_t range_finder_rel_alt;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -310,12 +305,6 @@ private:
 	 */
 	bool		vehicle_airspeed_poll();
 
-	/**
-	 * Check for range finder updates.
-	 */
-	bool		range_finder_poll();
-
-
 	/**
 	 * Check for position updates.
 	 */
@@ -337,9 +326,9 @@ private:
 	void navigation_capabilities_publish();
 
 	/**
-	 * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
+	 * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
 	 */
-	float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
+	float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
 
 	/**
 	 * Control position.
@@ -411,7 +400,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_params_sub(-1),
 	_manual_control_sub(-1),
 	_sensor_combined_sub(-1),
-	_range_finder_sub(-1),
 
 /* publications */
 	_attitude_sp_pub(-1),
@@ -428,7 +416,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_global_pos(),
 	_pos_sp_triplet(),
 	_sensor_combined(),
-	_range_finder(),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
@@ -477,7 +464,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
 	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
-	_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.time_const_throt = 			param_find("FW_T_THRO_CONST");
@@ -577,8 +563,6 @@ FixedwingPositionControl::parameters_update()
 
 	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
 
-	param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
-
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
@@ -669,20 +653,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 	return airspeed_updated;
 }
 
-bool
-FixedwingPositionControl::range_finder_poll()
-{
-	/* check if there is a range finder measurement */
-	bool range_finder_updated;
-	orb_check(_range_finder_sub, &range_finder_updated);
-
-	if (range_finder_updated) {
-		orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
-	}
-
-	return range_finder_updated;
-}
-
 void
 FixedwingPositionControl::vehicle_attitude_poll()
 {
@@ -820,21 +790,14 @@ void FixedwingPositionControl::navigation_capabilities_publish()
 	}
 }
 
-float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
+float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
 {
-	float rel_alt_estimated = current_alt - land_setpoint_alt;
-
-	/* only use range finder if:
-	 * parameter (range_finder_use_relative_alt) > 0
-	 * the measurement is valid
-	 * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
-	 */
-	if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) {
-		return rel_alt_estimated;
+	if (global_pos.terrain_alt_valid &&
+			isfinite(global_pos.terrain_alt)) {
+		return global_pos.terrain_alt;
+	} else {
+		return land_setpoint_alt;
 	}
-
-	return range_finder.distance;
-
 }
 
 bool
@@ -991,15 +954,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
+			/* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be
+			 * equal to _pos_sp_triplet.current.alt */
+			float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos);
+
 			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
-			float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
+			float L_altitude_rel = _pos_sp_triplet.previous.valid ?
+				_pos_sp_triplet.previous.alt - terrain_alt : 0.0f;
 
 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 			float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
 
-			float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
 
-			if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
+			if ( (_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 
 				/* land with minimal speed */
 
@@ -1009,7 +976,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* kill the throttle if param requests it */
 				throttle_max = _parameters.throttle_max;
 
-				 if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
+				 if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 					if (!land_motor_lim) {
 						land_motor_lim  = true;
@@ -1027,12 +994,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					land_stayonground = true;
 				}
 
-				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
+				tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
 						calculate_target_airspeed(airspeed_land), eas2tas,
 						flare_pitch_angle_rad, math::radians(15.0f),
 						0.0f, throttle_max, throttle_land,
 						false, flare_pitch_angle_rad,
-						_pos_sp_triplet.current.alt + relative_alt, ground_speed,
+						_global_pos.alt, ground_speed,
 						land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
 
 				if (!land_noreturn_vertical) {
@@ -1053,8 +1020,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				  * if current position is below the slope continue at previous wp altitude
 				  * until the intersection with slope
 				  * */
-				float altitude_desired_rel = relative_alt;
-				if (relative_alt > landing_slope_alt_rel_desired || land_onslope) {
+				float altitude_desired_rel;
+				if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) {
 					/* stay on slope */
 					altitude_desired_rel = landing_slope_alt_rel_desired;
 					if (!land_onslope) {
@@ -1063,10 +1030,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					}
 				} else {
 					/* continue horizontally */
-					altitude_desired_rel =  _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt;
+					altitude_desired_rel =  _pos_sp_triplet.previous.valid ? L_altitude_rel :
+						_global_pos.alt - terrain_alt;
 				}
 
-				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel,
+				tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
 						calculate_target_airspeed(airspeed_approach), eas2tas,
 						math::radians(_parameters.pitch_limit_min),
 						math::radians(_parameters.pitch_limit_max),
@@ -1075,7 +1043,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						_parameters.throttle_cruise,
 						false,
 						math::radians(_parameters.pitch_limit_min),
-						_pos_sp_triplet.current.alt + relative_alt,
+						_global_pos.alt,
 						ground_speed);
 			}
 
@@ -1221,7 +1189,6 @@ FixedwingPositionControl::task_main()
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_control_mode_sub, 200);
@@ -1295,7 +1262,6 @@ FixedwingPositionControl::task_main()
 			vehicle_setpoint_poll();
 			vehicle_sensor_combined_poll();
 			vehicle_airspeed_poll();
-			range_finder_poll();
 			// vehicle_baro_poll();
 
 			math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e,  _global_pos.vel_d);

From 7f0ba70b1e8e86334d1acb827d8d2d4bf7305870 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 3 Sep 2014 08:39:26 +0200
Subject: [PATCH 02/10] fw landing: if using terrain estimate, don't switch
 back during landing

---
 .../fw_pos_control_l1_main.cpp                | 23 +++++++++++--------
 1 file changed, 14 insertions(+), 9 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 064b8a6fae..0db857727a 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -160,12 +160,12 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	/* land states */
-	/* not in non-abort mode for landing yet */
 	bool land_noreturn_horizontal;
 	bool land_noreturn_vertical;
 	bool land_stayonground;
 	bool land_motor_lim;
 	bool land_onslope;
+	bool land_useterrain;
 
 	/* takeoff/launch states */
 	bool launch_detected;
@@ -425,6 +425,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_stayonground(false),
 	land_motor_lim(false),
 	land_onslope(false),
+	land_useterrain(false),
 	launch_detected(false),
 	usePreTakeoffThrust(false),
 	last_manual(false),
@@ -792,8 +793,14 @@ void FixedwingPositionControl::navigation_capabilities_publish()
 
 float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
 {
-	if (global_pos.terrain_alt_valid &&
-			isfinite(global_pos.terrain_alt)) {
+	if (!isfinite(global_pos.terrain_alt)) {
+		return land_setpoint_alt;
+	}
+
+	/* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it
+	 * for the whole landing */
+	if (global_pos.terrain_alt_valid || land_useterrain) {
+		land_useterrain = true;
 		return global_pos.terrain_alt;
 	} else {
 		return land_setpoint_alt;
@@ -941,11 +948,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			/* Vertical landing control */
 			//xxx: using the tecs altitude controller for slope control for now
-
-//				/* do not go down too early */
-//				if (wp_distance > 50.0f) {
-//					altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
-//				}
 			/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
 			// XXX this could make a great param
 
@@ -967,7 +969,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 
 			if ( (_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
-
 				/* land with minimal speed */
 
 //					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
@@ -993,6 +994,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					flare_curve_alt_rel = 0.0f; // stay on ground
 					land_stayonground = true;
 				}
+				warnx("flare: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + flare_curve_alt_rel));
 
 				tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
 						calculate_target_airspeed(airspeed_land), eas2tas,
@@ -1034,6 +1036,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						_global_pos.alt - terrain_alt;
 				}
 
+				warnx("approach: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + altitude_desired_rel));
+
 				tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
 						calculate_target_airspeed(airspeed_approach), eas2tas,
 						math::radians(_parameters.pitch_limit_min),
@@ -1326,6 +1330,7 @@ void FixedwingPositionControl::reset_landing_state()
 	land_stayonground = false;
 	land_motor_lim = false;
 	land_onslope = false;
+	land_useterrain = false;
 }
 
 void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,

From 484f2864d188fa04cbbbd767278b547b7f9969af Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 3 Sep 2014 12:36:44 +0200
Subject: [PATCH 03/10] remove warnx

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 0db857727a..6e06b2b788 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -994,7 +994,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					flare_curve_alt_rel = 0.0f; // stay on ground
 					land_stayonground = true;
 				}
-				warnx("flare: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + flare_curve_alt_rel));
 
 				tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
 						calculate_target_airspeed(airspeed_land), eas2tas,
@@ -1036,8 +1035,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						_global_pos.alt - terrain_alt;
 				}
 
-				warnx("approach: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + altitude_desired_rel));
-
 				tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
 						calculate_target_airspeed(airspeed_approach), eas2tas,
 						math::radians(_parameters.pitch_limit_min),

From 139ca8c327a80b61213e2d75dd8819cf119d22d7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 3 Sep 2014 18:53:05 +0200
Subject: [PATCH 04/10] fw landing: impose horizontal limit for start of flare

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 84c14be018..e71e7a2ac7 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -999,7 +999,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
 
-			if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
+			/* Check if we should start flaring with a vertical and a
+			 * horizontal limit (with some tolerance) */
+			if ( ((relative_alt < landingslope.flare_relative_alt()) &&
+						(wp_distance < landingslope.flare_length() + 5.0f)) ||
+					land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 
 				/* land with minimal speed */
 

From bf3f8861ddbf2af80fc121589ab6189538c1cadb Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 4 Sep 2014 11:11:03 +0200
Subject: [PATCH 05/10] fw landing: horiz flare check: fix wp distance

better calculation of wp distance when behind wp
---
 .../fw_pos_control_l1/fw_pos_control_l1_main.cpp   | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 409b57d63e..e8e316064d 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -909,10 +909,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 
 			float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
+			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 
 			/* Horizontal landing control */
 			/* switch to heading hold for the last meters, continue heading hold after */
 			float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+			/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
+			float wp_distance_save = wp_distance;
+			if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) {
+				wp_distance_save = 0.0f;
+			}
+
 			//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
 			if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
 
@@ -964,13 +971,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			float L_altitude_rel = _pos_sp_triplet.previous.valid ?
 				_pos_sp_triplet.previous.alt - terrain_alt : 0.0f;
 
-			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 			float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
 
 			/* Check if we should start flaring with a vertical and a
-			 * horizontal limit (with some tolerance) */
+			 * horizontal limit (with some tolerance)
+			 * The horizontal limit is only applied when we are in front of the wp
+			 */
 			if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) &&
-						(wp_distance < landingslope.flare_length() + 5.0f)) ||
+						(wp_distance_save < landingslope.flare_length() + 5.0f)) ||
 					land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 				/* land with minimal speed */
 

From 3684bf71d5068a03587005ba365c8705e327e936 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 4 Sep 2014 18:12:20 +0200
Subject: [PATCH 06/10] make flare pitch angle a param

---
 .../fw_pos_control_l1_main.cpp                |  14 +-
 .../fw_pos_control_l1_params.c                | 134 ++++++++++--------
 2 files changed, 86 insertions(+), 62 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index e8e316064d..c0c238ba9b 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -237,6 +237,8 @@ private:
 		float land_flare_alt_relative;
 		float land_thrust_lim_alt_relative;
 		float land_heading_hold_horizontal_distance;
+		float land_flare_pitch_min_deg;
+		float land_flare_pitch_max_deg;
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -281,6 +283,8 @@ private:
 		param_t land_flare_alt_relative;
 		param_t land_thrust_lim_alt_relative;
 		param_t land_heading_hold_horizontal_distance;
+		param_t land_flare_pitch_min_deg;
+		param_t land_flare_pitch_max_deg;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -465,6 +469,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
 	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
+	_parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN");
+	_parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.time_const_throt = 			param_find("FW_T_THRO_CONST");
@@ -563,6 +569,8 @@ FixedwingPositionControl::parameters_update()
 	}
 
 	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
+	param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg));
+	param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
@@ -958,7 +966,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
 			// XXX this could make a great param
 
-			float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
 			float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
@@ -1008,9 +1015,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 				tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
 						calculate_target_airspeed(airspeed_land), eas2tas,
-						flare_pitch_angle_rad, math::radians(15.0f),
+						 math::radians(_parameters.land_flare_pitch_min_deg),
+						 math::radians(_parameters.land_flare_pitch_max_deg),
 						0.0f, throttle_max, throttle_land,
-						false, flare_pitch_angle_rad,
+						false,  math::radians(_parameters.land_flare_pitch_min_deg),
 						_global_pos.alt, ground_speed,
 						land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 890ab3bad2..f2e8c422f3 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
 /**
  * Throttle limit max
  *
- * This is the maximum throttle % that can be used by the controller. 
- * For overpowered aircraft, this should be reduced to a value that 
+ * This is the maximum throttle % that can be used by the controller.
+ * For overpowered aircraft, this should be reduced to a value that
  * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
  *
  * @group L1 Control
@@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
 /**
  * Throttle limit min
  *
- * This is the minimum throttle % that can be used by the controller. 
- * For electric aircraft this will normally be set to zero, but can be set 
- * to a small non-zero value if a folding prop is fitted to prevent the 
- * prop from folding and unfolding repeatedly in-flight or to provide 
+ * This is the minimum throttle % that can be used by the controller.
+ * For electric aircraft this will normally be set to zero, but can be set
+ * to a small non-zero value if a folding prop is fitted to prevent the
+ * prop from folding and unfolding repeatedly in-flight or to provide
  * some aerodynamic drag from a turning prop to improve the descent rate.
  *
  * For aircraft with internal combustion engine this parameter should be set
@@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 /**
  * Throttle limit value before flare
  *
- * This throttle value will be set as throttle limit at FW_LND_TLALT, 
+ * This throttle value will be set as throttle limit at FW_LND_TLALT,
  * before arcraft will flare.
  *
  * @group L1 Control
@@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
 /**
  * Maximum climb rate
  *
- * This is the best climb rate that the aircraft can achieve with 
- * the throttle set to THR_MAX and the airspeed set to the 
- * default value. For electric aircraft make sure this number can be 
- * achieved towards the end of flight when the battery voltage has reduced. 
- * The setting of this parameter can be checked by commanding a positive 
- * altitude change of 100m in loiter, RTL or guided mode. If the throttle 
- * required to climb is close to THR_MAX and the aircraft is maintaining 
- * airspeed, then this parameter is set correctly. If the airspeed starts 
- * to reduce, then the parameter is set to high, and if the throttle 
- * demand required to climb and maintain speed is noticeably less than 
- * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or 
+ * This is the best climb rate that the aircraft can achieve with
+ * the throttle set to THR_MAX and the airspeed set to the
+ * default value. For electric aircraft make sure this number can be
+ * achieved towards the end of flight when the battery voltage has reduced.
+ * The setting of this parameter can be checked by commanding a positive
+ * altitude change of 100m in loiter, RTL or guided mode. If the throttle
+ * required to climb is close to THR_MAX and the aircraft is maintaining
+ * airspeed, then this parameter is set correctly. If the airspeed starts
+ * to reduce, then the parameter is set to high, and if the throttle
+ * demand required to climb and maintain speed is noticeably less than
+ * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
  * FW_THR_MAX reduced.
  *
  * @group L1 Control
@@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
 /**
  * Minimum descent rate
  *
- * This is the sink rate of the aircraft with the throttle 
- * set to THR_MIN and flown at the same airspeed as used 
+ * This is the sink rate of the aircraft with the throttle
+ * set to THR_MIN and flown at the same airspeed as used
  * to measure FW_T_CLMB_MAX.
  *
  * @group Fixed Wing TECS
@@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
 /**
  * Maximum descent rate
  *
- * This sets the maximum descent rate that the controller will use. 
- * If this value is too large, the aircraft can over-speed on descent. 
- * This should be set to a value that can be achieved without 
- * exceeding the lower pitch angle limit and without over-speeding 
+ * This sets the maximum descent rate that the controller will use.
+ * If this value is too large, the aircraft can over-speed on descent.
+ * This should be set to a value that can be achieved without
+ * exceeding the lower pitch angle limit and without over-speeding
  * the aircraft.
  *
  * @group Fixed Wing TECS
@@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
 /**
  * TECS time constant
  *
- * This is the time constant of the TECS control algorithm (in seconds). 
+ * This is the time constant of the TECS control algorithm (in seconds).
  * Smaller values make it faster to respond, larger values make it slower
  * to respond.
  *
@@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
 /**
  * TECS Throttle time constant
  *
- * This is the time constant of the TECS throttle control algorithm (in seconds). 
+ * This is the time constant of the TECS throttle control algorithm (in seconds).
  * Smaller values make it faster to respond, larger values make it slower
  * to respond.
  *
@@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
 /**
  * Throttle damping factor
  *
- * This is the damping gain for the throttle demand loop. 
+ * This is the damping gain for the throttle demand loop.
  * Increase to add damping to correct for oscillations in speed and height.
  *
  * @group Fixed Wing TECS
@@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
 /**
  * Integrator gain
  *
- * This is the integrator gain on the control loop. 
- * Increasing this gain increases the speed at which speed 
- * and height offsets are trimmed out, but reduces damping and 
+ * This is the integrator gain on the control loop.
+ * Increasing this gain increases the speed at which speed
+ * and height offsets are trimmed out, but reduces damping and
  * increases overshoot.
  *
  * @group Fixed Wing TECS
@@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
  * Maximum vertical acceleration
  *
  * This is the maximum vertical acceleration (in metres/second square)
- * either up or down that the controller will use to correct speed 
- * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) 
- * allows for reasonably aggressive pitch changes if required to recover 
+ * either up or down that the controller will use to correct speed
+ * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
+ * allows for reasonably aggressive pitch changes if required to recover
  * from under-speed conditions.
  *
  * @group Fixed Wing TECS
@@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
 /**
  * Complementary filter "omega" parameter for height
  *
- * This is the cross-over frequency (in radians/second) of the complementary 
- * filter used to fuse vertical acceleration and barometric height to obtain 
- * an estimate of height rate and height. Increasing this frequency weights 
- * the solution more towards use of the barometer, whilst reducing it weights 
+ * This is the cross-over frequency (in radians/second) of the complementary
+ * filter used to fuse vertical acceleration and barometric height to obtain
+ * an estimate of height rate and height. Increasing this frequency weights
+ * the solution more towards use of the barometer, whilst reducing it weights
  * the solution more towards use of the accelerometer data.
  *
  * @group Fixed Wing TECS
@@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
 /**
  * Complementary filter "omega" parameter for speed
  *
- * This is the cross-over frequency (in radians/second) of the complementary 
- * filter used to fuse longitudinal acceleration and airspeed to obtain an 
+ * This is the cross-over frequency (in radians/second) of the complementary
+ * filter used to fuse longitudinal acceleration and airspeed to obtain an
  * improved airspeed estimate. Increasing this frequency weights the solution
- * more towards use of the arispeed sensor, whilst reducing it weights the 
+ * more towards use of the arispeed sensor, whilst reducing it weights the
  * solution more towards use of the accelerometer data.
  *
  * @group Fixed Wing TECS
@@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
 /**
  * Roll -> Throttle feedforward
  *
- * Increasing this gain turn increases the amount of throttle that will 
- * be used to compensate for the additional drag created by turning. 
- * Ideally this should be set to  approximately 10 x the extra sink rate 
- * in m/s created by a 45 degree bank turn. Increase this gain if 
- * the aircraft initially loses energy in turns and reduce if the 
- * aircraft initially gains energy in turns. Efficient high aspect-ratio 
- * aircraft (eg powered sailplanes) can use a lower value, whereas 
+ * Increasing this gain turn increases the amount of throttle that will
+ * be used to compensate for the additional drag created by turning.
+ * Ideally this should be set to  approximately 10 x the extra sink rate
+ * in m/s created by a 45 degree bank turn. Increase this gain if
+ * the aircraft initially loses energy in turns and reduce if the
+ * aircraft initially gains energy in turns. Efficient high aspect-ratio
+ * aircraft (eg powered sailplanes) can use a lower value, whereas
  * inefficient low aspect-ratio models (eg delta wings) can use a higher value.
  *
  * @group Fixed Wing TECS
@@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
 /**
  * Speed <--> Altitude priority
  *
- * This parameter adjusts the amount of weighting that the pitch control 
- * applies to speed vs height errors. Setting it to 0.0 will cause the 
- * pitch control to control height and ignore speed errors. This will 
- * normally improve height accuracy but give larger airspeed errors. 
- * Setting it to 2.0 will cause the pitch control loop to control speed 
- * and ignore height errors. This will normally reduce airspeed errors, 
- * but give larger height errors. The default value of 1.0 allows the pitch 
- * control to simultaneously control height and speed. 
- * Note to Glider Pilots - set this parameter to 2.0 (The glider will 
+ * This parameter adjusts the amount of weighting that the pitch control
+ * applies to speed vs height errors. Setting it to 0.0 will cause the
+ * pitch control to control height and ignore speed errors. This will
+ * normally improve height accuracy but give larger airspeed errors.
+ * Setting it to 2.0 will cause the pitch control loop to control speed
+ * and ignore height errors. This will normally reduce airspeed errors,
+ * but give larger height errors. The default value of 1.0 allows the pitch
+ * control to simultaneously control height and speed.
+ * Note to Glider Pilots - set this parameter to 2.0 (The glider will
  * adjust its pitch angle to maintain airspeed, ignoring changes in height).
  *
  * @group Fixed Wing TECS
@@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
 /**
  * Pitch damping factor
  *
- * This is the damping gain for the pitch demand loop. Increase to add 
- * damping to correct for oscillations in height. The default value of 0.0 
- * will work well provided the pitch to servo controller has been tuned 
+ * This is the damping gain for the pitch demand loop. Increase to add
+ * damping to correct for oscillations in height. The default value of 0.0
+ * will work well provided the pitch to servo controller has been tuned
  * properly.
  *
  * @group Fixed Wing TECS
@@ -414,3 +414,19 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
  * @group L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
+
+/**
+ * Flare, minimum pitch
+ *
+ * Minimum pitch during flare, a positive sign means nose up
+ *
+ */
+PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f);
+
+/**
+ * Flare, maximum pitch
+ *
+ * Maximum pitch during flare, a positive sign means nose up
+ *
+ */
+PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f);

From dbe35d64ca4288de551ec11b4d5bff43650e8012 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 6 Sep 2014 15:32:12 +0200
Subject: [PATCH 07/10] Fixed delay calculation of laser

---
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 97abb76a9c..2c50e5c755 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1369,7 +1369,7 @@ FixedwingEstimator::task_main()
 					if (newRangeData) {
 						_ekf->fuseRngData = true;
 						_ekf->useRangeFinder = true;
-						_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f));
+						_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
 						_ekf->GroundEKF();
 					}
 

From 3e41945cc6b43e91d08082b1ce976d5382e0405a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 21 Sep 2014 10:46:46 +0200
Subject: [PATCH 08/10] flare pitch limit: apply at throttle lim altitude

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +-
 src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 ++
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index c0c238ba9b..77eb78097b 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1018,7 +1018,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						 math::radians(_parameters.land_flare_pitch_min_deg),
 						 math::radians(_parameters.land_flare_pitch_max_deg),
 						0.0f, throttle_max, throttle_land,
-						false,  math::radians(_parameters.land_flare_pitch_min_deg),
+						false,  land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min),
 						_global_pos.alt, ground_speed,
 						land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index f2e8c422f3..859cef8bdb 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -419,6 +419,7 @@ PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
  * Flare, minimum pitch
  *
  * Minimum pitch during flare, a positive sign means nose up
+ * Applied once FW_LND_TLALT is reached
  *
  */
 PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f);
@@ -427,6 +428,7 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f);
  * Flare, maximum pitch
  *
  * Maximum pitch during flare, a positive sign means nose up
+ * Applied once FW_LND_TLALT is reached
  *
  */
 PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f);

From 32131a069eeb77637a67e10011b4b756bf4d718a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 22 Sep 2014 09:44:38 +0200
Subject: [PATCH 09/10] inform in GCS when switching to laser

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index eb4ffdabb3..ddfbc233b0 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -814,7 +814,10 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
 	/* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it
 	 * for the whole landing */
 	if (global_pos.terrain_alt_valid || land_useterrain) {
-		land_useterrain = true;
+		if(!land_useterrain) {
+			mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate");
+			land_useterrain = true;
+		}
 		return global_pos.terrain_alt;
 	} else {
 		return land_setpoint_alt;

From 852159d9edaa85c3abee3059359264f3329841e8 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 22 Sep 2014 10:03:23 +0200
Subject: [PATCH 10/10] fw pos control: add param to enable/disable usage of
 terrain estimate during landing

---
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++++-
 src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 8 +++-----
 2 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index ddfbc233b0..a94311ccb3 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -240,6 +240,7 @@ private:
 		float land_heading_hold_horizontal_distance;
 		float land_flare_pitch_min_deg;
 		float land_flare_pitch_max_deg;
+		int land_use_terrain_estimate;
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -287,6 +288,7 @@ private:
 		param_t land_heading_hold_horizontal_distance;
 		param_t land_flare_pitch_min_deg;
 		param_t land_flare_pitch_max_deg;
+		param_t land_use_terrain_estimate;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -474,6 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
 	_parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN");
 	_parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX");
+	_parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.time_const_throt = 			param_find("FW_T_THRO_CONST");
@@ -576,6 +579,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
 	param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg));
 	param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
+	param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate));
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
@@ -813,7 +817,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
 
 	/* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it
 	 * for the whole landing */
-	if (global_pos.terrain_alt_valid || land_useterrain) {
+	if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) {
 		if(!land_useterrain) {
 			mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate");
 			land_useterrain = true;
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 41a3665381..c00d822327 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -412,15 +412,13 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
 PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
 
 /**
- * Relative altitude threshold for range finder measurements for use during landing
+ * Enable or disable usage of terrain estimate during landing
  *
- * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT
- * set to < 0 to disable
- * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location
+ * 0: disabled, 1: enabled
  *
  * @group L1 Control
  */
-PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
+PARAM_DEFINE_INT32(FW_LND_USETER, 0);
 
 /**
  * Flare, minimum pitch