PX4-Autopilot/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

/****************************************************************************
 *
 *   Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * 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.
 *
 ****************************************************************************/

#include "FixedwingPositionControl.hpp"

#include <px4_platform_common/events.h>

using math::constrain;
using math::max;
using math::min;
using math::radians;

using matrix::Dcmf;
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Vector2f;
using matrix::Vector2d;
using matrix::Vector3f;
using matrix::wrap_pi;

FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
	_launchDetector(this),
	_runway_takeoff(this)
{
	if (vtol) {
		_param_handle_airspeed_trans = param_find("VT_ARSP_TRANS");
	}

	// limit to 50 Hz
	_local_pos_sub.set_interval_ms(20);

	_pos_ctrl_status_pub.advertise();
	_pos_ctrl_landing_status_pub.advertise();
	_tecs_status_pub.advertise();

	_airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE);

	/* fetch initial parameter values */
	parameters_update();
}

FixedwingPositionControl::~FixedwingPositionControl()
{
	perf_free(_loop_perf);
}

bool
FixedwingPositionControl::init()
{
	if (!_local_pos_sub.registerCallback()) {
		PX4_ERR("callback registration failed");
		return false;
	}

	return true;
}

int
FixedwingPositionControl::parameters_update()
{
	updateParams();

	// VTOL parameter VT_ARSP_TRANS
	if (_param_handle_airspeed_trans != PARAM_INVALID) {
		param_get(_param_handle_airspeed_trans, &_param_airspeed_trans);
	}

	// L1 control parameters
	_l1_control.set_l1_damping(_param_fw_l1_damping.get());
	_l1_control.set_l1_period(_param_fw_l1_period.get());
	_l1_control.set_l1_roll_limit(radians(_param_fw_r_lim.get()));
	_l1_control.set_roll_slew_rate(radians(_param_fw_l1_r_slew_max.get()));

	// NPFG parameters
	_npfg.setPeriod(_param_npfg_period.get());
	_npfg.setDamping(_param_npfg_damping.get());
	_npfg.enablePeriodLB(_param_npfg_en_period_lb.get());
	_npfg.enablePeriodUB(_param_npfg_en_period_ub.get());
	_npfg.enableMinGroundSpeed(_param_npfg_en_min_gsp.get());
	_npfg.enableTrackKeeping(_param_npfg_en_track_keeping.get());
	_npfg.enableWindExcessRegulation(_param_npfg_en_wind_reg.get());
	_npfg.setMinGroundSpeed(_param_fw_gnd_spd_min.get());
	_npfg.setMaxTrackKeepingMinGroundSpeed(_param_npfg_track_keeping_gsp_max.get());
	_npfg.setRollTimeConst(_param_npfg_roll_time_const.get());
	_npfg.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get());
	_npfg.setRollLimit(radians(_param_fw_r_lim.get()));
	_npfg.setRollSlewRate(radians(_param_fw_l1_r_slew_max.get()));
	_npfg.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get());

	// TECS parameters
	_tecs.set_max_climb_rate(_param_fw_t_clmb_max.get());
	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
	_tecs.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get());
	_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get());
	_tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get());
	_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
	_tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
	_tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());
	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
	_tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get());
	_tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
	_tecs.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get());
	_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
	_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
	_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
	_tecs.set_heightrate_ff(_param_fw_t_hrate_ff.get());
	_tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
	_tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get());
	_tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get());
	_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());

	int check_ret = PX4_OK;

	// sanity check parameters
	if (_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) {
		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max smaller than min\t");
		/* EVENT
		 * @description
		 * - <param>FW_AIRSPD_MAX</param>: {1:.1}
		 * - <param>FW_AIRSPD_MIN</param>: {2:.1}
		 */
		events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_airspeed"), events::Log::Error,
					   "Invalid configuration: Airspeed max smaller than min",
					   _param_fw_airspd_max.get(), _param_fw_airspd_min.get());
		check_ret = PX4_ERROR;
	}

	if (_param_fw_airspd_max.get() < 5.0f || _param_fw_airspd_min.get() > 100.0f) {
		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max < 5 m/s or min > 100 m/s\t");
		/* EVENT
		 * @description
		 * - <param>FW_AIRSPD_MAX</param>: {1:.1}
		 * - <param>FW_AIRSPD_MIN</param>: {2:.1}
		 */
		events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_airspeed_bounds"), events::Log::Error,
					   "Invalid configuration: Airspeed max \\< 5 m/s or min \\> 100 m/s",
					   _param_fw_airspd_max.get(), _param_fw_airspd_min.get());
		check_ret = PX4_ERROR;
	}

	if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() ||
	    _param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) {
		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed trim out of min or max bounds\t");
		/* EVENT
		 * @description
		 * - <param>FW_AIRSPD_MAX</param>: {1:.1}
		 * - <param>FW_AIRSPD_MIN</param>: {2:.1}
		 * - <param>FW_AIRSPD_TRIM</param>: {3:.1}
		 */
		events::send<float, float, float>(events::ID("fixedwing_position_control_conf_invalid_trim_bounds"),
						  events::Log::Error,
						  "Invalid configuration: Airspeed trim out of min or max bounds",
						  _param_fw_airspd_max.get(), _param_fw_airspd_min.get(), _param_fw_airspd_trim.get());
		check_ret = PX4_ERROR;
	}

	if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get() * 0.9f) {
		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Stall airspeed higher than 0.9 of min\t");
		/* EVENT
		 * @description
		 * - <param>FW_AIRSPD_MIN</param>: {1:.1}
		 * - <param>FW_AIRSPD_STALL</param>: {2:.1}
		 */
		events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_stall"), events::Log::Error,
					   "Invalid configuration: Stall airspeed higher than 90% of minimum airspeed",
					   _param_fw_airspd_min.get(), _param_fw_airspd_stall.get());
		check_ret = PX4_ERROR;
	}

	return check_ret;
}

void
FixedwingPositionControl::vehicle_control_mode_poll()
{
	if (_control_mode_sub.updated()) {
		const bool was_armed = _control_mode.flag_armed;

		if (_control_mode_sub.copy(&_control_mode)) {

			// reset state when arming
			if (!was_armed && _control_mode.flag_armed) {
				reset_takeoff_state();
				reset_landing_state();
			}
		}
	}
}

void
FixedwingPositionControl::vehicle_command_poll()
{
	vehicle_command_s vehicle_command;

	while (_vehicle_command_sub.update(&vehicle_command)) {
		if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
			// only abort landing before point of no return (horizontal and vertical)
			if (_control_mode.flag_control_auto_enabled &&
			    _pos_sp_triplet.current.valid &&
			    (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {

				abort_landing(true);
			}

		} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {

			if ((static_cast<uint8_t>(vehicle_command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_AIRSPEED)) {
				if (vehicle_command.param2 > FLT_EPSILON) {	// param2 is an equivalent airspeed setpoint
					if (_control_mode_current == FW_POSCTRL_MODE_AUTO) {
						_pos_sp_triplet.current.cruising_speed = vehicle_command.param2;

					} else if (_control_mode_current == FW_POSCTRL_MODE_MANUAL_ALTITUDE
						   || _control_mode_current == FW_POSCTRL_MODE_MANUAL_POSITION) {
						_commanded_manual_airspeed_setpoint = vehicle_command.param2;
					}

				}
			}

		}
	}
}

void
FixedwingPositionControl::airspeed_poll()
{
	bool airspeed_valid = _airspeed_valid;
	airspeed_validated_s airspeed_validated;

	if ((_param_fw_arsp_mode.get() == 0) && _airspeed_validated_sub.update(&airspeed_validated)) {

		_eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed

		if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
		    && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
		    && (airspeed_validated.calibrated_airspeed_m_s > 0.0f)) {

			airspeed_valid = true;

			_time_airspeed_last_valid = airspeed_validated.timestamp;
			_airspeed = airspeed_validated.calibrated_airspeed_m_s;

			_eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
		}

	} else {
		// no airspeed updates for one second
		if (airspeed_valid && (hrt_elapsed_time(&_time_airspeed_last_valid) > 1_s)) {
			airspeed_valid = false;
		}
	}

	// update TECS if validity changed
	if (airspeed_valid != _airspeed_valid) {
		_tecs.enable_airspeed(airspeed_valid);
		_airspeed_valid = airspeed_valid;
	}
}

void
FixedwingPositionControl::wind_poll()
{
	if (_wind_sub.updated()) {
		wind_s wind;
		_wind_sub.update(&wind);

		// assumes wind is valid if finite
		_wind_valid = PX4_ISFINITE(wind.windspeed_north)
			      && PX4_ISFINITE(wind.windspeed_east);

		_time_wind_last_received = hrt_absolute_time();

		_wind_vel(0) = wind.windspeed_north;
		_wind_vel(1) = wind.windspeed_east;

	} else {
		// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
		_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT;
	}
}

void
FixedwingPositionControl::manual_control_setpoint_poll()
{
	_manual_control_setpoint_sub.update(&_manual_control_setpoint);

	_manual_control_setpoint_for_height_rate = _manual_control_setpoint.x;
	_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);

	if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_SWAP_STICKS_BIT) {
		/* Alternate stick allocation (similar concept as for multirotor systems:
		 * demanding up/down with the throttle stick, and move faster/break with the pitch one.
		 */
		_manual_control_setpoint_for_height_rate = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f);
		_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
	}

	// send neutral setpoints if no update for 1 s
	if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) {
		_manual_control_setpoint_for_height_rate = 0.f;
		_manual_control_setpoint_for_airspeed = 0.5f;
	}
}


void
FixedwingPositionControl::vehicle_attitude_poll()
{
	vehicle_attitude_s att;

	if (_vehicle_attitude_sub.update(&att)) {
		vehicle_angular_velocity_s angular_velocity{};
		_vehicle_angular_velocity_sub.copy(&angular_velocity);
		const Vector3f rates{angular_velocity.xyz};

		Dcmf R{Quatf(att.q)};

		// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
		// between multirotor and fixed wing flight
		if (_vehicle_status.is_vtol_tailsitter) {
			const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}};
			R = R * R_offset;

			_yawrate = rates(0);

		} else {
			_yawrate = rates(2);
		}

		const Eulerf euler_angles(R);
		_pitch = euler_angles(1);
		_yaw = euler_angles(2);

		_body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
		_body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz};

		// load factor due to banking
		const float load_factor = 1.f / cosf(euler_angles(0));
		_tecs.set_load_factor(load_factor);
	}
}

float
FixedwingPositionControl::get_manual_airspeed_setpoint()
{
	float altctrl_airspeed = _param_fw_airspd_trim.get();

	if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) {
		// neutral throttle corresponds to trim airspeed
		if (_manual_control_setpoint_for_airspeed < 0.5f) {
			// lower half of throttle is min to trim airspeed
			altctrl_airspeed = _param_fw_airspd_min.get() +
					   (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) *
					   _manual_control_setpoint_for_airspeed * 2;

		} else {
			// upper half of throttle is trim to max airspeed
			altctrl_airspeed = _param_fw_airspd_trim.get() +
					   (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) *
					   (_manual_control_setpoint_for_airspeed * 2 - 1);
		}

	} else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) {
		altctrl_airspeed = _commanded_manual_airspeed_setpoint;
	}

	return altctrl_airspeed;
}

float
FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interval, const float pos_sp_cruise_airspeed,
		const Vector2f &ground_speed)
{
	// overwrite internal setpoint (e.g. set prior through MAV_CMD_DO_CHANGE_SPEED) in case
	// the current position_setpoint contains a valid airspeed setpoint
	float airspeed_setpoint = _param_fw_airspd_trim.get();

	if (PX4_ISFINITE(pos_sp_cruise_airspeed) && pos_sp_cruise_airspeed > FLT_EPSILON) {
		airspeed_setpoint = pos_sp_cruise_airspeed;
	}

	// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
	if (!_l1_control.circle_mode() && !using_npfg_with_wind_estimate()) {
		/*
		 * This error value ensures that a plane (as long as its throttle capability is
		 * not exceeded) travels towards a waypoint (and is not pushed more and more away
		 * by wind). Not countering this would lead to a fly-away. Only non-zero in presence
		 * of sufficient wind. "minimum ground speed undershoot".
		 */
		const float ground_speed_body = _body_velocity(0);

		if (ground_speed_body < _param_fw_gnd_spd_min.get()) {
			airspeed_setpoint += _param_fw_gnd_spd_min.get() - ground_speed_body;
		}
	}

	float airspeed_min_adjusted = _param_fw_airspd_min.get();

	float load_factor = 1.0f;

	if (PX4_ISFINITE(_att_sp.roll_body)) {
		load_factor = 1.0f / sqrtf(cosf(_att_sp.roll_body));
	}

	float weight_ratio = 1.0f;

	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
		weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO,
					       MAX_WEIGHT_RATIO);
	}

	// Stall speed increases with the square root of the load factor times the weight ratio
	// Vs ~ sqrt(load_factor * weight_ratio)
	airspeed_min_adjusted = constrain(_param_fw_airspd_stall.get() * sqrtf(load_factor * weight_ratio),
					  airspeed_min_adjusted, _param_fw_airspd_max.get());

	// constrain setpoint
	airspeed_setpoint = constrain(airspeed_setpoint, airspeed_min_adjusted, _param_fw_airspd_max.get());

	// initialize to current airspeed setpoint, also if previous setpoint is out of bounds to not apply slew rate in that case
	const bool outside_of_limits = _airspeed_slew_rate_controller.getState() < airspeed_min_adjusted
				       || _airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get();

	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) || outside_of_limits) {
		_airspeed_slew_rate_controller.setForcedValue(airspeed_setpoint);

	} else if (control_interval > FLT_EPSILON) {
		// constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s
		airspeed_setpoint = _airspeed_slew_rate_controller.update(airspeed_setpoint, control_interval);
	}

	return airspeed_setpoint;
}

void
FixedwingPositionControl::tecs_status_publish()
{
	tecs_status_s t{};

	switch (_tecs.tecs_mode()) {
	case TECS::ECL_TECS_MODE_NORMAL:
		t.mode = tecs_status_s::TECS_MODE_NORMAL;
		break;

	case TECS::ECL_TECS_MODE_UNDERSPEED:
		t.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
		break;

	case TECS::ECL_TECS_MODE_BAD_DESCENT:
		t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT;
		break;

	case TECS::ECL_TECS_MODE_CLIMBOUT:
		t.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
		break;
	}

	t.altitude_sp = _tecs.hgt_setpoint();
	t.altitude_filtered = _tecs.vert_pos_state();

	t.true_airspeed_sp = _tecs.TAS_setpoint_adj();
	t.true_airspeed_filtered = _tecs.tas_state();

	t.height_rate_setpoint = _tecs.hgt_rate_setpoint();
	t.height_rate = _tecs.vert_vel_state();

	t.equivalent_airspeed_sp = _tecs.get_EAS_setpoint();
	t.true_airspeed_derivative_sp = _tecs.TAS_rate_setpoint();
	t.true_airspeed_derivative = _tecs.speed_derivative();
	t.true_airspeed_derivative_raw = _tecs.speed_derivative_raw();
	t.true_airspeed_innovation = _tecs.getTASInnovation();

	t.total_energy_error = _tecs.STE_error();
	t.total_energy_rate_error = _tecs.STE_rate_error();

	t.energy_distribution_error = _tecs.SEB_error();
	t.energy_distribution_rate_error = _tecs.SEB_rate_error();

	t.total_energy = _tecs.STE();
	t.total_energy_rate = _tecs.STE_rate();
	t.total_energy_balance = _tecs.SEB();
	t.total_energy_balance_rate = _tecs.SEB_rate();

	t.total_energy_sp = _tecs.STE_setpoint();
	t.total_energy_rate_sp = _tecs.STE_rate_setpoint();
	t.total_energy_balance_sp = _tecs.SEB_setpoint();
	t.total_energy_balance_rate_sp = _tecs.SEB_rate_setpoint();

	t.throttle_integ = _tecs.throttle_integ_state();
	t.pitch_integ = _tecs.pitch_integ_state();

	t.throttle_sp = _tecs.get_throttle_setpoint();
	t.pitch_sp_rad = _tecs.get_pitch_setpoint();
	t.throttle_trim = _tecs.get_throttle_trim();

	t.timestamp = hrt_absolute_time();

	_tecs_status_pub.publish(t);
}

void
FixedwingPositionControl::status_publish()
{
	position_controller_status_s pos_ctrl_status = {};

	pos_ctrl_status.nav_roll = _att_sp.roll_body;
	pos_ctrl_status.nav_pitch = _att_sp.pitch_body;

	if (_l1_control.has_guidance_updated()) {
		pos_ctrl_status.nav_bearing = _l1_control.nav_bearing();

		pos_ctrl_status.target_bearing = _l1_control.target_bearing();
		pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();

	} else {
		pos_ctrl_status.nav_bearing = NAN;
		pos_ctrl_status.target_bearing = NAN;
		pos_ctrl_status.xtrack_error = NAN;

	}

	if (_param_fw_use_npfg.get()) {
		npfg_status_s npfg_status = {};

		npfg_status.wind_est_valid = _wind_valid;

		const float bearing = _npfg.getBearing(); // dont repeat atan2 calc

		pos_ctrl_status.nav_bearing = bearing;
		pos_ctrl_status.target_bearing = _npfg.targetBearing();
		pos_ctrl_status.xtrack_error = _npfg.getTrackError();
		pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f);

		npfg_status.lat_accel = _npfg.getLateralAccel();
		npfg_status.lat_accel_ff = _npfg.getLateralAccelFF();
		npfg_status.heading_ref = _npfg.getHeadingRef();
		npfg_status.bearing = bearing;
		npfg_status.bearing_feas = _npfg.getBearingFeas();
		npfg_status.bearing_feas_on_track = _npfg.getOnTrackBearingFeas();
		npfg_status.signed_track_error = _npfg.getTrackError();
		npfg_status.track_error_bound = _npfg.getTrackErrorBound();
		npfg_status.airspeed_ref = _npfg.getAirspeedRef();
		npfg_status.min_ground_speed_ref = _npfg.getMinGroundSpeedRef();
		npfg_status.adapted_period = _npfg.getAdaptedPeriod();
		npfg_status.p_gain = _npfg.getPGain();
		npfg_status.time_const = _npfg.getTimeConst();
		npfg_status.timestamp = hrt_absolute_time();

		_npfg_status_pub.publish(npfg_status);

	} else {
		pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f);
	}

	pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude,
				  _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);

	pos_ctrl_status.yaw_acceptance = NAN;

	pos_ctrl_status.timestamp = hrt_absolute_time();

	pos_ctrl_status.type = _position_sp_type;

	_pos_ctrl_status_pub.publish(pos_ctrl_status);
}

void
FixedwingPositionControl::landing_status_publish()
{
	position_controller_landing_status_s pos_ctrl_landing_status = {};

	pos_ctrl_landing_status.lateral_touchdown_offset = _lateral_touchdown_position_offset;
	pos_ctrl_landing_status.abort_landing = _land_abort;
	pos_ctrl_landing_status.timestamp = hrt_absolute_time();

	_pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status);
}

void
FixedwingPositionControl::abort_landing(bool abort)
{
	// only announce changes
	if (abort && !_land_abort) {
		mavlink_log_critical(&_mavlink_log_pub, "Landing aborted\t");
		// TODO: add reason
		events::send(events::ID("fixedwing_position_control_land_aborted"), events::Log::Critical, "Landing aborted");
	}

	_land_abort = abort;
	landing_status_publish();
}

void
FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev,
		position_setpoint_s &waypoint_next, bool flag_init)
{
	position_setpoint_s temp_prev = waypoint_prev;
	position_setpoint_s temp_next = waypoint_next;

	if (flag_init) {
		// previous waypoint: HDG_HOLD_SET_BACK_DIST meters behind us
		waypoint_from_heading_and_distance(_current_latitude, _current_longitude, heading + radians(180.0f),
						   HDG_HOLD_SET_BACK_DIST, &temp_prev.lat, &temp_prev.lon);

		// next waypoint: HDG_HOLD_DIST_NEXT meters in front of us
		waypoint_from_heading_and_distance(_current_latitude, _current_longitude, heading,
						   HDG_HOLD_DIST_NEXT, &temp_next.lat, &temp_next.lon);

	} else {
		// use the existing flight path from prev to next

		// previous waypoint: shifted HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST
		create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon,
						   HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST, &temp_prev.lat, &temp_prev.lon);

		// next waypoint: shifted -(HDG_HOLD_DIST_NEXT + HDG_HOLD_REACHED_DIST)
		create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon,
						   -(HDG_HOLD_REACHED_DIST + HDG_HOLD_DIST_NEXT), &temp_next.lat, &temp_next.lon);
	}

	waypoint_prev = temp_prev;
	waypoint_prev.alt = _current_altitude;
	waypoint_prev.valid = true;

	waypoint_next = temp_next;
	waypoint_next.alt = _current_altitude;
	waypoint_next.valid = true;
}

float
FixedwingPositionControl::getManualHeightRateSetpoint()
{
	/*
	 * The complete range is -1..+1, so this is 6%
	 * of the up or down range or 3% of the total range.
	 */
	const float deadBand = 0.06f;

	/*
	 * The correct scaling of the complete range needs
	 * to account for the missing part of the slope
	 * due to the deadband
	 */
	const float factor = 1.0f - deadBand;

	float height_rate_setpoint = 0.0f;

	/*
	 * Manual control has as convention the rotation around
	 * an axis. Positive X means to rotate positively around
	 * the X axis in NED frame, which is pitching down
	 */
	if (_manual_control_setpoint_for_height_rate > deadBand) {
		/* pitching down */
		float pitch = -(_manual_control_setpoint_for_height_rate - deadBand) / factor;
		height_rate_setpoint = pitch * _param_sinkrate_target.get();

	} else if (_manual_control_setpoint_for_height_rate < - deadBand) {
		/* pitching up */
		float pitch = -(_manual_control_setpoint_for_height_rate + deadBand) / factor;
		const float climb_rate_target = _param_climbrate_target.get();

		height_rate_setpoint = pitch * climb_rate_target;

	}

	return height_rate_setpoint;
}

void
FixedwingPositionControl::updateManualTakeoffStatus()
{
	if (!_completed_manual_takeoff) {
		const bool at_controllable_airspeed = _airspeed > _param_fw_airspd_min.get()
						      || !_airspeed_valid;
		const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
					 && _control_mode.flag_armed;
		_completed_manual_takeoff = (!_landed && at_controllable_airspeed) || is_hovering;
	}
}

void
FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid)
{
	/* only run position controller in fixed-wing mode and during transitions for VTOL */
	if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) {
		_control_mode_current = FW_POSCTRL_MODE_OTHER;
		return; // do not publish the setpoint
	}

	FW_POSCTRL_MODE commanded_position_control_mode = _control_mode_current;

	_skipping_takeoff_detection = false;

	if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
	     _control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {

		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {

			if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
				_control_mode_current = FW_POSCTRL_MODE_AUTO;

				// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
				_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;

			} else {
				_control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF;

				if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) {
					// skip takeoff detection when switching from any other mode, auto or manual,
					// while already in air.
					// TODO: find a better place for / way of doing this
					_skipping_takeoff_detection = true;
				}
			}

		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {

			if (!_vehicle_status.in_transition_mode) {
				_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING;

			} else {
				// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
				_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
			}

		} else {
			_control_mode_current = FW_POSCTRL_MODE_AUTO;
		}

	} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled
		   && pos_sp_curr_valid) {

		// reset timer the first time we switch into this mode
		if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE
		    && commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE) {
			_time_in_fixed_bank_loiter = now;
		}

		if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)
		    && !_vehicle_status.in_transition_mode) {
			if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
				// Need to init because last loop iteration was in a different mode
				mavlink_log_critical(&_mavlink_log_pub, "Start loiter with fixed bank angle.\t");
				events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical,
					     "Start loiter with fixed bank angle");
			}

			_control_mode_current = FW_POSCTRL_MODE_AUTO_ALTITUDE;

		} else {
			if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) {
				mavlink_log_critical(&_mavlink_log_pub, "Start descending.\t");
				events::send(events::ID("fixedwing_position_control_descend"), events::Log::Critical, "Start descending");
			}

			_control_mode_current = FW_POSCTRL_MODE_AUTO_CLIMBRATE;
		}


	} else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) {
		if (commanded_position_control_mode != FW_POSCTRL_MODE_MANUAL_POSITION) {
			/* Need to init because last loop iteration was in a different mode */
			_hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw
			_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
			_yaw_lock_engaged = false;

			/* reset setpoints from other modes (auto) otherwise we won't
			 * level out without new manual input */
			_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_r_lim.get());
			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
		}

		_control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION;

	} else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_altitude_enabled) {

		_control_mode_current = FW_POSCTRL_MODE_MANUAL_ALTITUDE;

	} else {
		_control_mode_current = FW_POSCTRL_MODE_OTHER;
	}
}

void
FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
{
	/* save time when airplane is in air */
	if (!_was_in_air && !_landed) {
		_was_in_air = true;
		_time_went_in_air = now;

		_tecs.resetIntegrals();
		_tecs.resetTrajectoryGenerators(_current_altitude);
	}

	/* reset flag when airplane landed */
	if (_landed) {
		_was_in_air = false;
		_completed_manual_takeoff = false;

		_tecs.resetIntegrals();
		_tecs.resetTrajectoryGenerators(_current_altitude);
	}
}

void
FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp)
{
	// TODO: velocity, altitude, or just a heading hold position mode should be used for this, not position
	// shifting hacks

	if (_vehicle_status.in_transition_to_fw) {

		if (!PX4_ISFINITE(_transition_waypoint(0))) {
			double lat_transition, lon_transition;
			// create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
			// during the transition
			waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition,
							   &lon_transition);

			_transition_waypoint(0) = lat_transition;
			_transition_waypoint(1) = lon_transition;
		}


		current_sp.lat = _transition_waypoint(0);
		current_sp.lon = _transition_waypoint(1);

	} else {
		/* reset transition waypoint, will be set upon entering front transition */
		_transition_waypoint(0) = static_cast<double>(NAN);
		_transition_waypoint(1) = static_cast<double>(NAN);
	}
}

void
FixedwingPositionControl::control_auto(const float control_interval, const Vector2d &curr_pos,
				       const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
				       const position_setpoint_s &pos_sp_next)
{
	position_setpoint_s current_sp = pos_sp_curr;
	move_position_setpoint_for_vtol_transition(current_sp);

	const uint8_t position_sp_type = handle_setpoint_type(current_sp);

	_position_sp_type = position_sp_type;

	if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER
	    || current_sp.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
		publishOrbitStatus(current_sp);
	}

	const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();

	switch (position_sp_type) {
	case position_setpoint_s::SETPOINT_TYPE_IDLE:
		_att_sp.thrust_body[0] = 0.0f;
		_att_sp.roll_body = 0.0f;
		_att_sp.pitch_body = radians(_param_fw_psp_off.get());
		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;

		break;

	case position_setpoint_s::SETPOINT_TYPE_POSITION:
		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
		break;

	case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
		control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
		break;

	case position_setpoint_s::SETPOINT_TYPE_LOITER:
		control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
		break;
	}

	if (was_circle_mode && !_l1_control.circle_mode()) {
		/* just kicked out of loiter, reset roll integrals */
		_att_sp.roll_reset_integral = true;
	}

	/* Copy thrust output for publication, handle special cases */
	if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {

		_att_sp.thrust_body[0] = 0.0f;

	} else {
		// when we are landed state we want the motor to spin at idle speed
		_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
	}

	/* Copy thrust and pitch values from tecs */
	_att_sp.pitch_body = get_tecs_pitch();

	if (!_vehicle_status.in_transition_to_fw) {
		publishLocalPositionSetpoint(current_sp);
	}
}

void
FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval)
{
	// only control altitude and airspeed ("fixed-bank loiter")

	tecs_update_pitch_throttle(control_interval,
				   _current_altitude,
				   _param_fw_airspd_trim.get(),
				   radians(_param_fw_p_lim_min.get()),
				   radians(_param_fw_p_lim_max.get()),
				   _param_fw_thr_min.get(),
				   _param_fw_thr_max.get(),
				   false,
				   _param_fw_p_lim_min.get(),
				   _param_sinkrate_target.get());

	_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
	_att_sp.yaw_body = 0.f;

	if (_landed) {
		_att_sp.thrust_body[0] = _param_fw_thr_min.get();

	} else {
		_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
	}

	_att_sp.pitch_body = get_tecs_pitch();

	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
}

void
FixedwingPositionControl::control_auto_descend(const float control_interval)
{
	// Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover,
	// but not letting it drift too far away.
	const float descend_rate = -0.5f;

	tecs_update_pitch_throttle(control_interval,
				   _current_altitude,
				   _param_fw_airspd_trim.get(),
				   radians(_param_fw_p_lim_min.get()),
				   radians(_param_fw_p_lim_max.get()),
				   _param_fw_thr_min.get(),
				   _param_fw_thr_max.get(),
				   false,
				   _param_fw_p_lim_min.get(),
				   _param_sinkrate_target.get(),
				   false,
				   descend_rate);

	_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
	_att_sp.yaw_body = 0.f;

	_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
	_att_sp.pitch_body = get_tecs_pitch();

	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
}

uint8_t
FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr)
{
	uint8_t position_sp_type = pos_sp_curr.type;

	if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
		return position_setpoint_s::SETPOINT_TYPE_VELOCITY;
	}

	Vector2d curr_wp{0, 0};

	/* current waypoint (the one currently heading for) */
	curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);

	const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);

	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
	    || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {

		float dist_xy = -1.f;
		float dist_z = -1.f;

		const float dist = get_distance_to_point_global_wgs84(
					   (double)curr_wp(0), (double)curr_wp(1), pos_sp_curr.alt,
					   _current_latitude, _current_longitude, _current_altitude,
					   &dist_xy, &dist_z);

		float loiter_radius_abs = fabsf(_param_nav_loiter_rad.get());

		if (fabsf(pos_sp_curr.loiter_radius) > FLT_EPSILON) {
			loiter_radius_abs = fabsf(pos_sp_curr.loiter_radius);
		}

		if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
			// POSITION: achieve position setpoint altitude via loiter
			// close to waypoint, but altitude error greater than twice acceptance
			if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f)
			    && (dist_z > _param_nav_fw_alt_rad.get())
			    && (dist_xy < 2.f * math::max(acc_rad, loiter_radius_abs))) {
				// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
				position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
			}

		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
			// LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER (NPFG handles this internally)
			if ((dist >= 0.f)
			    && (dist_xy > 2.f * math::max(acc_rad, loiter_radius_abs))
			    && !_param_fw_use_npfg.get()) {
				// SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION
				position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
			}
		}
	}

	return position_sp_type;
}

void
FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos,
		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
	const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
	Vector2d curr_wp{0, 0};
	Vector2d prev_wp{0, 0};

	/* current waypoint (the one currently heading for) */
	curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);

	if (pos_sp_prev.valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
		prev_wp(0) = pos_sp_prev.lat;
		prev_wp(1) = pos_sp_prev.lon;

	} else {
		// No valid previous waypoint, go for the current wp.
		// This is automatically handled by the L1/NPFG libraries.
		prev_wp(0) = pos_sp_curr.lat;
		prev_wp(1) = pos_sp_curr.lon;
	}

	float tecs_fw_thr_min;
	float tecs_fw_thr_max;

	if (pos_sp_curr.gliding_enabled) {
		/* enable gliding with this waypoint */
		_tecs.set_speed_weight(2.0f);
		tecs_fw_thr_min = 0.0;
		tecs_fw_thr_max = 0.0;

	} else {
		tecs_fw_thr_min = _param_fw_thr_min.get();
		tecs_fw_thr_max = _param_fw_thr_max.get();
	}

	// waypoint is a plain navigation waypoint
	float position_sp_alt = pos_sp_curr.alt;

	// Altitude first order hold (FOH)
	if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) &&
	    ((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) ||
	     (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER))
	   ) {
		const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
					  pos_sp_prev.lat, pos_sp_prev.lon);

		// Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one
		if (d_curr_prev > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) {
			// Calculate distance to current waypoint
			const float d_curr = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
					     _current_latitude, _current_longitude);

			// Save distance to waypoint if it is the smallest ever achieved, however make sure that
			// _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp
			_min_current_sp_distance_xy = math::min(d_curr, _min_current_sp_distance_xy, d_curr_prev);

			// if the minimal distance is smaller than the acceptance radius, we should be at waypoint alt
			// navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude
			if (_min_current_sp_distance_xy > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) {
				// The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
				// radius around the current waypoint
				const float delta_alt = (pos_sp_curr.alt - pos_sp_prev.alt);
				const float grad = -delta_alt / (d_curr_prev - math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)));
				const float a = pos_sp_prev.alt - grad * d_curr_prev;

				position_sp_alt = a + grad * _min_current_sp_distance_xy;
			}
		}
	}

	float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, ground_speed);

	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
	Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));

	if (_param_fw_use_npfg.get()) {
		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);

		if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) {
			// Navigate directly on position setpoint and path tangent
			matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
			float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
					  0.0f;
			_npfg.navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), get_nav_speed_2d(ground_speed),
						  _wind_vel, curvature);

		} else {
			_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel);
		}

		_att_sp.roll_body = _npfg.getRollSetpoint();
		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

	} else {
		_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed));
		_att_sp.roll_body = _l1_control.get_roll_setpoint();
	}

	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;

	tecs_update_pitch_throttle(control_interval,
				   position_sp_alt,
				   target_airspeed,
				   radians(_param_fw_p_lim_min.get()),
				   radians(_param_fw_p_lim_max.get()),
				   tecs_fw_thr_min,
				   tecs_fw_thr_max,
				   false,
				   radians(_param_fw_p_lim_min.get()),
				   _param_sinkrate_target.get());
}

void
FixedwingPositionControl::control_auto_velocity(const float control_interval, const Vector2d &curr_pos,
		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
{
	float tecs_fw_thr_min;
	float tecs_fw_thr_max;

	if (pos_sp_curr.gliding_enabled) {
		/* enable gliding with this waypoint */
		_tecs.set_speed_weight(2.0f);
		tecs_fw_thr_min = 0.0;
		tecs_fw_thr_max = 0.0;

	} else {
		tecs_fw_thr_min = _param_fw_thr_min.get();
		tecs_fw_thr_max = _param_fw_thr_max.get();
	}

	// waypoint is a plain navigation waypoint
	float position_sp_alt = pos_sp_curr.alt;


	//Offboard velocity control
	Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy};
	_target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0)));

	float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, ground_speed);

	if (_param_fw_use_npfg.get()) {
		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
		_npfg.navigateBearing(_target_bearing, ground_speed, _wind_vel);
		_att_sp.roll_body = _npfg.getRollSetpoint();
		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

	} else {
		_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
		_att_sp.roll_body = _l1_control.get_roll_setpoint();
	}

	_att_sp.yaw_body = _yaw;

	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;

	tecs_update_pitch_throttle(control_interval,
				   position_sp_alt,
				   target_airspeed,
				   radians(_param_fw_p_lim_min.get()),
				   radians(_param_fw_p_lim_max.get()),
				   tecs_fw_thr_min,
				   tecs_fw_thr_max,
				   false,
				   radians(_param_fw_p_lim_min.get()),
				   _param_sinkrate_target.get(),
				   tecs_status_s::TECS_MODE_NORMAL,
				   pos_sp_curr.vz);
}

void
FixedwingPositionControl::control_auto_loiter(const float control_interval, const Vector2d &curr_pos,
		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
		const position_setpoint_s &pos_sp_next)
{
	Vector2d curr_wp{0, 0};
	Vector2d prev_wp{0, 0};

	/* current waypoint (the one currently heading for) */
	curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);

	if (pos_sp_prev.valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
		prev_wp(0) = pos_sp_prev.lat;
		prev_wp(1) = pos_sp_prev.lon;

	} else {
		// No valid previous waypoint, go for the current wp.
		// This is automatically handled by the L1/NPFG libraries.
		prev_wp(0) = pos_sp_curr.lat;
		prev_wp(1) = pos_sp_curr.lon;
	}

	float airspeed_sp = -1.f;

	if (PX4_ISFINITE(pos_sp_curr.cruising_speed) &&
	    pos_sp_curr.cruising_speed > FLT_EPSILON) {

		airspeed_sp = pos_sp_curr.cruising_speed;
	}

	float tecs_fw_thr_min;
	float tecs_fw_thr_max;

	if (pos_sp_curr.gliding_enabled) {
		/* enable gliding with this waypoint */
		_tecs.set_speed_weight(2.0f);
		tecs_fw_thr_min = 0.0;
		tecs_fw_thr_max = 0.0;

	} else {
		tecs_fw_thr_min = _param_fw_thr_min.get();
		tecs_fw_thr_max = _param_fw_thr_max.get();
	}

	/* waypoint is a loiter waypoint */
	float loiter_radius = pos_sp_curr.loiter_radius;
	uint8_t loiter_direction = pos_sp_curr.loiter_direction;

	if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) {
		loiter_radius = _param_nav_loiter_rad.get();
		loiter_direction = signNoZero(loiter_radius);
	}

	const bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();

	if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid
	    && in_circle_mode && _param_fw_lnd_earlycfg.get()) {
		// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
		// landing airspeed and potentially tighter altitude control) already such that we don't
		// have to do this switch (which can cause significant altitude errors) close to the ground.
		_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
		airspeed_sp = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;

	} else {
		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
	}

	float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_sp, ground_speed);

	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));

	if (_param_fw_use_npfg.get()) {
		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
		_npfg.navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed),
				     _wind_vel);
		_att_sp.roll_body = _npfg.getRollSetpoint();
		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

	} else {
		_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction,
					    get_nav_speed_2d(ground_speed));
		_att_sp.roll_body = _l1_control.get_roll_setpoint();
	}

	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

	float alt_sp = pos_sp_curr.alt;

	if (_land_abort) {
		if (pos_sp_curr.alt - _current_altitude  < _param_fw_clmbout_diff.get()) {
			// aborted landing complete, normal loiter over landing point
			abort_landing(false);

		} else {
			// continue straight until vehicle has sufficient altitude
			_att_sp.roll_body = 0.0f;
		}

		_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
	}

	tecs_update_pitch_throttle(control_interval,
				   alt_sp,
				   target_airspeed,
				   radians(_param_fw_p_lim_min.get()),
				   radians(_param_fw_p_lim_max.get()),
				   tecs_fw_thr_min,
				   tecs_fw_thr_max,
				   false,
				   radians(_param_fw_p_lim_min.get()),
				   _param_sinkrate_target.get());
}

void
FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval,
		const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
{
	if (!_control_mode.flag_armed) {
		reset_takeoff_state();
	}

	// for now taking current position setpoint altitude as clearance altitude. this is the altitude we need to
	// clear all occlusions in the takeoff path
	const float clearance_altitude_amsl = pos_sp_curr.alt;

	// set the altitude to something above the clearance altitude to ensure the vehicle climbs past the value
	// (navigator will accept the takeoff as complete once crossing the clearance altitude)
	const float altitude_setpoint_amsl = clearance_altitude_amsl + kClearanceAltitudeBuffer;

	Vector2f local_2D_position{_local_pos.x, _local_pos.y};

	if (_runway_takeoff.runwayTakeoffEnabled()) {
		if (!_runway_takeoff.isInitialized()) {
			_runway_takeoff.init(now, _yaw, global_position);

			_takeoff_ground_alt = _current_altitude;

			mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway\t");
			events::send(events::ID("fixedwing_position_control_takeoff"), events::Log::Info, "Takeoff on runway");
		}

		if (_skipping_takeoff_detection) {
			_runway_takeoff.forceSetFlyState();
		}

		_runway_takeoff.update(now, _airspeed, _current_altitude - _takeoff_ground_alt,
				       clearance_altitude_amsl - _takeoff_ground_alt,
				       &_mavlink_log_pub);

		const float takeoff_airspeed = _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get();
		float target_airspeed = get_auto_airspeed_setpoint(control_interval, takeoff_airspeed, ground_speed);

		// yaw control is disabled once in "taking off" state
		_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();

		// XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface.
		if (_param_rwto_nudge.get()) {
			_att_sp.yaw_sp_move_rate = _manual_control_setpoint.r;
		}

		// tune up the lateral position control guidance when on the ground
		if (_att_sp.fw_control_yaw) {
			_npfg.setPeriod(_param_rwto_l1_period.get());
			_l1_control.set_l1_period(_param_rwto_l1_period.get());

		}

		const Vector2f start_pos_local = _global_local_proj_ref.project(_runway_takeoff.getStartPosition()(0),
						 _runway_takeoff.getStartPosition()(1));
		const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);

		// the bearing from runway start to the takeoff waypoint is followed until the clearance altitude is exceeded
		const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local);

		if (_param_fw_use_npfg.get()) {
			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
			_npfg.navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed,
						  _wind_vel, 0.0f);

			_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());

			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

			// use npfg's bearing to commanded course, controlled via yaw angle while on runway
			const float bearing = _npfg.getBearing();

			// heading hold mode will override this bearing setpoint
			_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);

		} else {
			// make a fake waypoint in the direction of the takeoff bearing
			const Vector2f virtual_waypoint = start_pos_local + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;

			_l1_control.navigate_waypoints(start_pos_local, virtual_waypoint, local_2D_position, ground_speed);

			const float l1_roll_setpoint = _l1_control.get_roll_setpoint();
			_att_sp.roll_body = _runway_takeoff.getRoll(l1_roll_setpoint);

			// use l1's nav bearing to command course, controlled via yaw angle while on runway
			const float bearing = _l1_control.nav_bearing();

			// heading hold mode will override this bearing setpoint
			_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
		}

		// update tecs
		const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
		const float takeoff_pitch_min_climbout_deg = _runway_takeoff.getMinPitch(_takeoff_pitch_min.get(),
				_param_fw_p_lim_min.get());

		if (_runway_takeoff.resetIntegrators()) {
			// reset integrals except yaw (which also counts for the wheel controller)
			_att_sp.roll_reset_integral = true;
			_att_sp.pitch_reset_integral = true;

			// throttle is open loop anyway during ground roll, no need to wind up the integrator
			_tecs.resetIntegrals();
		}

		if (takeoff_airspeed < _param_fw_airspd_min.get()) {
			// adjust underspeed detection bounds for takeoff airspeed
			_tecs.set_equivalent_airspeed_min(takeoff_airspeed);
		}

		tecs_update_pitch_throttle(control_interval,
					   altitude_setpoint_amsl,
					   target_airspeed,
					   radians(_param_fw_p_lim_min.get()),
					   radians(takeoff_pitch_max_deg),
					   _param_fw_thr_min.get(),
					   _param_fw_thr_max.get(),
					   _runway_takeoff.climbout(),
					   radians(takeoff_pitch_min_climbout_deg),
					   _param_sinkrate_target.get());

		_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation

		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());

		// apply flaps for takeoff according to the corresponding scale factor set via FW_FLAPS_TO_SCL
		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF;
		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;

	} else {
		/* Perform launch detection */
		if (!_skipping_takeoff_detection && _launchDetector.launchDetectionEnabled() &&
		    _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {

			if (_control_mode.flag_armed) {
				/* Perform launch detection */

				/* Inform user that launchdetection is running every 4s */
				if ((now - _last_time_launch_detection_notified) > 4_s) {
					mavlink_log_critical(&_mavlink_log_pub, "Launch detection running\t");
					events::send(events::ID("fixedwing_position_control_launch_detection"), events::Log::Info, "Launch detection running");
					_last_time_launch_detection_notified = now;
				}

				/* Detect launch using body X (forward) acceleration */
				_launchDetector.update(control_interval, _body_acceleration(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;
		}

		if (!_launch_detected && _launch_detection_state != LAUNCHDETECTION_RES_NONE) {
			_launch_detected = true;
			_launch_global_position = global_position;
			_takeoff_ground_alt = _current_altitude;
		}

		const Vector2f launch_local_position = _global_local_proj_ref.project(_launch_global_position(0),
						       _launch_global_position(1));
		const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);

		// the bearing from launch to the takeoff waypoint is followed until the clearance altitude is exceeded
		const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(launch_local_position, takeoff_waypoint_local);

		/* 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. */

			float target_airspeed = get_auto_airspeed_setpoint(control_interval, _param_fw_airspd_trim.get(), ground_speed);

			if (_param_fw_use_npfg.get()) {
				_npfg.setAirspeedNom(target_airspeed * _eas2tas);
				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
				_npfg.navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel,
							  0.0f);
				_att_sp.roll_body = _npfg.getRollSetpoint();
				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

			} else {
				// make a fake waypoint in the direction of the takeoff bearing
				const Vector2f virtual_waypoint = launch_local_position + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;
				_l1_control.navigate_waypoints(launch_local_position, virtual_waypoint, local_2D_position, ground_speed);
				_att_sp.roll_body = _l1_control.get_roll_setpoint();
			}

			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

			/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
			 * full throttle, otherwise we use idle throttle */
			float takeoff_throttle = _param_fw_thr_max.get();

			if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
				takeoff_throttle = _param_fw_thr_idle.get();
			}

			if (_current_altitude < clearance_altitude_amsl) {
				// select maximum pitch: the launchdetector may impose another limit for the pitch
				// depending on the state of the launch
				const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_param_fw_p_lim_max.get());

				tecs_update_pitch_throttle(control_interval,
							   altitude_setpoint_amsl,
							   target_airspeed,
							   radians(_param_fw_p_lim_min.get()),
							   radians(takeoff_pitch_max_deg),
							   _param_fw_thr_min.get(),
							   takeoff_throttle,
							   true,
							   radians(_takeoff_pitch_min.get()),
							   _param_sinkrate_target.get());

			} else {
				tecs_update_pitch_throttle(control_interval,
							   altitude_setpoint_amsl,
							   target_airspeed,
							   radians(_param_fw_p_lim_min.get()),
							   radians(_param_fw_p_lim_max.get()),
							   _param_fw_thr_min.get(),
							   takeoff_throttle,
							   false,
							   radians(_param_fw_p_lim_min.get()),
							   _param_sinkrate_target.get());
			}

			if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
				// explicitly set idle throttle until motors are enabled
				_att_sp.thrust_body[0] = _param_fw_thr_idle.get();

			} else {
				_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
			}

			_att_sp.pitch_body = get_tecs_pitch();

		} 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.thrust_body[0] = _param_fw_thr_idle.get();
			_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
		}

		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
	}

	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, _takeoff_ground_alt);

	if (!_vehicle_status.in_transition_to_fw) {
		publishLocalPositionSetpoint(pos_sp_curr);
	}
}

void
FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const float control_interval,
		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
	// Enable tighter altitude control for landings
	_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());

	const Vector2f local_position{_local_pos.x, _local_pos.y};
	Vector2f local_land_point = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);

	initializeAutoLanding(now, pos_sp_prev, pos_sp_curr, local_position, local_land_point);

	// touchdown may get nudged by manual inputs
	local_land_point = calculateTouchdownPosition(control_interval, local_land_point);
	const Vector2f landing_approach_vector = calculateLandingApproachVector();

	const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();

	if (airspeed_land < _param_fw_airspd_min.get()) {
		// adjust underspeed detection bounds for landing airspeed
		_tecs.set_equivalent_airspeed_min(airspeed_land);
	}

	float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_land, ground_speed);

	// calculate the altitude setpoint based on the landing glide slope
	const float along_track_dist_to_touchdown = -landing_approach_vector.unit_or_zero().dot(
				local_position - local_land_point);
	const float glide_slope = _landing_approach_entrance_rel_alt / _landing_approach_entrance_offset_vector.norm();
	const float glide_slope_rel_alt = math::constrain(along_track_dist_to_touchdown * glide_slope, 0.0f,
					  _landing_approach_entrance_rel_alt);

	const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt);
	float altitude_setpoint;

	if (_current_altitude > terrain_alt + glide_slope_rel_alt) {
		// descend to the glide slope
		altitude_setpoint = terrain_alt + glide_slope_rel_alt;

	} else {
		// continue horizontally
		altitude_setpoint = _current_altitude;
	}

	// flare at the maximum of the altitude determined by the time before touchdown and a minimum flare altitude
	const float flare_rel_alt = math::max(_param_fw_lnd_fl_time.get() * _local_pos.vz, _param_fw_lnd_flalt.get());

	if ((_current_altitude < terrain_alt + flare_rel_alt) || _flaring) {
		// flare and land with minimal speed

		// flaring is a "point of no return"
		if (!_flaring) {
			_flaring = true;
			_heightrate_setpoint_at_flare_start = _tecs.hgt_rate_setpoint();
			mavlink_log_info(&_mavlink_log_pub, "Landing, flaring\t");
			events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info, "Landing, flaring");
		}

		/* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */

		// tune up the lateral position control guidance when on the ground
		_npfg.setPeriod(_param_rwto_l1_period.get());
		_l1_control.set_l1_period(_param_rwto_l1_period.get());

		// align heading with the approach bearing
		const float landing_approach_bearing = atan2f(landing_approach_vector(1), landing_approach_vector(0));

		if (_param_fw_use_npfg.get()) {
			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
			_npfg.navigateHeading(landing_approach_bearing, ground_speed, _wind_vel);
			target_airspeed = _npfg.getAirspeedRef() /  _eas2tas;
			_att_sp.roll_body = _npfg.getRollSetpoint();

		} else {
			_l1_control.navigate_heading(landing_approach_bearing, _yaw, ground_speed);
			_att_sp.roll_body = _l1_control.get_roll_setpoint();
		}

		/* longitudinal guidance */

		// ramp in flare limits and setpoints with the flare time, command a soft touchdown
		const float seconds_since_flare_start = hrt_elapsed_time(&_time_started_flaring) * 1e-6;
		const float flare_ramp_in_scaler = math::constrain(seconds_since_flare_start / _param_fw_lnd_fl_time.get(), 0.0f, 1.0f);

		const float height_rate_setpoint = flare_ramp_in_scaler * (-_param_fw_lnd_fl_sink.get()) +
						   (1.0f - flare_ramp_in_scaler) * _heightrate_setpoint_at_flare_start;

		const float pitch_min_rad = flare_ramp_in_scaler * radians(_param_fw_lnd_fl_pmin.get()) +
					    (1.0f - flare_ramp_in_scaler) * radians(_param_fw_p_lim_min.get());
		const float pitch_max_rad = flare_ramp_in_scaler * radians(_param_fw_lnd_fl_pmax.get()) +
					    (1.0f - flare_ramp_in_scaler) * radians(_param_fw_p_lim_max.get());

		tecs_update_pitch_throttle(control_interval,
					   altitude_setpoint,
					   target_airspeed,
					   pitch_min_rad,
					   pitch_max_rad,
					   0.0f,
					   0.0f,
					   false,
					   pitch_min_rad,
					   _param_sinkrate_target.get(),
					   true,
					   height_rate_setpoint);

		/* set the attitude and throttle commands */

		// TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle
		_att_sp.pitch_body = get_tecs_pitch();

		// flaring is just before touchdown, align the yaw as much as possible with the landing vector
		_att_sp.yaw_body = landing_approach_bearing;

		// enable direct yaw control using rudder/wheel
		_att_sp.fw_control_yaw = true;

		// idle throttle may be >0 for internal combustion engines
		// normally set to zero for electric motors
		_att_sp.thrust_body[0] = _param_fw_thr_idle.get();

	} else {

		// follow the glide slope

		/* lateral guidance */

		const Vector2f local_approach_entrance = local_land_point - landing_approach_vector;

		if (_param_fw_use_npfg.get()) {
			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
			_npfg.navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
			_att_sp.roll_body = _npfg.getRollSetpoint();

		} else {
			// make a fake waypoint beyond the land point in the direction of the landing approach bearing
			// (always HDG_HOLD_DIST_NEXT meters in front of the aircraft's progress along the landing approach vector)

			const float along_track_distance_from_entrance = local_approach_entrance.unit_or_zero().dot(
						local_position - local_approach_entrance);

			const Vector2f virtual_waypoint = local_approach_entrance + (along_track_distance_from_entrance + HDG_HOLD_DIST_NEXT) *
							  local_approach_entrance.unit_or_zero();

			_l1_control.navigate_waypoints(local_approach_entrance, virtual_waypoint, local_position, ground_speed);
			_att_sp.roll_body = _l1_control.get_roll_setpoint();
		}

		/* longitudinal guidance */

		// open the desired max sink rate to encompass the glide slope if within the aircraft's performance limits
		const float glide_slope_sink_rate = airspeed_land * sinf(math::radians(_param_fw_lnd_ang.get()));
		const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()),
						   _param_fw_t_sink_max.get());

		tecs_update_pitch_throttle(control_interval,
					   altitude_setpoint,
					   target_airspeed,
					   radians(_param_fw_p_lim_min.get()),
					   radians(_param_fw_p_lim_max.get()),
					   _param_fw_thr_min.get(),
					   _param_fw_thr_max.get(),
					   false,
					   radians(_param_fw_p_lim_min.get()),
					   desired_max_sinkrate);

		/* set the attitude and throttle commands */

		// TECS has authority (though constrained) over pitch during flare, throttle is killed
		_att_sp.pitch_body = get_tecs_pitch();

		// yaw is not controlled in nominal flight
		_att_sp.yaw_body = _yaw;

		// enable direct yaw control using rudder/wheel
		_att_sp.fw_control_yaw = false;

		_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : get_tecs_thrust();
	}

	_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation

	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);

	// Apply flaps and spoilers for landing. Amount of deflection is handled in the FW attitdue controller
	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;

	if (!_vehicle_status.in_transition_to_fw) {
		publishLocalPositionSetpoint(pos_sp_curr);
	}

	landing_status_publish();
}

void
FixedwingPositionControl::control_manual_altitude(const float control_interval, const Vector2d &curr_pos,
		const Vector2f &ground_speed)
{
	updateManualTakeoffStatus();

	const float calibrated_airspeed_sp = get_manual_airspeed_setpoint();
	const float height_rate_sp = getManualHeightRateSetpoint();

	// TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are
	// just passed launch
	const float min_pitch = (_completed_manual_takeoff) ? radians(_param_fw_p_lim_min.get()) :
				MIN_PITCH_DURING_MANUAL_TAKEOFF;

	float throttle_max = _param_fw_thr_max.get();

	// enable the operator to kill the throttle on ground
	if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
		throttle_max = 0.0f;
	}

	tecs_update_pitch_throttle(control_interval,
				   _current_altitude,
				   calibrated_airspeed_sp,
				   min_pitch,
				   radians(_param_fw_p_lim_max.get()),
				   _param_fw_thr_min.get(),
				   throttle_max,
				   false,
				   min_pitch,
				   _param_sinkrate_target.get(),
				   false,
				   height_rate_sp);

	_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_r_lim.get());
	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

	_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
	_att_sp.pitch_body = get_tecs_pitch();

	// In Manual modes flaps and spoilers are directly controlled in FW Attitude controller and not passed
	// through attitdue setpoints
	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
}

void
FixedwingPositionControl::control_manual_position(const float control_interval, const Vector2d &curr_pos,
		const Vector2f &ground_speed)
{
	updateManualTakeoffStatus();

	float calibrated_airspeed_sp = get_manual_airspeed_setpoint();
	const float height_rate_sp = getManualHeightRateSetpoint();

	// TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are
	// just passed launch
	const float min_pitch = (_completed_manual_takeoff) ? radians(_param_fw_p_lim_min.get()) :
				MIN_PITCH_DURING_MANUAL_TAKEOFF;

	float throttle_max = _param_fw_thr_max.get();

	// enable the operator to kill the throttle on ground
	if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
		throttle_max = 0.0f;
	}

	/* heading control */
	if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
	    fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) {

		/* heading / roll is zero, lock onto current heading */
		if (fabsf(_yawrate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
			// little yaw movement, lock to current heading
			_yaw_lock_engaged = true;
		}

		/* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration
			to make sure the plane does not start rolling
		*/
		if (!_completed_manual_takeoff) {
			_hdg_hold_enabled = false;
			_yaw_lock_engaged = true;
		}

		if (_yaw_lock_engaged) {

			/* just switched back from non heading-hold to heading hold */
			if (!_hdg_hold_enabled) {
				_hdg_hold_enabled = true;
				_hdg_hold_yaw = _yaw;

				get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
			}

			/* we have a valid heading hold position, are we too close? */
			const float dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _hdg_hold_curr_wp.lat,
					   _hdg_hold_curr_wp.lon);

			if (dist < HDG_HOLD_REACHED_DIST) {
				get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false);
			}

			Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon};
			Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon};

			Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
			Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
			Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
						 prev_wp(1));

			if (_param_fw_use_npfg.get()) {
				_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
				_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
				_att_sp.roll_body = _npfg.getRollSetpoint();
				calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;

			} else {
				/* populate l1 control setpoint */
				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
				_att_sp.roll_body = _l1_control.get_roll_setpoint();
			}

			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
		}
	}

	tecs_update_pitch_throttle(control_interval,
				   _current_altitude,
				   calibrated_airspeed_sp,
				   min_pitch,
				   radians(_param_fw_p_lim_max.get()),
				   _param_fw_thr_min.get(),
				   throttle_max,
				   false,
				   min_pitch,
				   _param_sinkrate_target.get(),
				   false,
				   height_rate_sp);

	if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
	    fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) {

		_hdg_hold_enabled = false;
		_yaw_lock_engaged = false;

		// do slew rate limiting on roll if enabled
		float roll_sp_new = _manual_control_setpoint.y * radians(_param_fw_r_lim.get());
		const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get());

		if (control_interval > 0.f && roll_rate_slew_rad > 0.f) {
			roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * control_interval,
						_att_sp.roll_body + roll_rate_slew_rad * control_interval);
		}

		_att_sp.roll_body = roll_sp_new;
		_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
	}

	_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
	_att_sp.pitch_body = get_tecs_pitch();

	// In Manual modes flaps and spoilers are directly controlled in FW Attitude controller and not passed
	// through attitdue setpoints
	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
}

float
FixedwingPositionControl::get_tecs_pitch()
{
	if (_tecs_is_running) {
		return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get());
	}

	// return level flight pitch offset to prevent stale tecs state when it's not running
	return radians(_param_fw_psp_off.get());
}

float
FixedwingPositionControl::get_tecs_thrust()
{
	if (_tecs_is_running) {
		return min(_tecs.get_throttle_setpoint(), 1.f);
	}

	// return 0 to prevent stale tecs state when it's not running
	return 0.0f;
}

void
FixedwingPositionControl::Run()
{
	if (should_exit()) {
		_local_pos_sub.unregisterCallback();
		exit_and_cleanup();
		return;
	}

	perf_begin(_loop_perf);

	/* only run controller if position changed */

	if (_local_pos_sub.update(&_local_pos)) {

		const float control_interval = math::constrain((_local_pos.timestamp - _last_time_position_control_called) * 1e-6f,
					       MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
		_last_time_position_control_called = _local_pos.timestamp;

		// check for parameter updates
		if (_parameter_update_sub.updated()) {
			// clear update
			parameter_update_s pupdate;
			_parameter_update_sub.copy(&pupdate);

			// update parameters from storage
			parameters_update();
		}

		vehicle_global_position_s gpos;

		if (_global_pos_sub.update(&gpos)) {
			_current_latitude = gpos.lat;
			_current_longitude = gpos.lon;
		}

		_current_altitude = -_local_pos.z + _local_pos.ref_alt; // Altitude AMSL in meters

		// handle estimator reset events. we only adjust setpoins for manual modes
		if (_control_mode.flag_control_manual_enabled) {
			if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _alt_reset_counter) {
				// make TECS accept step in altitude and demanded altitude
				_tecs.handle_alt_step(-_local_pos.delta_z, _current_altitude);
			}

			// adjust navigation waypoints in position control mode
			if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
			    && _local_pos.vxy_reset_counter != _pos_reset_counter) {

				// reset heading hold flag, which will re-initialise position control
				_hdg_hold_enabled = false;
			}
		}

		// update the reset counters in any case
		_alt_reset_counter = _local_pos.vz_reset_counter;
		_pos_reset_counter = _local_pos.vxy_reset_counter;

		// Convert Local setpoints to global setpoints
		if (!_global_local_proj_ref.isInitialized()
		    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)
		    || (_local_pos.vxy_reset_counter != _pos_reset_counter)) {

			_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
							     _local_pos.ref_timestamp);
			_global_local_alt0 = _local_pos.ref_alt;
		}

		if (_control_mode.flag_control_offboard_enabled) {
			vehicle_local_position_setpoint_s trajectory_setpoint;

			if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
				bool valid_setpoint = false;
				_pos_sp_triplet = {}; // clear any existing
				_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
				_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
				_pos_sp_triplet.current.cruising_speed = NAN; // ignored
				_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
				_pos_sp_triplet.current.vx = NAN;
				_pos_sp_triplet.current.vy = NAN;
				_pos_sp_triplet.current.vz = NAN;
				_pos_sp_triplet.current.lat = static_cast<double>(NAN);
				_pos_sp_triplet.current.lon = static_cast<double>(NAN);
				_pos_sp_triplet.current.alt = NAN;

				if (PX4_ISFINITE(trajectory_setpoint.x) && PX4_ISFINITE(trajectory_setpoint.y) && PX4_ISFINITE(trajectory_setpoint.z)) {
					if (_global_local_proj_ref.isInitialized()) {
						double lat;
						double lon;
						_global_local_proj_ref.reproject(trajectory_setpoint.x, trajectory_setpoint.y, lat, lon);
						valid_setpoint = true;
						_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
						_pos_sp_triplet.current.lat = lat;
						_pos_sp_triplet.current.lon = lon;
						_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z;
					}

				}

				if (PX4_ISFINITE(trajectory_setpoint.vx) && PX4_ISFINITE(trajectory_setpoint.vx)
				    && PX4_ISFINITE(trajectory_setpoint.vz)) {
					valid_setpoint = true;
					_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
					_pos_sp_triplet.current.vx = trajectory_setpoint.vx;
					_pos_sp_triplet.current.vy = trajectory_setpoint.vy;
					_pos_sp_triplet.current.vz = trajectory_setpoint.vz;

					if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1])
					    && PX4_ISFINITE(trajectory_setpoint.acceleration[2])) {
						Vector2f velocity_sp_2d(trajectory_setpoint.vx, trajectory_setpoint.vy);
						Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]);
						Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(velocity_sp_2d) *
									       velocity_sp_2d.normalized();
						_pos_sp_triplet.current.loiter_radius = velocity_sp_2d.norm() * velocity_sp_2d.norm() / acceleration_normal.norm();

					} else {
						_pos_sp_triplet.current.loiter_radius = NAN;
					}
				}

				if (!valid_setpoint) {
					mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint\t");
					events::send(events::ID("fixedwing_position_control_invalid_offboard_sp"), events::Log::Error,
						     "Invalid offboard setpoint");

				} else {
					_pos_sp_triplet.current.valid = valid_setpoint;
				}
			}

		} else {
			if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
				// reset the altitude foh (first order hold) logic
				_min_current_sp_distance_xy = FLT_MAX;
			}
		}

		airspeed_poll();
		manual_control_setpoint_poll();
		vehicle_attitude_poll();
		vehicle_command_poll();
		vehicle_control_mode_poll();
		wind_poll();

		vehicle_air_data_s air_data;

		if (_vehicle_air_data_sub.update(&air_data)) {
			_air_density = PX4_ISFINITE(air_data.rho) ? air_data.rho : _air_density;
		}

		if (_vehicle_land_detected_sub.updated()) {
			vehicle_land_detected_s vehicle_land_detected;

			if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) {
				_landed = vehicle_land_detected.landed;
			}
		}

		_vehicle_status_sub.update(&_vehicle_status);

		Vector2d curr_pos(_current_latitude, _current_longitude);
		Vector2f ground_speed(_local_pos.vx, _local_pos.vy);

		set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);

		update_in_air_states(_local_pos.timestamp);

		// update lateral guidance timesteps for slewrates
		if (_param_fw_use_npfg.get()) {
			_npfg.setDt(control_interval);

		} else {
			_l1_control.set_dt(control_interval);
		}

		// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
		_tecs.set_speed_weight(_param_fw_t_spdweight.get());
		_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());

		// restore lateral-directional guidance parameters (changed in takeoff mode)
		_npfg.setPeriod(_param_npfg_period.get());
		_l1_control.set_l1_period(_param_fw_l1_period.get());

		_att_sp.roll_reset_integral = false;
		_att_sp.pitch_reset_integral = false;
		_att_sp.yaw_reset_integral = false;

		// by default we don't want yaw to be contoller directly with rudder
		_att_sp.fw_control_yaw = false;

		// default to zero - is used (IN A HACKY WAY) to pass direct nose wheel steering via yaw stick to the actuators during auto takeoff
		_att_sp.yaw_sp_move_rate = 0.0f;

		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING) {
			reset_landing_state();
		}

		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF) {
			reset_takeoff_state();
		}

		switch (_control_mode_current) {
		case FW_POSCTRL_MODE_AUTO: {
				control_auto(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
					     _pos_sp_triplet.next);
				break;
			}

		case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
				control_auto_fixed_bank_alt_hold(control_interval);
				break;
			}

		case FW_POSCTRL_MODE_AUTO_CLIMBRATE: {
				control_auto_descend(control_interval);
				break;
			}

		case FW_POSCTRL_MODE_AUTO_LANDING: {
				control_auto_landing(_local_pos.timestamp, control_interval, ground_speed, _pos_sp_triplet.previous,
						     _pos_sp_triplet.current);
				break;
			}

		case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
				control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
				break;
			}

		case FW_POSCTRL_MODE_MANUAL_POSITION: {
				control_manual_position(control_interval, curr_pos, ground_speed);
				break;
			}

		case FW_POSCTRL_MODE_MANUAL_ALTITUDE: {
				control_manual_altitude(control_interval, curr_pos, ground_speed);
				break;
			}

		case FW_POSCTRL_MODE_OTHER: {
				_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());

				_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
				_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;

				_tecs.reset_state();

				break;
			}

		}

		if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {

			if (_control_mode.flag_control_manual_enabled) {
				_att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_r_lim.get()),
							      radians(_param_fw_r_lim.get()));
				_att_sp.pitch_body = constrain(_att_sp.pitch_body, radians(_param_fw_p_lim_min.get()),
							       radians(_param_fw_p_lim_max.get()));
			}

			if (_control_mode.flag_control_position_enabled ||
			    _control_mode.flag_control_velocity_enabled ||
			    _control_mode.flag_control_acceleration_enabled ||
			    _control_mode.flag_control_altitude_enabled ||
			    _control_mode.flag_control_climb_rate_enabled) {

				const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
				q.copyTo(_att_sp.q_d);

				_att_sp.timestamp = hrt_absolute_time();
				_attitude_sp_pub.publish(_att_sp);

				// only publish status in full FW mode
				if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
				    || _vehicle_status.in_transition_mode) {
					status_publish();

				}
			}

			_l1_control.reset_has_guidance_updated();
		}

		perf_end(_loop_perf);
	}
}

void
FixedwingPositionControl::reset_takeoff_state()
{
	_runway_takeoff.reset();

	_launchDetector.reset();
	_launch_detection_state = LAUNCHDETECTION_RES_NONE;
	_last_time_launch_detection_notified = 0;

	_launch_detected = false;

	_takeoff_ground_alt = _current_altitude;
}

void
FixedwingPositionControl::reset_landing_state()
{
	_time_started_landing = 0;

	_flaring = false;
	_time_started_flaring = 0;
	_heightrate_setpoint_at_flare_start = 0.0f;

	_lateral_touchdown_position_offset = 0.0f;

	_last_time_terrain_alt_was_valid = 0;

	// reset abort land, unless loitering after an abort
	if (_land_abort && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) {

		abort_landing(false);
	}
}

bool FixedwingPositionControl::using_npfg_with_wind_estimate() const
{
	// high wind mitigation is handled by NPFG if the wind estimate is valid
	return _param_fw_use_npfg.get() && _wind_valid;
}

Vector2f
FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed)
{

	Vector2f nav_speed_2d{ground_speed};

	if (_airspeed_valid && !using_npfg_with_wind_estimate()) {
		// l1 navigation logic breaks down when wind speed exceeds max airspeed
		// compute 2D groundspeed from airspeed-heading projection
		const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};

		// angle between air_speed_2d and ground_speed
		const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));

		// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
		if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
			nav_speed_2d = air_speed_2d;
		}
	}

	return nav_speed_2d;
}

float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min,
		float throttle_max)
{
	float weight_ratio = 1.0f;

	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
		weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO,
					       MAX_WEIGHT_RATIO);
	}

	float air_density_throttle_scale = 1.0f;

	if (PX4_ISFINITE(_air_density)) {
		// scale throttle as a function of sqrt(rho0/rho)
		const float eas2tas = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / _air_density);
		const float eas2tas_at_5000m_amsl = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / AIR_DENSITY_STANDARD_ATMOS_5000_AMSL);
		air_density_throttle_scale = constrain(eas2tas, 1.f, eas2tas_at_5000m_amsl);
	}

	// compensate trim throttle for both weight and air density
	return math::constrain(throttle_trim * sqrtf(weight_ratio) * air_density_throttle_scale, throttle_min, throttle_max);
}

void
FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
		float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, bool climbout_mode,
		float climbout_pitch_min_rad, const float desired_max_sinkrate, bool disable_underspeed_detection, float hgt_rate_sp)
{
	_tecs_is_running = true;

	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
	// (it should also not run during VTOL blending because airspeed is too low still)
	if (_vehicle_status.is_vtol) {
		if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) {
			_tecs_is_running = false;
		}

		if (_vehicle_status.in_transition_mode) {
			// we're in transition
			_was_in_transition = true;

			// set this to transition airspeed to init tecs correctly
			if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) {
				// some vtols fly without airspeed sensor
				_airspeed_after_transition = _param_airspeed_trans;

			} else {
				_airspeed_after_transition = _airspeed;
			}

			_airspeed_after_transition = constrain(_airspeed_after_transition, _param_fw_airspd_min.get(),
							       _param_fw_airspd_max.get());

		} else if (_was_in_transition) {
			// after transition we ramp up desired airspeed from the speed we had coming out of the transition
			_airspeed_after_transition += control_interval * 2.0f; // increase 2m/s

			if (_airspeed_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
				airspeed_sp = max(_airspeed_after_transition, _airspeed);

			} else {
				_was_in_transition = false;
				_airspeed_after_transition = 0.0f;
			}
		}
	}

	if (!_tecs_is_running) {
		// next time we run TECS we should reinitialize states
		_reinitialize_tecs = true;
		return;
	}

	if (_reinitialize_tecs) {
		_tecs.reset_state();
		_reinitialize_tecs = false;
	}

	/* No underspeed protection in landing mode */
	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);

	if (_landed) {
		_tecs.resetIntegrals();
		_tecs.resetTrajectoryGenerators(_current_altitude);
	}

	/* update TECS vehicle state estimates */
	_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), _current_altitude,
					     _local_pos.vz);

	const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min,
					 throttle_max);

	_tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()),
				    _current_altitude,
				    alt_sp,
				    airspeed_sp,
				    _airspeed,
				    _eas2tas,
				    climbout_mode,
				    climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
				    throttle_min,
				    throttle_max,
				    throttle_trim_comp,
				    pitch_min_rad - radians(_param_fw_psp_off.get()),
				    pitch_max_rad - radians(_param_fw_psp_off.get()),
				    _param_climbrate_target.get(),
				    desired_max_sinkrate,
				    hgt_rate_sp);

	tecs_status_publish();
}

float
FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, const float altitude,
		const float terrain_altitude) const
{
	// we want the wings level when at the wing height above ground
	const float height_above_ground = math::max(altitude - (terrain_altitude + _param_fw_wing_height.get()), 0.0f);

	// this is a conservative (linear) approximation of the roll angle that would cause wing tip strike
	// roll strike = arcsin( 2 * height / span )
	// d(roll strike)/d(height) = 2 / span / cos(2 * height / span)
	// d(roll strike)/d(height) (@height=0) = 2 / span
	// roll strike ~= 2 * height / span
	const float roll_wingtip_strike = 2.0f * height_above_ground / _param_fw_wing_span.get();

	return math::constrain(roll_setpoint, -roll_wingtip_strike, roll_wingtip_strike);
}

Vector2f
FixedwingPositionControl::calculateTakeoffBearingVector(const Vector2f &launch_position,
		const Vector2f &takeoff_waypoint) const
{
	Vector2f takeoff_bearing_vector = takeoff_waypoint - launch_position;

	if (takeoff_bearing_vector.norm_squared() > FLT_EPSILON) {
		takeoff_bearing_vector.normalize();

	} else {
		// TODO: a new bearing only based fixed-wing takeoff command / mission item will get rid of the need
		// for this check

		// takeoff in the direction of the airframe
		takeoff_bearing_vector(0) = cosf(_yaw);
		takeoff_bearing_vector(0) = sinf(_yaw);
	}

	return takeoff_bearing_vector;
}

void
FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
		const position_setpoint_s &pos_sp_curr, const Vector2f &local_position, const Vector2f &local_land_point)
{
	if (_time_started_landing == 0) {

		float height_above_land_point;
		Vector2f local_approach_entrance;

		// set the landing approach entrance location when we have just started the landing and store it
		// NOTE: the landing approach vector is relative to the land point. ekf resets may cause a local frame
		// jump, so we reference to the land point, which is globally referenced and will update
		if (pos_sp_prev.valid) {
			height_above_land_point = pos_sp_prev.alt - pos_sp_curr.alt;
			local_approach_entrance = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon);

		} else {
			// no valid previous waypoint, construct one from the glide slope and direction from current
			// position to land point

			// NOTE: this is not really a supported use case at the moment, this is just bandaiding any
			// ill-advised usage of the current implementation

			// TODO: proper handling of on-the-fly landing points would need to involve some more sophisticated
			// landing pattern generation and corresponding logic

			height_above_land_point = _current_altitude - pos_sp_curr.alt;
			local_approach_entrance = local_position;
		}

		_landing_approach_entrance_rel_alt = math::max(height_above_land_point, FLT_EPSILON);

		const Vector2f landing_approach_vector = local_land_point - local_approach_entrance;
		float landing_approach_distance = landing_approach_vector.norm();

		const float max_glide_slope = tanf(math::radians(_param_fw_lnd_ang.get()));
		const float glide_slope = _landing_approach_entrance_rel_alt / landing_approach_distance;

		if (glide_slope > max_glide_slope) {
			// rescale the landing distance - this will have the same effect as dropping down the approach
			// entrance altitude on the vehicle's behavior. if we reach here.. it means the navigator checks
			// didn't work, or something is using the control_auto_landing() method inappropriately
			landing_approach_distance = _landing_approach_entrance_rel_alt / max_glide_slope;
		}

		if (landing_approach_vector.norm_squared() > FLT_EPSILON) {
			_landing_approach_entrance_offset_vector = -landing_approach_vector.unit_or_zero() * landing_approach_distance;

		} else {
			// land in direction of airframe
			_landing_approach_entrance_offset_vector = Vector2f({cosf(_yaw), sinf(_yaw)}) * landing_approach_distance;
		}

		// save time at which we started landing and reset abort_landing
		reset_landing_state();
		_time_started_landing = now;
	}
}

Vector2f
FixedwingPositionControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position)
{
	if (fabsf(_manual_control_setpoint.r) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE && _param_fw_lnd_nudge.get() > 0) {
		// laterally nudge touchdown location with yaw stick
		// positive is defined in the direction of a right hand turn starting from the approach vector direction
		const float signed_deadzone_threshold = MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE * math::signNoZero(
				_manual_control_setpoint.r);
		_lateral_touchdown_position_offset += (_manual_control_setpoint.r - signed_deadzone_threshold) *
						      MAX_TOUCHDOWN_POSITION_NUDGE_RATE * control_interval;
		_lateral_touchdown_position_offset =  math::constrain(_lateral_touchdown_position_offset, -_param_fw_lnd_td_off.get(),
						      _param_fw_lnd_td_off.get());
	}

	const Vector2f approach_unit_vector = -_landing_approach_entrance_offset_vector.unit_or_zero();
	const Vector2f approach_unit_normal_vector{-approach_unit_vector(1), approach_unit_vector(0)};

	return local_land_position + approach_unit_normal_vector * _lateral_touchdown_position_offset;
}

Vector2f
FixedwingPositionControl::calculateLandingApproachVector() const
{
	Vector2f landing_approach_vector = -_landing_approach_entrance_offset_vector;
	const Vector2f approach_unit_vector = landing_approach_vector.unit_or_zero();
	const Vector2f approach_unit_normal_vector{-approach_unit_vector(1), approach_unit_vector(0)};

	// if _param_fw_lnd_nudge.get() == 0, no nudging

	if (_param_fw_lnd_nudge.get() == 1) {
		// nudge the approach angle -- i.e. we adjust the approach vector to reach from the original approach
		// entrance position to the newly nudged touchdown point
		// NOTE: this lengthens the landing distance.. which will adjust the glideslope height slightly
		landing_approach_vector += approach_unit_normal_vector * _lateral_touchdown_position_offset;
	}

	// if _param_fw_lnd_nudge.get() == 2, the full path (including approach entrance point) is nudged with the touchdown
	// point, which does not require any additions to the approach vector

	return landing_approach_vector;
}

float
FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude)
{
	float terrain_alt = land_point_altitude;

	if (_param_fw_lnd_useter.get() == 1) {
		if (_local_pos.dist_bottom_valid) {
			// all good, have valid terrain altitude
			float terrain_vpos = _local_pos.dist_bottom + _local_pos.z;
			terrain_alt = (_local_pos.ref_alt - terrain_vpos);
			_last_valid_terrain_alt_estimate = terrain_alt;
			_last_time_terrain_alt_was_valid = now;

		} else if (_last_time_terrain_alt_was_valid == 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 ((now - _time_started_landing) < TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT) {
				terrain_alt = land_point_altitude;

			} else {
				// still no valid terrain, abort landing
				terrain_alt = land_point_altitude;
				abort_landing(true);
			}

		} else if ((!_local_pos.dist_bottom_valid && (now - _last_time_terrain_alt_was_valid) < TERRAIN_ALT_TIMEOUT)
			   || _flaring) {
			// 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 = _last_valid_terrain_alt_estimate;

		} else {
			// terrain alt was not valid for long time, abort landing
			terrain_alt = _last_valid_terrain_alt_estimate;
			abort_landing(true);
		}
	}

	return terrain_alt;
}

void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint)
{
	if (_global_local_proj_ref.isInitialized()) {
		Vector2f current_setpoint = _global_local_proj_ref.project(current_waypoint.lat, current_waypoint.lon);
		vehicle_local_position_setpoint_s local_position_setpoint{};
		local_position_setpoint.timestamp = hrt_absolute_time();
		local_position_setpoint.x = current_setpoint(0);
		local_position_setpoint.y = current_setpoint(1);
		local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt;
		local_position_setpoint.yaw = NAN;
		local_position_setpoint.yawspeed = NAN;
		local_position_setpoint.vx = NAN;
		local_position_setpoint.vy = NAN;
		local_position_setpoint.vz = NAN;
		local_position_setpoint.acceleration[0] = NAN;
		local_position_setpoint.acceleration[1] = NAN;
		local_position_setpoint.acceleration[2] = NAN;
		local_position_setpoint.thrust[0] = _att_sp.thrust_body[0];
		local_position_setpoint.thrust[1] = _att_sp.thrust_body[1];
		local_position_setpoint.thrust[2] = _att_sp.thrust_body[2];
		_local_pos_sp_pub.publish(local_position_setpoint);
	}
}

void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp)
{
	orbit_status_s orbit_status{};
	orbit_status.timestamp = hrt_absolute_time();
	float loiter_radius = pos_sp.loiter_radius;
	uint8_t loiter_direction = pos_sp.loiter_direction;

	if (fabsf(loiter_radius) < FLT_EPSILON) {
		loiter_radius = _param_nav_loiter_rad.get();
		loiter_direction = signNoZero(loiter_radius);
	}

	orbit_status.radius = static_cast<float>(loiter_direction) * loiter_radius;
	orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
	orbit_status.x = static_cast<double>(pos_sp.lat);
	orbit_status.y = static_cast<double>(pos_sp.lon);
	orbit_status.z = pos_sp.alt;
	orbit_status.yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE;
	_orbit_status_pub.publish(orbit_status);
}

int FixedwingPositionControl::task_spawn(int argc, char *argv[])
{
	bool vtol = false;

	if (argc > 1) {
		if (strcmp(argv[1], "vtol") == 0) {
			vtol = true;
		}
	}

	FixedwingPositionControl *instance = new FixedwingPositionControl(vtol);

	if (instance) {
		_object.store(instance);
		_task_id = task_id_is_work_queue;

		if (instance->init()) {
			return PX4_OK;
		}

	} else {
		PX4_ERR("alloc failed");
	}

	delete instance;
	_object.store(nullptr);
	_task_id = -1;

	return PX4_ERROR;
}

int FixedwingPositionControl::custom_command(int argc, char *argv[])
{
	return print_usage("unknown command");
}

int FixedwingPositionControl::print_usage(const char *reason)
{
	if (reason) {
		PX4_WARN("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
fw_pos_control_l1 is the fixed wing position controller.

)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("fw_pos_control_l1", "controller");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

	return 0;
}

extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[])
{
	return FixedwingPositionControl::main(argc, argv);
}