PX4-Autopilot/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp

/****************************************************************************
 *
 * Copyright (c) 2025 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 "FwLateralLongitudinalControl.hpp"
#include <px4_platform_common/events.h>

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

using matrix::Dcmf;
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Vector2f;

// [m/s] maximum reference altitude rate threshhold
static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f;
// [us] time after which the wind estimate is disabled if no longer updating
static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s;
// [s] slew rate with which we change altitude time constant
static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f;

static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_VALID{3.f}; ///< Factor of velocity standard deviation above which course calculation is considered good enough
static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_INVALID{2.f}; ///< Factor of velocity standard deviation below which course calculation is considered unsafe
static constexpr float COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK{0.09f}; ///< Cos of Heading to track angle below which it is assumed that the vehicle is not pushed back by the wind ~cos(85°)
static constexpr float COS_HEADING_TRACK_ANGLE_PUSHED_BACK{0.f}; ///< Cos of Heading to track angle above which it is assumed that the vehicle is pushed back by the wind

// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered
static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s;

// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning
static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f;

// [m/s/s] slew rate limit for airspeed setpoint changes
static constexpr float ASPD_SP_SLEW_RATE = 1.f;

FwLateralLongitudinalControl::FwLateralLongitudinalControl(bool is_vtol) :
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
	_attitude_sp_pub(is_vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
	_tecs_status_pub.advertise();
	_flight_phase_estimation_pub.advertise();
	_fixed_wing_lateral_status_pub.advertise();
	parameters_update();
	_airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE);
}

FwLateralLongitudinalControl::~FwLateralLongitudinalControl()
{
	perf_free(_loop_perf);
}
void
FwLateralLongitudinalControl::parameters_update()
{
	updateParams();
	_performance_model.updateParameters();
	_performance_model.runSanityChecks();

	_tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density));
	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
	_tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density));
	_tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed());
	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed());
	_tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed());
	_tecs.set_throttle_damp(_param_fw_t_thr_damping.get());
	_tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.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_roll_throttle_compensation(_param_fw_t_rll2thr.get());
	_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
	_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
	_tecs.set_fast_descend_altitude_error(_param_fw_t_fast_alt_err.get());
	_tecs.set_altitude_rate_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_seb_rate_ff_gain(_param_seb_rate_ff.get());
	_tecs.set_airspeed_measurement_std_dev(_param_speed_standard_dev.get());
	_tecs.set_airspeed_rate_measurement_std_dev(_param_speed_rate_standard_dev.get());
	_tecs.set_airspeed_filter_process_std_dev(_param_process_noise_standard_dev.get());

	_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));

	_tecs_alt_time_const_slew_rate.setSlewRate(TECS_ALT_TIME_CONST_SLEW_RATE);
	_tecs_alt_time_const_slew_rate.setForcedValue(_param_fw_t_h_error_tc.get() * _param_fw_thrtc_sc.get());
}

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

	perf_begin(_loop_perf);

	/* only run controller if position changed */
	// 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();
	}

	if (_local_pos_sub.update(&_local_pos)) {

		const float control_interval = math::constrain((_local_pos.timestamp - _last_time_loop_ran) * 1e-6f,
					       0.001f, 0.1f);
		_last_time_loop_ran = _local_pos.timestamp;

		updateControllerConfiguration();

		_tecs.set_speed_weight(_long_configuration.speed_weight);
		updateTECSAltitudeTimeConstant(checkLowHeightConditions()
					       || _long_configuration.enforce_low_height_condition, control_interval);
		_tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState());

		if (_vehicle_air_data_sub.updated()) {
			_vehicle_air_data_sub.update();
			_air_density = PX4_ISFINITE(_vehicle_air_data_sub.get().rho) ? _vehicle_air_data_sub.get().rho : _air_density;
			_tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density));
			_tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density));
		}

		if (_vehicle_landed_sub.updated()) {
			vehicle_land_detected_s landed{};
			_vehicle_landed_sub.copy(&landed);
			_landed = landed.landed;
		}

		_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;

		_vehicle_status_sub.update();
		_control_mode_sub.update();
		update_control_state();

		if (_control_mode_sub.get().flag_control_manual_enabled && _control_mode_sub.get().flag_control_altitude_enabled
		    && _local_pos.z_reset_counter != _z_reset_counter) {
			if (_control_mode_sub.get().flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) {
				// make TECS accept step in altitude and demanded altitude
				_tecs.handle_alt_step(_long_control_state.altitude_msl, _long_control_state.height_rate);
			}
		}

		const bool should_run = (_control_mode_sub.get().flag_control_position_enabled ||
					 _control_mode_sub.get().flag_control_velocity_enabled ||
					 _control_mode_sub.get().flag_control_acceleration_enabled ||
					 _control_mode_sub.get().flag_control_altitude_enabled ||
					 _control_mode_sub.get().flag_control_climb_rate_enabled) &&
					(_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
					 || _vehicle_status_sub.get().in_transition_mode);

		if (should_run) {

			// ----- Longitudinal ------
			float pitch_sp{NAN};
			float throttle_sp{NAN};

			if (_fw_longitudinal_ctrl_sub.updated()) {
				_fw_longitudinal_ctrl_sub.copy(&_long_control_sp);
			}

			const float airspeed_sp_eas = adapt_airspeed_setpoint(control_interval, _long_control_sp.equivalent_airspeed,
						      _min_airspeed_from_guidance, _lateral_control_state.wind_speed.length());

			// If the both altitude and height rate are set, set altitude setpoint to NAN
			const float altitude_sp = PX4_ISFINITE(_long_control_sp.height_rate) ? NAN : _long_control_sp.altitude;

			tecs_update_pitch_throttle(control_interval, altitude_sp,
						   airspeed_sp_eas,
						   _long_configuration.pitch_min,
						   _long_configuration.pitch_max,
						   _long_configuration.throttle_min,
						   _long_configuration.throttle_max,
						   _long_configuration.sink_rate_target,
						   _long_configuration.climb_rate_target,
						   _long_configuration.disable_underspeed_protection,
						   _long_control_sp.height_rate
						  );

			pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_direct) ? _long_control_sp.pitch_direct : _tecs.get_pitch_setpoint();
			throttle_sp = PX4_ISFINITE(_long_control_sp.throttle_direct) ? _long_control_sp.throttle_direct :
				      _tecs.get_throttle_setpoint();

			// ----- Lateral ------
			float roll_sp {NAN};

			if (_fw_lateral_ctrl_sub.updated()) {
				// We store the update of _fw_lateral_ctrl_sub in a member variable instead of only local such that we can run
				// the controllers also without new setpoints.
				_fw_lateral_ctrl_sub.copy(&_lat_control_sp);
			}

			float airspeed_direction_sp{NAN};
			float lateral_accel_sp {NAN};
			const Vector2f airspeed_vector = _lateral_control_state.ground_speed - _lateral_control_state.wind_speed;

			if (PX4_ISFINITE(_lat_control_sp.course) && !PX4_ISFINITE(_lat_control_sp.airspeed_direction)) {
				// only use the course setpoint if it's finite but airspeed_direction is not

				airspeed_direction_sp = _course_to_airspeed.mapCourseSetpointToHeadingSetpoint(
								_lat_control_sp.course, _lateral_control_state.wind_speed,
								airspeed_sp_eas);

				// Note: the here updated _min_airspeed_from_guidance is only used in the next iteration
				// in the longitudinal controller.
				const float max_true_airspeed = _performance_model.getMaximumCalibratedAirspeed() * _long_control_state.eas2tas;
				_min_airspeed_from_guidance = _course_to_airspeed.getMinAirspeedForCurrentBearing(
								      _lat_control_sp.course, _lateral_control_state.wind_speed,
								      max_true_airspeed, _param_fw_gnd_spd_min.get())
							      / _long_control_state.eas2tas;

			} else if (PX4_ISFINITE(_lat_control_sp.airspeed_direction)) {
				// If the airspeed_direction is finite we use that instead of the course.

				airspeed_direction_sp = _lat_control_sp.airspeed_direction;
				_min_airspeed_from_guidance = 0.f; // reset if no longer in course control

			} else {
				_min_airspeed_from_guidance = 0.f; // reset if no longer in course control
			}

			if (PX4_ISFINITE(airspeed_direction_sp)) {
				const float heading = atan2f(airspeed_vector(1), airspeed_vector(0));
				lateral_accel_sp = _airspeed_direction_control.controlHeading(airspeed_direction_sp, heading,
						   airspeed_vector.norm());
			}

			if (PX4_ISFINITE(_lat_control_sp.lateral_acceleration)) {
				lateral_accel_sp = PX4_ISFINITE(lateral_accel_sp) ? lateral_accel_sp + _lat_control_sp.lateral_acceleration :
						   _lat_control_sp.lateral_acceleration;
			}

			if (!PX4_ISFINITE(lateral_accel_sp)) {
				lateral_accel_sp = 0.f; // mitigation if no valid setpoint is received: 0 lateral acceleration
			}

			lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp);
			lateral_accel_sp = math::constrain(lateral_accel_sp, -_lateral_configuration.lateral_accel_max,
							   _lateral_configuration.lateral_accel_max);
			roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp);

			fixed_wing_lateral_status_s fixed_wing_lateral_status{};
			fixed_wing_lateral_status.timestamp = hrt_absolute_time();
			fixed_wing_lateral_status.lateral_acceleration = lateral_accel_sp;
			fixed_wing_lateral_status.can_run_factor = _can_run_factor;

			_fixed_wing_lateral_status_pub.publish(fixed_wing_lateral_status);

			// additional is_finite checks that should not be necessary, but are kept for safety
			float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f;
			float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f;
			const float yaw_body = _yaw; // yaw is not controlled in fixed wing, need to set it though for quaternion generation
			const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f;

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

			// roll slew rate
			roll_body = _roll_slew_rate.update(roll_body, control_interval);

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

			_att_sp.thrust_body[0] = thrust_body_x;

			_attitude_sp_pub.publish(_att_sp);

		}

		_z_reset_counter = _local_pos.z_reset_counter;
	}

	perf_end(_loop_perf);
}

void FwLateralLongitudinalControl::updateControllerConfiguration()
{
	if (_lateral_configuration.timestamp == 0) {
		_lateral_configuration.timestamp = _local_pos.timestamp;
		_lateral_configuration.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G;

	}

	if (_long_configuration.timestamp == 0) {
		setDefaultLongitudinalControlConfiguration();
	}

	if (_long_control_configuration_sub.updated() || _parameter_update_sub.updated()) {
		longitudinal_control_configuration_s configuration_in{};
		_long_control_configuration_sub.copy(&configuration_in);
		updateLongitudinalControlConfiguration(configuration_in);
	}

	if (_lateral_control_configuration_sub.updated() || _parameter_update_sub.updated()) {
		lateral_control_configuration_s configuration_in{};
		_lateral_control_configuration_sub.copy(&configuration_in);
		_lateral_configuration.timestamp = configuration_in.timestamp;

		if (PX4_ISFINITE(configuration_in.lateral_accel_max)) {
			_lateral_configuration.lateral_accel_max = min(configuration_in.lateral_accel_max, tanf(radians(
						_param_fw_r_lim.get())) * CONSTANTS_ONE_G);

		} else {
			_lateral_configuration.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G;
		}
	}
}

void
FwLateralLongitudinalControl::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, const float desired_max_sinkrate,
		const float desired_max_climbrate,
		bool disable_underspeed_detection, float hgt_rate_sp)
{
	bool tecs_is_running = true;

	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
	if (_vehicle_status_sub.get().is_vtol
	    && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
		|| _vehicle_status_sub.get().in_transition_mode)) {
		tecs_is_running = false;
		return;

	}

	const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min,
						throttle_max, airspeed_sp, _air_density);

	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);

	// HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases
	// when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0.
	const float airspeed_rate_estimate = 0.f;

	_tecs.update(_long_control_state.pitch_rad - radians(_param_fw_psp_off.get()),
		     _long_control_state.altitude_msl,
		     alt_sp,
		     airspeed_sp,
		     _long_control_state.airspeed_eas,
		     _long_control_state.eas2tas,
		     throttle_min,
		     throttle_max,
		     throttle_trim_compensated,
		     pitch_min_rad - radians(_param_fw_psp_off.get()),
		     pitch_max_rad - radians(_param_fw_psp_off.get()),
		     desired_max_climbrate,
		     desired_max_sinkrate,
		     airspeed_rate_estimate,
		     _long_control_state.height_rate,
		     hgt_rate_sp);

	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated);

	if (tecs_is_running && !_vehicle_status_sub.get().in_transition_mode
	    && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
		const TECS::DebugOutput &tecs_output{_tecs.getStatus()};

		// Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving
		if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) &&
		    fabsf(_long_control_state.altitude_msl - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) {
			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL;

		} else if (((tecs_output.altitude_reference - _long_control_state.altitude_msl) >= _param_nav_fw_alt_rad.get()) ||
			   (tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB;

		} else if (((_long_control_state.altitude_msl - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) ||
			   (tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;

		} else {
			//We can't infer the flight phase , do nothing, estimation is reset at each step
		}
	}
}

void
FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
		float true_airspeed_derivative_raw, float throttle_trim)
{
	tecs_status_s tecs_status{};

	const TECS::DebugOutput &debug_output{_tecs.getStatus()};

	tecs_status.altitude_sp = alt_sp;
	tecs_status.altitude_reference = debug_output.altitude_reference;
	tecs_status.altitude_time_constant = _tecs.get_altitude_error_time_constant();
	tecs_status.height_rate_reference = debug_output.height_rate_reference;
	tecs_status.height_rate_direct = debug_output.height_rate_direct;
	tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control;
	tecs_status.height_rate = -_local_pos.vz;
	tecs_status.equivalent_airspeed_sp = equivalent_airspeed_sp;
	tecs_status.true_airspeed_sp = debug_output.true_airspeed_sp;
	tecs_status.true_airspeed_filtered = debug_output.true_airspeed_filtered;
	tecs_status.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
	tecs_status.true_airspeed_derivative = debug_output.true_airspeed_derivative;
	tecs_status.true_airspeed_derivative_raw = true_airspeed_derivative_raw;
	tecs_status.total_energy_rate = debug_output.control.total_energy_rate_estimate;
	tecs_status.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate;
	tecs_status.total_energy_rate_sp = debug_output.control.total_energy_rate_sp;
	tecs_status.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp;
	tecs_status.throttle_integ = debug_output.control.throttle_integrator;
	tecs_status.pitch_integ = debug_output.control.pitch_integrator;
	tecs_status.throttle_sp = _tecs.get_throttle_setpoint();
	tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint();
	tecs_status.throttle_trim = throttle_trim;
	tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio();
	tecs_status.fast_descend_ratio = debug_output.fast_descend;

	tecs_status.timestamp = hrt_absolute_time();

	_tecs_status_pub.publish(tecs_status);
}

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

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

	FwLateralLongitudinalControl *instance = new FwLateralLongitudinalControl(is_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;
}

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

	return true;
}

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

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

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
fw_lat_lon_control computes attitude and throttle setpoints from lateral and longitudinal control setpoints.

)DESCR_STR");

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

	return 0;
}

void FwLateralLongitudinalControl::update_control_state() {
	updateAltitudeAndHeightRate();
	updateAirspeed();
	updateAttitude();
	updateWind();

	_lateral_control_state.ground_speed = Vector2f(_local_pos.vx, _local_pos.vy);
}

void FwLateralLongitudinalControl::updateWind() {
	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();

		_lateral_control_state.wind_speed(0) = wind.windspeed_north;
		_lateral_control_state.wind_speed(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;
	}

	if (!_wind_valid) {
		_lateral_control_state.wind_speed.setZero();
	}
}

void FwLateralLongitudinalControl::updateAltitudeAndHeightRate() {
	float ref_alt{0.f};
	if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) {
		ref_alt = _local_pos.ref_alt;
	}

	_long_control_state.altitude_msl = -_local_pos.z + ref_alt; // Altitude AMSL in meters
	_long_control_state.height_rate = -_local_pos.vz;

}

void FwLateralLongitudinalControl::updateAttitude() {
	vehicle_attitude_s att;

	if (_vehicle_attitude_sub.update(&att)) {

		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_sub.get().is_vtol_tailsitter) {
			const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}};
			R = R * R_offset;
		}

		const Eulerf euler_angles(R);
		_long_control_state.pitch_rad = euler_angles.theta();
		_yaw = euler_angles.psi();

		// load factor due to banking
		const float load_factor_from_bank_angle = 1.0f / max(cosf(euler_angles.phi()), FLT_EPSILON);
		_tecs.set_load_factor(load_factor_from_bank_angle);
	}
}

void FwLateralLongitudinalControl::updateAirspeed() {

	airspeed_validated_s airspeed_validated;

	if (_param_fw_use_airspd.get() && _airspeed_validated_sub.update(&airspeed_validated)) {

		// do not use synthetic airspeed as this would create a thrust loop
		if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
		    && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
		    && airspeed_validated.airspeed_source != airspeed_validated_s::SYNTHETIC) {

			_time_airspeed_last_valid = airspeed_validated.timestamp;
			_long_control_state.airspeed_eas = airspeed_validated.calibrated_airspeed_m_s;
			_long_control_state.eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
		}
	}

	// no airspeed updates for one second --> declare invalid
	const bool airspeed_valid = hrt_elapsed_time(&_time_airspeed_last_valid) < 1_s;

	if (!airspeed_valid) {
		_long_control_state.eas2tas = 1.f;
	}

	_tecs.enable_airspeed(airspeed_valid);
}

float
FwLateralLongitudinalControl::adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint,
		float calibrated_min_airspeed_guidance, float wind_speed)
{
	float system_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor());

	const float system_max_airspeed = _performance_model.getMaximumCalibratedAirspeed();

	// airspeed setpoint adjustments
	if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) {
		calibrated_airspeed_setpoint = _performance_model.getCalibratedTrimAirspeed();

		// Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds.
		if (_wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) {
			system_min_airspeed = math::min(system_min_airspeed + _param_fw_wind_arsp_sc.get() *
			wind_speed, system_max_airspeed);
		}
	}

	// increase setpoint to at what's at least required for the lateral guidance
	calibrated_airspeed_setpoint = math::max(calibrated_airspeed_setpoint, calibrated_min_airspeed_guidance);

	// constrain airspeed to feasible range
	calibrated_airspeed_setpoint = math::constrain(calibrated_airspeed_setpoint, system_min_airspeed, system_max_airspeed);

	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) {

		// initialize the airspeed setpoint
		if (PX4_ISFINITE(_long_control_state.airspeed_eas) && _long_control_state.airspeed_eas < system_min_airspeed) {
			// current airpseed is below minimum - init with minimum
			_airspeed_slew_rate_controller.setForcedValue(system_min_airspeed);

		} else if (PX4_ISFINITE(_long_control_state.airspeed_eas) && _long_control_state.airspeed_eas > system_max_airspeed) {
			// current airpseed is above maximum - init with maximum
			_airspeed_slew_rate_controller.setForcedValue(system_max_airspeed);

		} else if (PX4_ISFINITE(_long_control_state.airspeed_eas)) {
			// current airpseed is between min and max - init with current
			_airspeed_slew_rate_controller.setForcedValue(_long_control_state.airspeed_eas);

		} else {
			// current airpseed is invalid - init with setpoint
			_airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint);
		}
	} else {
		// update slew rate state
		if (_airspeed_slew_rate_controller.getState() < system_min_airspeed) {
			// current airpseed setpoint is below minimum - reset to minimum
			_airspeed_slew_rate_controller.setForcedValue(system_min_airspeed);

		} else if (_airspeed_slew_rate_controller.getState() > system_max_airspeed) {
			// current airpseed setpoint is above maximum - reset to maximum
			_airspeed_slew_rate_controller.setForcedValue(system_max_airspeed);

		} else if (PX4_ISFINITE(_long_control_state.airspeed_eas)) {
			// current airpseed setpoint is between min and max - update
			_airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval);

		}
	}

	return _airspeed_slew_rate_controller.getState();
}

bool FwLateralLongitudinalControl::checkLowHeightConditions() const
{
	// Are conditions for low-height
	return _param_fw_t_thr_low_hgt.get() >= 0.f && _local_pos.dist_bottom_valid
	       && _local_pos.dist_bottom < _param_fw_t_thr_low_hgt.get();
}

void FwLateralLongitudinalControl::updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt)
{
	// Target time constant for the TECS altitude tracker
	float alt_tracking_tc = _param_fw_t_h_error_tc.get();

	if (is_low_height) {
		// If low-height conditions satisfied, compute target time constant for altitude tracking
		alt_tracking_tc *= _param_fw_thrtc_sc.get();
	}

	_tecs_alt_time_const_slew_rate.update(alt_tracking_tc, dt);
}

float FwLateralLongitudinalControl::getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const
{
	if (is_wind_valid) {
		// If we have a valid wind estimate, npfg is able to handle all degenerated cases
		return 1.f;
	}

	// NPFG can run without wind information as long as the system is not flying backwards and has a minimal ground speed
	// Check the minimal ground speed. if it is greater than twice the standard deviation, we assume that we can infer a valid track angle
	const Vector2f ground_vel(local_pos.vx, local_pos.vy);
	const float ground_speed(ground_vel.norm());
	const float low_ground_speed_factor(math::constrain((ground_speed - HORIZONTAL_EVH_FACTOR_COURSE_INVALID *
									    local_pos.evh) / ((HORIZONTAL_EVH_FACTOR_COURSE_VALID - HORIZONTAL_EVH_FACTOR_COURSE_INVALID)*local_pos.evh),
							    0.f, 1.f));

	// Check that the angle between heading and track is not off too much. if it is greater than 90° we will be pushed back from the wind and the npfg will propably give a roll command in the wrong direction.
	const Vector2f heading_vector(matrix::Dcm2f(local_pos.heading)*Vector2f({1.f, 0.f}));
	const Vector2f ground_vel_norm(ground_vel.normalized());
	const float flying_forward_factor(math::constrain((heading_vector.dot(ground_vel_norm) -
							   COS_HEADING_TRACK_ANGLE_PUSHED_BACK) / ((COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK -
												    COS_HEADING_TRACK_ANGLE_PUSHED_BACK)),
							  0.f, 1.f));

	return flying_forward_factor * low_ground_speed_factor;
}
float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float lateral_accel_sp)
{
	// Scale the npfg output to zero if npfg is not certain for correct output
	_can_run_factor = math::constrain(getGuidanceQualityFactor(_local_pos, _wind_valid), 0.f, 1.f);

	hrt_abstime now{hrt_absolute_time()};

	// Warn the user when the scale is less than 90% for at least 2 seconds (disable in transition)

	// If the npfg was not running before, reset the user warning variables.
	if ((now - _time_since_last_npfg_call) > ROLL_WARNING_TIMEOUT) {
		_need_report_npfg_uncertain_condition = true;
		_time_since_first_reduced_roll = 0U;
	}

	if (_vehicle_status_sub.get().in_transition_mode || _can_run_factor > ROLL_WARNING_CAN_RUN_THRESHOLD || _landed) {
		// NPFG reports a good condition or we are in transition, reset the user warning variables.
		_need_report_npfg_uncertain_condition = true;
		_time_since_first_reduced_roll = 0U;

	} else if (_need_report_npfg_uncertain_condition) {
		if (_time_since_first_reduced_roll == 0U) {
			_time_since_first_reduced_roll = now;
		}

		if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) {
			_need_report_npfg_uncertain_condition = false;
			events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning,
				     "Roll command reduced due to uncertain velocity/wind estimates!");
		}

	} else {
		// Nothing to do, already reported.
	}

	_time_since_last_npfg_call = now;

	return _can_run_factor * (lateral_accel_sp);
}
float FwLateralLongitudinalControl::mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const {
	return  atanf(lateral_acceleration_sp / CONSTANTS_ONE_G);
}

void FwLateralLongitudinalControl::setDefaultLongitudinalControlConfiguration() {
	_long_configuration.timestamp = hrt_absolute_time();
	_long_configuration.pitch_min = radians(_param_fw_p_lim_min.get());
	_long_configuration.pitch_max = radians(_param_fw_p_lim_max.get());
	_long_configuration.throttle_min = _param_fw_thr_min.get();
	_long_configuration.throttle_max = _param_fw_thr_max.get();
	_long_configuration.climb_rate_target = _param_climbrate_target.get();
	_long_configuration.sink_rate_target = _param_sinkrate_target.get();
	_long_configuration.disable_underspeed_protection = false;
	_long_configuration.enforce_low_height_condition = false;
}

void FwLateralLongitudinalControl::updateLongitudinalControlConfiguration(const longitudinal_control_configuration_s &configuration_in) {
	_long_configuration.timestamp = configuration_in.timestamp;

	if (PX4_ISFINITE(configuration_in.pitch_min)) {
		_long_configuration.pitch_min = math::constrain(configuration_in.pitch_min, radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()));
	} else {
		_long_configuration.pitch_min = radians(_param_fw_p_lim_min.get());
	}

	if (PX4_ISFINITE(configuration_in.pitch_max)) {
		_long_configuration.pitch_max = math::constrain(configuration_in.pitch_max, _long_configuration.pitch_min, radians(_param_fw_p_lim_max.get()));
	} else {
		_long_configuration.pitch_max = radians(_param_fw_p_lim_max.get());
	}

	if (PX4_ISFINITE(configuration_in.throttle_min)) {
		_long_configuration.throttle_min = math::constrain(configuration_in.throttle_min, _param_fw_thr_min.get(), _param_fw_thr_max.get());
	} else {
		_long_configuration.throttle_min = _param_fw_thr_min.get();
	}

	if (PX4_ISFINITE(configuration_in.throttle_max)) {
		_long_configuration.throttle_max = math::constrain(configuration_in.throttle_max, _long_configuration.throttle_min, _param_fw_thr_max.get());
	} else {
		_long_configuration.throttle_max = _param_fw_thr_max.get();
	}

	if (PX4_ISFINITE(configuration_in.climb_rate_target)) {
		_long_configuration.climb_rate_target = math::max(0.0f, configuration_in.climb_rate_target);
	} else {
		_long_configuration.climb_rate_target = _param_climbrate_target.get();
	}

	if (PX4_ISFINITE(configuration_in.sink_rate_target)) {
		_long_configuration.sink_rate_target = math::max(0.0f, configuration_in.sink_rate_target);
	} else {
		_long_configuration.sink_rate_target = _param_sinkrate_target.get();
	}
}

float FwLateralLongitudinalControl::getLoadFactor() const
{
	float load_factor_from_bank_angle = 1.f;

	const float roll_body = Eulerf(Quatf(_att_sp.q_d)).phi();

	if (PX4_ISFINITE(roll_body)) {
		load_factor_from_bank_angle = 1.f / math::max(cosf(roll_body), FLT_EPSILON);
	}

	return load_factor_from_bank_angle;
}

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