Merge pull request #3143 from PX4/takeoff_landing

Takeoff landing

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -69,6 +69,7 @@
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #include <geo/geo.h>
+#include <terrain_estimation/terrain_estimator.h>
 #include <systemlib/perf_counter.h>
 #include <lib/ecl/validation/data_validator_group.h>
 #include "estimator_22states.h"
@@ -278,6 +279,8 @@ private:
 
     AttPosEKF                   *_ekf;
 
+    TerrainEstimator            *_terrain_estimator;
+
     /* Low pass filter for attitude rates */
     math::LowPassFilter2p _LP_att_P;
     math::LowPassFilter2p _LP_att_Q;

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -212,6 +212,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_parameters{},
 	_parameter_handles{},
 	_ekf(nullptr),
+	_terrain_estimator(nullptr),
 
 	_LP_att_P(250.0f, 20.0f),
 	_LP_att_Q(250.0f, 20.0f),
@@ -219,6 +220,8 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 {
 	_voter_mag.set_timeout(200000);
 
+	_terrain_estimator = new TerrainEstimator();
+
 	_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
 	_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
 	_parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS");
@@ -267,7 +270,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF()
 			}
 		} while (_estimator_task != -1);
 	}
-
+	delete _terrain_estimator;
 	delete _ekf;
 
 	estimator::g_estimator = nullptr;
@@ -697,6 +700,10 @@ void AttitudePositionEstimatorEKF::task_main()
 					// Run EKF data fusion steps
 					updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
 
+					// Run separate terrain estimator
+					_terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance);
+					_terrain_estimator->measurement_update(hrt_absolute_time(), &_gps, &_distance, &_att);
+
 					// Publish attitude estimations
 					publishAttitude();
 
@@ -997,9 +1004,12 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 	}
 
 	/* terrain altitude */
-	_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
-	_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
-					(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
+	if (_terrain_estimator->is_valid()) {
+		_global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground();
+		_global_pos.terrain_alt_valid = true;
+	} else {
+		_global_pos.terrain_alt_valid = false;
+	}
 
 	_global_pos.yaw = _local_pos.yaw;
 

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -82,6 +82,7 @@
 #include <ecl/attitude_fw/ecl_pitch_controller.h>
 #include <ecl/attitude_fw/ecl_roll_controller.h>
 #include <ecl/attitude_fw/ecl_yaw_controller.h>
+#include <ecl/attitude_fw/ecl_wheel_controller.h>
 #include <platforms/px4_defines.h>
 
 /**
@@ -160,6 +161,10 @@ private:
 	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
 	bool		_debug;				/**< if set to true, print debug output */
 
+	float _flaps_cmd_last;
+	float _flaperons_cmd_last;
+
+
 	struct {
 		float tconst;
 		float p_p;
@@ -181,6 +186,11 @@ private:
 		float y_coordinated_min_speed;
 		int32_t y_coordinated_method;
 		float y_rmax;
+		float w_p;
+		float w_i;
+		float w_ff;
+		float w_integrator_max;
+		float w_rmax;
 
 		float airspeed_min;
 		float airspeed_trim;
@@ -196,6 +206,9 @@ private:
 		float man_roll_max;				/**< Max Roll in rad */
 		float man_pitch_max;			/**< Max Pitch in rad */
 
+		float flaps_scale;				/**< Scale factor for flaps */
+		float flaperon_scale;			/**< Scale factor for flaperons */
+
 		int vtol_type;					/**< VTOL type: 0 = tailsitter, 1 = tiltrotor */
 
 	}		_parameters;			/**< local copies of interesting parameters */
@@ -222,6 +235,11 @@ private:
 		param_t y_coordinated_min_speed;
 		param_t y_coordinated_method;
 		param_t y_rmax;
+		param_t w_p;
+		param_t w_i;
+		param_t w_ff;
+		param_t w_integrator_max;
+		param_t w_rmax;
 
 		param_t airspeed_min;
 		param_t airspeed_trim;
@@ -235,6 +253,9 @@ private:
 		param_t man_roll_max;
 		param_t man_pitch_max;
 
+		param_t flaps_scale;
+		param_t flaperon_scale;
+
 		param_t vtol_type;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
@@ -248,6 +269,7 @@ private:
 	ECL_RollController				_roll_ctrl;
 	ECL_PitchController				_pitch_ctrl;
 	ECL_YawController				_yaw_ctrl;
+	ECL_WheelController			_wheel_ctrl;
 
 
 	/**
@@ -345,7 +367,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
 /* states */
 	_setpoint_valid(false),
-	_debug(false)
+	_debug(false),
+	_flaps_cmd_last(0),
+	_flaperons_cmd_last(0)
 {
 	/* safely initialize structs */
 	_ctrl_state = {};
@@ -380,6 +404,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
 	_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
 
+	_parameter_handles.w_p = param_find("FW_WR_P");
+	_parameter_handles.w_i = param_find("FW_WR_I");
+	_parameter_handles.w_ff = param_find("FW_WR_FF");
+	_parameter_handles.w_integrator_max = param_find("FW_WR_IMAX");
+	_parameter_handles.w_rmax = param_find("FW_W_RMAX");
+
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
@@ -396,6 +426,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
 	_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
 
+	_parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL");
+	_parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL");
+
 	_parameter_handles.vtol_type = param_find("VT_TYPE");
 
 	/* fetch initial parameter values */
@@ -458,6 +491,12 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method));
 	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
 
+	param_get(_parameter_handles.w_p, &(_parameters.w_p));
+	param_get(_parameter_handles.w_i, &(_parameters.w_i));
+	param_get(_parameter_handles.w_ff, &(_parameters.w_ff));
+	param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max));
+	param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax));
+
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
 	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
@@ -474,6 +513,9 @@ FixedwingAttitudeControl::parameters_update()
 	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
 	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
 
+	param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale);
+	param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale);
+
 	param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
 
 	/* pitch control parameters */
@@ -502,6 +544,13 @@ FixedwingAttitudeControl::parameters_update()
 	_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
 	_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
 
+	/* wheel control parameters */
+	_wheel_ctrl.set_k_p(_parameters.w_p);
+	_wheel_ctrl.set_k_i(_parameters.w_i);
+	_wheel_ctrl.set_k_ff(_parameters.w_ff);
+	_wheel_ctrl.set_integrator_max(_parameters.w_integrator_max);
+	_wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax));
+
 	return OK;
 }
 
@@ -751,6 +800,10 @@ FixedwingAttitudeControl::task_main()
 
 			vehicle_status_poll();
 
+			// the position controller will not emit attitude setpoints in some modes
+			// we need to make sure that this flag is reset
+			_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
+
 			/* lock integrator until control is started */
 			bool lock_integrator;
 
@@ -778,9 +831,53 @@ FixedwingAttitudeControl::task_main()
 			/* default flaps to center */
 			float flaps_control = 0.0f;
 
+			static float delta_flaps = 0;
+
 			/* map flaps by default to manual if valid */
-			if (PX4_ISFINITE(_manual.flaps)) {
-				flaps_control = _manual.flaps;
+			if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) {
+				flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale;
+			} else if (_vcontrol_mode.flag_control_auto_enabled) {
+				flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
+			}
+
+			// move the actual control value continuous with time
+			static hrt_abstime t_flaps_changed = 0;
+			if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) {
+				t_flaps_changed = hrt_absolute_time();
+				delta_flaps = flaps_control - _flaps_cmd_last;
+				_flaps_cmd_last = flaps_control;
+			}
+
+			static float flaps_applied = 0.0f;
+
+			if (fabsf(flaps_applied - flaps_control) > 0.01f) {
+				flaps_applied = (flaps_control - delta_flaps) + (float)hrt_elapsed_time(&t_flaps_changed) * (delta_flaps) / 1000000;
+			}
+
+			/* default flaperon to center */
+			float flaperon = 0.0f;
+
+			static float delta_flaperon = 0.0f;
+
+			/* map flaperons by default to manual if valid */
+			if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) {
+				flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
+			} else if (_vcontrol_mode.flag_control_auto_enabled) {
+				flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
+			}
+
+			// move the actual control value continuous with time
+			static hrt_abstime t_flaperons_changed = 0;
+			if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) {
+				t_flaperons_changed = hrt_absolute_time();
+				delta_flaperon = flaperon - _flaperons_cmd_last;
+				_flaperons_cmd_last = flaperon;
+			}
+
+			static float flaperon_applied = 0.0f;
+
+			if (fabsf(flaperon_applied - flaperon) > 0.01f) {
+				flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * (delta_flaperon) / 1000000;
 			}
 
 			/* decide if in stabilized or full manual control */
@@ -806,11 +903,19 @@ FixedwingAttitudeControl::task_main()
 				 *
 				 * Forcing the scaling to this value allows reasonable handheld tests.
 				 */
-
 				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
 
+				/* Use min airspeed to calculate ground speed scaling region.
+				 * Don't scale below gspd_scaling_trim
+				 */
+				float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
+						_global_pos.vel_e * _global_pos.vel_e);
+				float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f);
+				float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
+
 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
+				float yaw_sp = 0.0f;
 				float yaw_manual = 0.0f;
 				float throttle_sp = 0.0f;
 
@@ -824,6 +929,7 @@ FixedwingAttitudeControl::task_main()
 					/* read in attitude setpoint from attitude setpoint uorb topic */
 					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
 					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
+					yaw_sp = _att_sp.yaw_body;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
@@ -835,6 +941,7 @@ FixedwingAttitudeControl::task_main()
 					}
 					if (_att_sp.yaw_reset_integral) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 					}
 				} else if (_vcontrol_mode.flag_control_velocity_enabled) {
 
@@ -860,6 +967,7 @@ FixedwingAttitudeControl::task_main()
 					}
 					if (_att_sp.yaw_reset_integral) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 					}
 
 				} else if (_vcontrol_mode.flag_control_altitude_enabled) {
@@ -879,6 +987,7 @@ FixedwingAttitudeControl::task_main()
 					}
 					if (_att_sp.yaw_reset_integral) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 					}
 				} else {
 					/*
@@ -929,6 +1038,7 @@ FixedwingAttitudeControl::task_main()
 					_roll_ctrl.reset_integrator();
 					_pitch_ctrl.reset_integrator();
 					_yaw_ctrl.reset_integrator();
+					_wheel_ctrl.reset_integrator();
 				}
 
 				/* Prepare speed_body_u and speed_body_w */
@@ -952,17 +1062,23 @@ FixedwingAttitudeControl::task_main()
 				control_input.acc_body_z = _accel.z;
 				control_input.roll_setpoint = roll_sp;
 				control_input.pitch_setpoint = pitch_sp;
+				control_input.yaw_setpoint = yaw_sp;
 				control_input.airspeed_min = _parameters.airspeed_min;
 				control_input.airspeed_max = _parameters.airspeed_max;
 				control_input.airspeed = airspeed;
 				control_input.scaler = airspeed_scaling;
 				control_input.lock_integrator = lock_integrator;
+				control_input.groundspeed = groundspeed;
+				control_input.groundspeed_scaler = groundspeed_scaler;
+
+				_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
 
 				/* Run attitude controllers */
 				if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
 					_roll_ctrl.control_attitude(control_input);
 					_pitch_ctrl.control_attitude(control_input);
 					_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
+					_wheel_ctrl.control_attitude(control_input);
 
 					/* Update input data for rate controllers */
 					control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
@@ -1002,13 +1118,21 @@ FixedwingAttitudeControl::task_main()
 						}
 					}
 
-					float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
+					float yaw_u = 0.0f;
+					if (_att_sp.fw_control_yaw == true) {
+						yaw_u = _wheel_ctrl.control_bodyrate(control_input);
+					}
+
+					else {
+						yaw_u = _yaw_ctrl.control_bodyrate(control_input);
+					}
 					_actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
 
 					/* add in manual rudder control */
 					_actuators.control[2] += yaw_manual;
 					if (!PX4_ISFINITE(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 						if (_debug && loop_counter % 10 == 0) {
 							warnx("yaw_u %.4f", (double)yaw_u);
@@ -1059,9 +1183,9 @@ FixedwingAttitudeControl::task_main()
 				_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
 			}
 
-			_actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control;
+			_actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_applied;
 			_actuators.control[5] = _manual.aux1;
-			_actuators.control[6] = _manual.aux2;
+			_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon_applied;
 			_actuators.control[7] = _manual.aux3;
 
 			/* lazily publish the setpoint only once available */

src/modules/fw_att_control/fw_att_control_params.c

@@ -220,6 +220,55 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
  */
 PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
 
+/**
+ * Wheel steering rate proportional gain
+ *
+ * This defines how much the wheel steering input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @min 0.005
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f);
+
+/**
+ * Wheel steering rate integrator gain
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 50.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f);
+
+/**
+ * Wheel steering rate integrator limit
+ *
+ * The portion of the integrator part in the control surface deflection is
+ * limited to this value
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f);
+
+/**
+ * Maximum wheel steering rate
+ *
+ * This limits the maximum wheel steering rate the controller will output (in degrees per
+ * second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f);
+
 /**
  * Roll rate feed forward
  *
@@ -255,6 +304,17 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
  */
 PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
 
+/**
+ * Wheel steering rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f);
+
 /**
  * Minimal speed for yaw coordination
  *
@@ -374,3 +434,21 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
+
+/**
+ * Scale factor for flaps
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f);
+
+/**
+ * Scale factor for flaperons
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -49,6 +49,7 @@
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Andreas Antener <andreas@uaventure.com>
  */
 
 #include <px4_config.h>
@@ -95,6 +96,7 @@
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
 #include <platforms/px4_defines.h>
+#include <runway_takeoff/RunwayTakeoff.h>
 
 static int	_control_task = -1;			/**< task handle for sensor task */
 #define HDG_HOLD_DIST_NEXT 			3000.0f 	// initial distance of waypoint in front of plane in heading hold mode
@@ -103,6 +105,7 @@ static int	_control_task = -1;			/**< task handle for sensor task */
 #define HDG_HOLD_YAWRATE_THRESH 	0.15f 		// max yawrate at which plane locks yaw for heading hold mode
 #define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 		// max manual roll input from user which does not change the locked heading
 #define TAKEOFF_IDLE				0.2f 		// idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0)
+#define T_ALT_TIMEOUT 				1 			// time after which we abort landing if terrain estimate is not valid
 
 static constexpr float THROTTLE_THRESH = 0.05f; 	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
 static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f;	///< a throttle / pitch input above this value leads to the system switching to climbout mode
@@ -194,9 +197,17 @@ private:
 	bool land_onslope;
 	bool land_useterrain;
 
+	// landing relevant states
+	float _t_alt_prev_valid;	//**< last terrain estimate which was valid */
+	hrt_abstime _time_last_t_alt; //*< time at which we had last valid terrain alt */
+	hrt_abstime _time_started_landing;	//*< time at which landing started */
+	float height_flare;					//*< estimated height to ground at which flare started */
+
 	bool _was_in_air;	/**< indicated wether the plane was in the air in the previous interation*/
 	hrt_abstime _time_went_in_air;	/**< time at which the plane went in the air */
 
+	runwaytakeoff::RunwayTakeoff _runway_takeoff;
+
 	/* takeoff/launch states */
 	LaunchDetectionResult launch_detection_state;
 
@@ -277,6 +288,7 @@ private:
 		float land_flare_pitch_min_deg;
 		float land_flare_pitch_max_deg;
 		int land_use_terrain_estimate;
+		float land_airspeed_scale;
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -325,6 +337,7 @@ private:
 		param_t land_flare_pitch_min_deg;
 		param_t land_flare_pitch_max_deg;
 		param_t land_use_terrain_estimate;
+		param_t land_airspeed_scale;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -391,6 +404,11 @@ private:
 	 */
 	float		get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
 
+	/**
+	 * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
+	 */
+	float		get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos);
+
 	/**
 	 * Check if we are in a takeoff situation
 	 */
@@ -519,8 +537,13 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_motor_lim(false),
 	land_onslope(false),
 	land_useterrain(false),
+	_t_alt_prev_valid(0),
+	_time_last_t_alt(0),
+	_time_started_landing(0),
+	height_flare(0.0f),
 	_was_in_air(false),
 	_time_went_in_air(0),
+	_runway_takeoff(),
 	launch_detection_state(LAUNCHDETECTION_RES_NONE),
 	last_manual(false),
 	landingslope(),
@@ -562,6 +585,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_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.land_airspeed_scale = param_find("FW_AIRSPD_SCALE");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.time_const_throt = 			param_find("FW_T_THRO_CONST");
@@ -665,6 +689,7 @@ FixedwingPositionControl::parameters_update()
 	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));
+	param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale));
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
@@ -715,6 +740,8 @@ FixedwingPositionControl::parameters_update()
 	/* Update the mTecs */
 	_mTecs.updateParams();
 
+	_runway_takeoff.updateParams();
+
 	return OK;
 }
 
@@ -947,9 +974,9 @@ 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 (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) {
+	if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) {
 		if(!land_useterrain) {
-			mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate");
+			mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate");
 			land_useterrain = true;
 		}
 		return global_pos.terrain_alt;
@@ -958,6 +985,15 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
 	}
 }
 
+float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos)
+{
+	if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) {
+		return global_pos.terrain_alt;
+	}
+
+	return takeoff_alt;
+}
+
 bool FixedwingPositionControl::update_desired_altitude(float dt)
 {
 	/*
@@ -1054,6 +1090,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	bool setpoint = true;
 
+	_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
+	_att_sp.apply_flaps = false;		// by default we don't use flaps
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
 	/* filter speed and altitude for controller */
@@ -1171,19 +1209,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-			if (in_takeoff_situation()) {
+			float alt_sp;
+			if (_nav_capabilities.abort_landing == true) {
+				// if we entered loiter due to an aborted landing, demand
+				// altitude setpoint well above landing waypoint
+				alt_sp = _pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff;
+			} else {
+				// use altitude given by wapoint
+				alt_sp = _pos_sp_triplet.current.alt;
+			}
+
+			if (in_takeoff_situation() ||
+				((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff ) && _nav_capabilities.abort_landing == true)) {
 					/* limit roll motion to ensure enough lift */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
 							math::radians(15.0f));
 			}
 
-			tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
+			tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
 						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
 		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 
+			// apply full flaps for landings. this flag will also trigger the use of flaperons
+			// if they have been enabled using the corresponding parameter
+			_att_sp.apply_flaps = true;
+
+			// save time at which we started landing
+			if (_time_started_landing == 0) {
+				_time_started_landing = hrt_absolute_time();
+			}
+
 			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));
 
@@ -1196,7 +1254,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				wp_distance_save = 0.0f;
 			}
 
-			//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
+			// create virtual waypoint which is on the desired flight path but
+			// some distance behind landing waypoint. This will make sure that the plane
+			// will always follow the desired flight path even if we get close or past
+			// the landing waypoint
+			math::Vector<2> curr_wp_shifted;
+			double lat;
+			double lon;
+			create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon);
+			curr_wp_shifted(0) = (float)lat;
+			curr_wp_shifted(1) = (float)lon;
+
+			// we want the plane to keep tracking the desired flight path until we start flaring
+			// if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds
+			//if (land_noreturn_vertical) {
 			if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
 
 				/* heading hold, along the line connecting this and the last waypoint */
@@ -1207,7 +1278,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					} else {
 						target_bearing = _yaw;
 					}
-					mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
+					mavlink_log_info(_mavlink_fd, "#Landing, heading hold");
 				}
 
 //					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
@@ -1236,12 +1307,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			// XXX this could make a great param
 
 			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;
+			float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min;
+			float airspeed_approach = _parameters.land_airspeed_scale * _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);
+			float terrain_alt;
+			if (_parameters.land_use_terrain_estimate) {
+				if (_global_pos.terrain_alt_valid) {
+					// all good, have valid terrain altitude
+					terrain_alt = _global_pos.terrain_alt;
+					_t_alt_prev_valid = terrain_alt;
+					_time_last_t_alt = hrt_absolute_time();
+				} else if (_time_last_t_alt == 0) {
+					// we have started landing phase but don't have valid terrain
+					// wait for some time, maybe we will soon get a valid estimate
+					// until then just use the altitude of the landing waypoint
+					if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) {
+						terrain_alt = _pos_sp_triplet.current.alt;
+					} else {
+						// still no valid terrain, abort landing
+						terrain_alt = _pos_sp_triplet.current.alt;
+						_nav_capabilities.abort_landing = true;
+					}
+				} else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000)
+					|| land_noreturn_vertical) {
+					// use previous terrain estimate for some time and hope to recover
+					// if we are already flaring (land_noreturn_vertical) then just
+					//  go with the old estimate
+					terrain_alt = _t_alt_prev_valid;
+				} else {
+					// terrain alt was not valid for long time, abort landing
+					terrain_alt = _t_alt_prev_valid;
+					_nav_capabilities.abort_landing = true;
+				}
+			} else {
+				// no terrain estimation, just use landing waypoint altitude
+				terrain_alt = _pos_sp_triplet.current.alt;
+			}
+			
 
 			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
 			float L_altitude_rel = _pos_sp_triplet.previous.valid ?
@@ -1264,11 +1368,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* kill the throttle if param requests it */
 				throttle_max = _parameters.throttle_max;
 
+				/* enable direct yaw control using rudder/wheel */
+				_att_sp.yaw_body = target_bearing;
+				_att_sp.fw_control_yaw = true;
+
 				 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;
-						mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle");
+						mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle");
 					}
 
 				 }
@@ -1292,10 +1400,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND);
 
 				if (!land_noreturn_vertical) {
-					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
+					// just started with the flaring phase
+					_att_sp.pitch_body = 0.0f;
+					height_flare = _global_pos.alt - terrain_alt;
+					mavlink_log_info(_mavlink_fd, "#Landing, flaring");
 					land_noreturn_vertical = true;
+				} else {
+					if (_global_pos.vel_d > 0.1f) {
+						_att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) *
+								math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f);
+					} else {
+						_att_sp.pitch_body = _att_sp.pitch_body;
+					}
 				}
-				//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
 
 				flare_curve_alt_rel_last = flare_curve_alt_rel;
 			} else {
@@ -1314,7 +1431,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					/* stay on slope */
 					altitude_desired_rel = landing_slope_alt_rel_desired;
 					if (!land_onslope) {
-						mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
+						mavlink_log_info(_mavlink_fd, "#Landing, on slope");
 						land_onslope = true;
 					}
 				} else {
@@ -1338,94 +1455,160 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
-			/* Perform launch detection */
-			if (launchDetector.launchDetectionEnabled() &&
-					launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
-				/* Inform user that launchdetection is running */
-				static hrt_abstime last_sent = 0;
-				if(hrt_absolute_time() - last_sent > 4e6) {
-					mavlink_log_critical(_mavlink_fd, "Launchdetection running");
-					last_sent = hrt_absolute_time();
+			if (_runway_takeoff.runwayTakeoffEnabled()) {
+				if (!_runway_takeoff.isInitialized()) {
+					math::Quaternion q(&_ctrl_state.q[0]);
+					math::Vector<3> euler = q.to_euler();
+					_runway_takeoff.init(euler(2), _global_pos.lat, _global_pos.lon);
+
+					/* need this already before takeoff is detected
+					 * doesn't matter if it gets reset when takeoff is detected eventually */
+					_takeoff_ground_alt = _global_pos.alt;
+
+					mavlink_log_info(_mavlink_fd, "#Takeoff on runway");
 				}
 
-				/* Detect launch */
-				launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
+				float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos);
 
-				/* update our copy of the launch detection state */
-				launch_detection_state = launchDetector.getLaunchDetected();
-			} else	{
-				/* no takeoff detection --> fly */
-				launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
-			}
+				// update runway takeoff helper
+				_runway_takeoff.update(
+					_ctrl_state.airspeed,
+					_global_pos.alt - terrain_alt,
+					_global_pos.lat,
+					_global_pos.lon,
+					_mavlink_fd);
 
-			/* Set control values depending on the detection state */
-			if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
-				/* Launch has been detected, hence we have to control the plane. */
+				/*
+				 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
+				 * If we use the navigator heading or not is decided later.
+				 */
+				_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d);
 
-				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
-
-				/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
-				 * full throttle, otherwise we use the preTakeOff Throttle */
-				float takeoff_throttle = launch_detection_state !=
-					LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
-					launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
-
-				/* select maximum pitch: the launchdetector may impose another limit for the pitch
-				 * depending on the state of the launch */
-				float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
+				// update tecs
+				float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max);
 				float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
 
-				/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
-				 * meters */
-				if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
+				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
+						calculate_target_airspeed(
+							_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min),
+						eas2tas,
+						math::radians(_parameters.pitch_limit_min),
+						takeoff_pitch_max_rad,
+						_parameters.throttle_min,
+						_parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here?
+						_parameters.throttle_cruise,
+						_runway_takeoff.climbout(),
+						math::radians(_runway_takeoff.getMinPitch(
+							_pos_sp_triplet.current.pitch_min,
+							10.0f,
+							_parameters.pitch_limit_min)),
+						_global_pos.alt,
+						ground_speed,
+						tecs_status_s::TECS_MODE_TAKEOFF,
+						takeoff_pitch_max_deg != _parameters.pitch_limit_max);
 
-					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
-					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
-							calculate_target_airspeed(1.3f * _parameters.airspeed_min),
-							eas2tas,
-							math::radians(_parameters.pitch_limit_min),
-							takeoff_pitch_max_rad,
-							_parameters.throttle_min, takeoff_throttle,
-							_parameters.throttle_cruise,
-							true,
-							math::max(math::radians(pos_sp_triplet.current.pitch_min),
-							math::radians(10.0f)),
-							_global_pos.alt,
-							ground_speed,
-							tecs_status_s::TECS_MODE_TAKEOFF,
-							takeoff_pitch_max_deg != _parameters.pitch_limit_max);
+				// assign values
+				_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll());
+				_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
+				_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
+				_att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand());
 
-					/* limit roll motion to ensure enough lift */
-					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
-							math::radians(15.0f));
+				// reset integrals except yaw (which also counts for the wheel controller)
+				_att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators();
+				_att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators();
+
+				/*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body,
+					(double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/
 
-				} else {
-					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
-							calculate_target_airspeed(_parameters.airspeed_trim),
-							eas2tas,
-							math::radians(_parameters.pitch_limit_min),
-							math::radians(_parameters.pitch_limit_max),
-							_parameters.throttle_min,
-							takeoff_throttle,
-							_parameters.throttle_cruise,
-							false,
-							math::radians(_parameters.pitch_limit_min),
-							_global_pos.alt,
-							ground_speed);
-				}
 			} else {
-				/* Tell the attitude controller to stop integrating while we are waiting
-				 * for the launch */
-				_att_sp.roll_reset_integral = true;
-				_att_sp.pitch_reset_integral = true;
-				_att_sp.yaw_reset_integral = true;
+				/* Perform launch detection */
+				if (launchDetector.launchDetectionEnabled() &&
+					launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+					/* Inform user that launchdetection is running */
+					static hrt_abstime last_sent = 0;
+					if(hrt_absolute_time() - last_sent > 4e6) {
+						mavlink_log_critical(_mavlink_fd, "#Launchdetection running");
+						last_sent = hrt_absolute_time();
+					}
 
-				/* Set default roll and pitch setpoints during detection phase */
-				_att_sp.roll_body = 0.0f;
-				_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
-						math::radians(10.0f));
+					/* Detect launch */
+					launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
+
+					/* update our copy of the launch detection state */
+					launch_detection_state = launchDetector.getLaunchDetected();
+				} else	{
+					/* no takeoff detection --> fly */
+					launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+				}
+
+				/* Set control values depending on the detection state */
+				if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
+					/* Launch has been detected, hence we have to control the plane. */
+
+					_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+					_att_sp.roll_body = _l1_control.nav_roll();
+					_att_sp.yaw_body = _l1_control.nav_bearing();
+
+					/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
+					 * full throttle, otherwise we use the preTakeOff Throttle */
+					float takeoff_throttle = launch_detection_state !=
+								LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
+								launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
+
+					/* select maximum pitch: the launchdetector may impose another limit for the pitch
+					 * depending on the state of the launch */
+					float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
+					float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
+
+					/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
+					 * meters */
+					if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
+						/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
+						tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
+								calculate_target_airspeed(1.3f * _parameters.airspeed_min),
+								eas2tas,
+								math::radians(_parameters.pitch_limit_min),
+								takeoff_pitch_max_rad,
+								_parameters.throttle_min, takeoff_throttle,
+								_parameters.throttle_cruise,
+								true,
+								math::max(math::radians(_pos_sp_triplet.current.pitch_min),
+								math::radians(10.0f)),
+								_global_pos.alt,
+								ground_speed,
+								tecs_status_s::TECS_MODE_TAKEOFF,
+								takeoff_pitch_max_deg != _parameters.pitch_limit_max);
+
+						/* limit roll motion to ensure enough lift */
+						_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
+								math::radians(15.0f));
+
+					} else {
+						tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
+								calculate_target_airspeed(_parameters.airspeed_trim),
+								eas2tas,
+								math::radians(_parameters.pitch_limit_min),
+								math::radians(_parameters.pitch_limit_max),
+								_parameters.throttle_min,
+								takeoff_throttle,
+								_parameters.throttle_cruise,
+								false,
+								math::radians(_parameters.pitch_limit_min),
+								_global_pos.alt,
+								ground_speed);
+					}
+				} else {
+					/* Tell the attitude controller to stop integrating while we are waiting
+					 * for the launch */
+					_att_sp.roll_reset_integral = true;
+					_att_sp.pitch_reset_integral = true;
+					_att_sp.yaw_reset_integral = true;
+
+					/* Set default roll and pitch setpoints during detection phase */
+					_att_sp.roll_body = 0.0f;
+					_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
+							math::radians(10.0f));
+				}
 			}
 
 		}
@@ -1620,9 +1803,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/** MANUAL FLIGHT **/
 
-		/* reset hold altitude */
+		// reset hold altitude
 		_hold_alt = _global_pos.alt;
 
+		// reset terrain estimation relevant values
+		_time_started_landing = 0;
+		_time_last_t_alt = 0;
+
+		// reset lading abort state
+		_nav_capabilities.abort_landing = false;
+
 		/* no flight mode applies, do not publish an attitude setpoint */
 		setpoint = false;
 
@@ -1637,15 +1827,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
 		/* Set thrust to 0 to minimize damage */
 		_att_sp.thrust = 0.0f;
+
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
 			pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
-			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
+			!_runway_takeoff.runwayTakeoffEnabled()) {
 		 /* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons */
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
+
+	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
+			pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
+			_runway_takeoff.runwayTakeoffEnabled()) {
+		_att_sp.thrust = _runway_takeoff.getThrottle(
+				math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
+				_tecs.get_throttle_demand(), throttle_max));
+
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
 			pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 		_att_sp.thrust = 0.0f;
+
 	} else {
 		/* Copy thrust and pitch values from tecs */
 		if (_vehicle_status.condition_landed &&
@@ -1663,9 +1864,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	/* During a takeoff waypoint while waiting for launch the pitch sp is set
 	 * already (not by tecs) */
-	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
-				pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
-				launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
+	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO && (
+				(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
+					(launch_detection_state == LAUNCHDETECTION_RES_NONE ||
+					_runway_takeoff.runwayTakeoffEnabled())) ||
+				(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND &&
+					land_noreturn_vertical))
+	)) {
 		_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 	}
 
@@ -1825,6 +2030,7 @@ void FixedwingPositionControl::reset_takeoff_state()
 {
 	launch_detection_state = LAUNCHDETECTION_RES_NONE;
 	launchDetector.reset();
+	_runway_takeoff.reset();
 }
 
 void FixedwingPositionControl::reset_landing_state()

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -443,3 +443,15 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f);
  *
  */
 PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f);
+
+/**
+ * Takeoff and landing airspeed scale factor
+ *
+ * Multiplying this factor with the minimum airspeed of the plane
+ * gives the target airspeed for takeoff and landing approach.
+ *
+ * @min 1.0
+ * @max 1.5
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_SCALE, 1.3f);

src/modules/navigator/navigator_main.cpp

@@ -440,8 +440,15 @@ Navigator::task_main()
 				_can_loiter_at_sp = false;
 				break;
 			case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-				_pos_sp_triplet_published_invalid_once = false;
-				_navigation_mode = &_mission;
+				if (_nav_caps.abort_landing) {
+					// pos controller aborted landing, requests loiter
+					// above landing waypoint
+					_navigation_mode = &_loiter;
+					_pos_sp_triplet_published_invalid_once = false;
+				} else {
+					_pos_sp_triplet_published_invalid_once = false;
+					_navigation_mode = &_mission;
+				}
 				break;
 			case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 				_pos_sp_triplet_published_invalid_once = false;