TECS: fix publishing of tecs_status

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

msg/tecs_status.msg

@@ -6,6 +6,7 @@ uint8 TECS_MODE_LAND = 3
 uint8 TECS_MODE_LAND_THROTTLELIM = 4
 uint8 TECS_MODE_BAD_DESCENT = 5
 uint8 TECS_MODE_CLIMBOUT = 6
+uint8 TECS_MODE_ECO = 7
 
 
 float32 altitude_sp
@@ -42,3 +43,9 @@ float32 throttle_sp
 float32 pitch_sp_rad
 
 uint8 mode
+
+uint8 flight_phase
+
+uint8 TECS_FLIGHT_PHASE_LEVEL 	= 0		# In level flight (no altitude setpoint change)
+uint8 TECS_FLIGHT_PHASE_CLIMB 	= 1		# Climb is triggered (altitude setpoint is increasing)
+uint8 TECS_FLIGHT_PHASE_DESCEND = 2		# Descend is triggered (altitude setpoint is descreasing)

src/lib/tecs/TECS.cpp

@@ -50,6 +50,12 @@ static constexpr float DT_MAX = 1.0f;	///< max value of _dt allowed before a fil
  * @author Paul Riseborough
  */
 
+
+TECS::TECS()
+{
+	_tecs_status_pub.advertise();
+}
+
 /*
  * This function implements a complementary filter to estimate the climb rate when
  * inertial nav data is not available. It also calculates a true airspeed derivative
@@ -227,7 +233,7 @@ void TECS::_update_energy_estimates()
 	// Calculate the specific energy balance demand which specifies how the available total
 	// energy should be allocated to speed (kinetic energy) and height (potential energy)
 	// Calculate the specific energy balance error
-	_SEB_error = SEB_setpoint() - (_SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting);
+	_SEB_error = get_SEB_setpoint() - (_SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting);
 
 	// Calculate specific energy rate demands in units of (m**2/sec**3)
 	_SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change
@@ -452,6 +458,38 @@ void TECS::_updateTrajectoryGenerationConstraints()
 	_velocity_control_traj_generator.setMaxVelDown(_max_climb_rate); // different convention for FW than for MC
 }
 
+void TECS::_updateFlightPhase(float altitude_sp_amsl, float height_rate_setpoint)
+{
+	const bool input_is_height_rate = PX4_ISFINITE(height_rate_setpoint);
+
+	// update flight phase state (are we flying level, climbing or descending)
+	if (input_is_height_rate) {
+
+		if (PX4_ISFINITE(_velocity_control_traj_generator.getCurrentPosition())) {
+			// we have a valid altitude setpoint which means that we are flying level
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_LEVEL;
+
+		} else if (_velocity_control_traj_generator.getCurrentVelocity() > FLT_EPSILON) {
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_CLIMB;
+
+		} else {
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_DESCEND;
+		}
+
+	} else {
+		// stay in flight phase level if only small altitude changes are demanded (<10m)
+		if (altitude_sp_amsl - _alt_control_traj_generator.getCurrentPosition() > 10.0f) {
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_CLIMB;
+
+		} else if (altitude_sp_amsl - _alt_control_traj_generator.getCurrentPosition() < -10.0f) {
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_DESCEND;
+
+		} else {
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_LEVEL;
+		}
+	}
+}
+
 void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate,
 					float target_sinkrate, float altitude_amsl)
 {
@@ -554,7 +592,7 @@ void TECS::_update_STE_rate_lim()
 void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
 				 float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
 				 float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max,
-				 float target_climbrate, float target_sinkrate, float hgt_rate_sp)
+				 float hgt_rate_sp, bool eco_mode_enabled)
 {
 	// Calculate the time since last update (seconds)
 	uint64_t now = hrt_absolute_time();
@@ -591,10 +629,16 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	// Detect an uncommanded descent caused by an unachievable airspeed demand
 	_detect_uncommanded_descent();
 
+	_eco_mode_enabled = eco_mode_enabled;
+
 	// Calculate the demanded true airspeed
 	_update_speed_setpoint();
 
-	_calculateHeightRateSetpoint(hgt_setpoint, hgt_rate_sp, target_climbrate, target_sinkrate, baro_altitude);
+	_updateFlightPhase(hgt_setpoint, hgt_rate_sp);
+
+	const float target_climb_rate = _tecs_mode == ECL_TECS_MODE_ECO ? _target_climb_rate_eco : _target_climb_rate;
+
+	_calculateHeightRateSetpoint(hgt_setpoint, hgt_rate_sp, target_climb_rate, _target_sink_rate, baro_altitude);
 
 	// Calculate the specific energy values required by the control loop
 	_update_energy_estimates();
@@ -618,15 +662,24 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	} else if (_climbout_mode_active) {
 		_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
 
+	} else if (_eco_mode_enabled) {
+		_tecs_mode = ECL_TECS_MODE_ECO;
+
 	} else {
 		// This is the default operation mode
 		_tecs_mode = ECL_TECS_MODE_NORMAL;
 	}
 
+	tecs_status_publish(now);
 }
 
 void TECS::_update_speed_height_weights()
 {
+	if (_eco_mode_enabled) {
+		_pitch_speed_weight = _pitch_speed_weight_eco;
+		_height_error_gain = _height_error_gain_eco;
+	}
+
 	// Calculate the weight applied to control of specific kinetic energy error
 	_SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f);
 
@@ -642,3 +695,67 @@ void TECS::_update_speed_height_weights()
 	_SPE_weighting = constrain(2.0f - _SKE_weighting, 0.f, 1.f);
 	_SKE_weighting = constrain(_SKE_weighting, 0.f, 1.f);
 }
+
+void
+TECS::tecs_status_publish(const hrt_abstime &now)
+{
+	tecs_status_s tecs_status{};
+
+	switch (_tecs_mode) {
+	case TECS::ECL_TECS_MODE_NORMAL:
+		tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL;
+		break;
+
+	case TECS::ECL_TECS_MODE_UNDERSPEED:
+		tecs_status.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
+		break;
+
+	case TECS::ECL_TECS_MODE_BAD_DESCENT:
+		tecs_status.mode = tecs_status_s::TECS_MODE_BAD_DESCENT;
+		break;
+
+	case TECS::ECL_TECS_MODE_CLIMBOUT:
+		tecs_status.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
+		break;
+
+	case TECS::ECL_TECS_MODE_ECO:
+		tecs_status.mode = tecs_status_s::TECS_MODE_ECO;
+		break;
+
+	default:
+		tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL;
+	}
+
+	tecs_status.altitude_sp = _hgt_setpoint;
+	tecs_status.altitude_filtered = _vert_pos_state;
+	tecs_status.energy_distribution_error = _SEB_error;
+	tecs_status.energy_distribution_rate_error = _SEB_rate_error;
+	tecs_status.equivalent_airspeed_sp = _EAS_setpoint;
+	tecs_status.flight_phase = _flight_phase;
+	tecs_status.height_rate_setpoint = _hgt_rate_setpoint;
+	tecs_status.height_rate = _vert_vel_state;
+	tecs_status.pitch_integ = _pitch_integ_state;
+	tecs_status.pitch_sp_rad = get_pitch_setpoint();
+	tecs_status.throttle_integ = _throttle_integ_state;
+	tecs_status.throttle_sp = get_throttle_setpoint();
+	tecs_status.total_energy_balance = get_SEB();
+	tecs_status.total_energy_balance_sp = get_SEB_setpoint();
+	tecs_status.total_energy_balance_rate = get_SEB_rate();
+	tecs_status.total_energy_balance_rate_sp = get_SEB_rate_setpoint();
+	tecs_status.total_energy = get_STE();
+	tecs_status.total_energy_error = _STE_error;
+	tecs_status.total_energy_sp = get_STE_setpoint();
+	tecs_status.total_energy_rate = get_STE_rate();
+	tecs_status.total_energy_rate_error = _STE_rate_error;
+	tecs_status.total_energy_rate_sp = get_STE_rate_setpoint();
+	tecs_status.true_airspeed_sp = _TAS_setpoint_adj;
+	tecs_status.true_airspeed_filtered = _tas_state;
+	tecs_status.true_airspeed_derivative = _tas_rate_filtered;
+	tecs_status.true_airspeed_derivative_raw = _tas_rate_raw;
+	tecs_status.true_airspeed_derivative_sp = _TAS_rate_setpoint;
+	tecs_status.true_airspeed_innovation = _tas_innov;
+
+	tecs_status.timestamp = now;
+
+	_tecs_status_pub.publish(tecs_status);
+}

src/lib/tecs/TECS.hpp

@@ -49,10 +49,14 @@
 #include <motion_planning/VelocitySmoothing.hpp>
 #include <motion_planning/ManualVelocitySmoothingZ.hpp>
 
+#include <uORB/Publication.hpp>
+#include <uORB/topics/tecs_status.h>
+#include <uORB/uORB.h>
+
 class TECS
 {
 public:
-	TECS() = default;
+	TECS();
 	~TECS() = default;
 
 	// no copy, assignment, move, move assignment
@@ -90,11 +94,8 @@ public:
 	void update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
 				   float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
 				   float throttle_min, float throttle_setpoint_max, float throttle_cruise,
-				   float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN);
-
-	float get_throttle_setpoint() { return _last_throttle_setpoint; }
-	float get_pitch_setpoint() { return _last_pitch_setpoint; }
-	float get_speed_weight() { return _pitch_speed_weight; }
+				   float pitch_limit_min, float pitch_limit_max, float hgt_rate_sp = NAN,
+				   bool eco_mode_enabled = false);
 
 	void reset_state() { _states_initialized = false; }
 
@@ -102,7 +103,8 @@ public:
 		ECL_TECS_MODE_NORMAL = 0,
 		ECL_TECS_MODE_UNDERSPEED,
 		ECL_TECS_MODE_BAD_DESCENT,
-		ECL_TECS_MODE_CLIMBOUT
+		ECL_TECS_MODE_CLIMBOUT,
+		ECL_TECS_MODE_ECO
 	};
 
 	void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }
@@ -140,51 +142,31 @@ public:
 
 	void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; }
 
+	void set_target_climb_rate(float target_climb_rate) { _target_climb_rate = target_climb_rate; }
+	void set_target_sink_rate(float target_sink_rate) { _target_sink_rate = target_sink_rate; }
 
-	// TECS status
-	uint64_t timestamp() { return _pitch_update_timestamp; }
-	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
+	// eco mode settings
+	void set_speed_weight_eco(float weight_eco) { _pitch_speed_weight_eco = weight_eco; }
+	void set_height_error_time_constant_eco(float time_const_eco) { _height_error_gain_eco = 1.0f / math::max(time_const_eco, 0.1f); }
+	void set_target_climb_rate_eco(float target_climb_rate_eco) { _target_climb_rate_eco = target_climb_rate_eco; }
 
-	float hgt_setpoint() { return _hgt_setpoint; }
-	float vert_pos_state() { return _vert_pos_state; }
+	// getters
+	float get_throttle_setpoint() { return _last_throttle_setpoint; }
+	float get_pitch_setpoint() { return _last_pitch_setpoint; }
 
-	float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
-	float tas_state() { return _tas_state; }
-	float getTASInnovation() { return _tas_innov; }
-
-	float hgt_rate_setpoint() { return _hgt_rate_setpoint; }
-	float vert_vel_state() { return _vert_vel_state; }
-
-	float get_EAS_setpoint() { return _EAS_setpoint; };
-	float TAS_rate_setpoint() { return _TAS_rate_setpoint; }
-	float speed_derivative() { return _tas_rate_filtered; }
-	float speed_derivative_raw() { return _tas_rate_raw; }
-
-	float STE_error() { return _STE_error; }
-	float STE_rate_error() { return _STE_rate_error; }
-
-	float SEB_error() { return _SEB_error; }
-	float SEB_rate_error() { return _SEB_rate_error; }
-
-	float throttle_integ_state() { return _throttle_integ_state; }
-	float pitch_integ_state() { return _pitch_integ_state; }
-
-	float STE() { return _SPE_estimate + _SKE_estimate; }
-
-	float STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; }
-
-	float STE_rate() { return _SPE_rate + _SKE_rate; }
-
-	float STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; }
-
-	float SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; }
-
-	float SEB_setpoint() { return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting; }
-
-	float SEB_rate() { return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting; }
-
-	float SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; }
+	float get_hgt_setpoint() { return _hgt_setpoint; }
+	float get_hgt_rate_setpoint() { return _hgt_rate_setpoint; }
+	float get_STE() { return _SPE_estimate + _SKE_estimate; }
+	float get_STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; }
+	float get_STE_rate() { return _SPE_rate + _SKE_rate; }
+	float get_STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; }
+	float get_SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; }
+	float get_SEB_setpoint() { return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting; }
+	float get_SEB_rate() { return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting; }
+	float get_SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; }
+	int get_flight_phase() { return _flight_phase; }
 
+	void tecs_status_publish(const hrt_abstime &now);
 
 	/**
 	 * Handle the altitude reset
@@ -237,6 +219,10 @@ private:
 	float _speed_derivative_time_const{0.01f};			///< speed derivative filter time constant (s)
 	float _SEB_rate_ff{1.0f};
 
+	float _height_error_gain_eco{0.2f};				///< in eco mode: height error inverse time constant [1/s]
+	float _pitch_speed_weight_eco{1.0f};				///< in eco mode: speed control weighting used by pitch demand calculation
+	float _target_climb_rate_eco{2.f};
+
 	// complimentary filter states
 	float _vert_vel_state{0.0f};					///< complimentary filter state - height rate (m/sec)
 	float _vert_pos_state{0.0f};					///< complimentary filter state - height (m)
@@ -295,6 +281,9 @@ private:
 	float _SPE_weighting{1.0f};
 	float _SKE_weighting{1.0f};
 
+	float _target_climb_rate{3.f};
+	float _target_sink_rate{2.f};
+
 	// time steps (non-fixed)
 	float _dt{DT_DEFAULT};						///< Time since last update of main TECS loop (sec)
 	static constexpr float DT_DEFAULT = 0.02f;			///< default value for _dt (sec)
@@ -307,6 +296,10 @@ private:
 	bool _airspeed_enabled{false};					///< true when airspeed use has been enabled
 	bool _states_initialized{false};					///< true when TECS states have been iniitalized
 	bool _in_air{false};						///< true when the vehicle is flying
+	bool _eco_mode_enabled{false};
+
+	// flight phase
+	int _flight_phase{tecs_status_s::TECS_FLIGHT_PHASE_LEVEL};
 
 	/**
 	 * Update the airspeed internal state using a second order complementary filter
@@ -351,6 +344,8 @@ private:
 
 	void _updateTrajectoryGenerationConstraints();
 
+	void _updateFlightPhase(float altitude_sp_amsl, float height_rate_setpoint);
+
 	void _calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate,
 					  float target_sinkrate, float altitude_amsl);
 
@@ -376,4 +371,6 @@ private:
 	ManualVelocitySmoothingZ
 	_velocity_control_traj_generator;	// generates height rate and altitude setpoint trajectory when height rate is commanded
 
+	uORB::Publication<tecs_status_s>	_tecs_status_pub{ORB_ID(tecs_status)};
+
 };

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -70,8 +70,6 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	// limit to 50 Hz
 	_local_pos_sub.set_interval_ms(20);
 
-	_tecs_status_pub.advertise();
-
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -131,6 +129,12 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get());
 	_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
 
+	_tecs.set_speed_weight_eco(_param_fw_t_spdweight_eco.get());
+	_tecs.set_height_error_time_constant_eco(_param_fw_t_alt_tc_eco.get());
+
+	_tecs.set_target_climb_rate(_param_climbrate_target.get());
+	_tecs.set_target_sink_rate(_param_sinkrate_target.get());
+	_tecs.set_target_climb_rate_eco(_param_fw_t_clmb_r_sp_eco.get());
 
 	// Landing slope
 	/* check if negative value for 2/3 of flare altitude is set for throttle cut */
@@ -354,24 +358,64 @@ FixedwingPositionControl::get_demanded_airspeed()
 	return altctrl_airspeed;
 }
 
+
 float
-FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed)
+FixedwingPositionControl::get_cruise_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed,
+		const Vector2f &ground_speed, float dt, const FW_SPEED_MODE spd_mode)
 {
+	float airspeed_setpoint = _param_fw_airspd_trim.get();
+
+	if (spd_mode == FW_SPEED_MODE::FW_SPEED_MODE_ECO) {
+		airspeed_setpoint = _param_fw_airspd_min.get();
+
+		// Adapt cruise airspeed setpoint based on wind estimate (disable in airspeed-less mode)
+		if (_airspeed_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) {
+			wind_s wind_estimate;
+
+			if (_wind_sub.update(&wind_estimate)) {
+				const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east);
+
+				if (PX4_ISFINITE(wind.length())) {
+					airspeed_setpoint += _param_fw_wind_arsp_sc.get() * wind.length();
+				}
+			}
+		}
+
+	} else if (spd_mode == FW_SPEED_MODE::FW_SPEED_MODE_DASH) {
+		airspeed_setpoint = _param_fw_airspd_max.get();
+	}
+
+	// Adapt cruise airspeed setpoint if given from other source (e.g. position setpoint)
+	if (PX4_ISFINITE(pos_sp_cru_airspeed) && pos_sp_cru_airspeed > 0.1f) {
+		airspeed_setpoint = constrain(pos_sp_cru_airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_max.get());
+	}
+
+	// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
+	if (!_l1_control.circle_mode()) {
+		/*
+		 * 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 += max(_param_fw_gnd_spd_min.get() - ground_speed_body, 0.0f);
+		}
+	}
+
+	// Constrain cruise airspeed to be in valid and safe range
+
 	/*
 	 * Calculate accelerated stall airspeed factor from commanded bank angle and use it to increase minimum airspeed.
 	 *
-	 *  We don't know the stall speed of the aircraft, but assuming user defined
-	 *  minimum airspeed (FW_AIRSPD_MIN) is slightly larger than stall speed
-	 *  this is close enough.
-	 *
-	 * increase lift vector to balance additional weight in bank
-	 *  cos(bank angle) = W/L = 1/n
-	 *   n is the load factor
-	 *
-	 * lift is proportional to airspeed^2 so the increase in stall speed is
-	 *  Vsacc = Vs * sqrt(n)
+	 * Increase lift vector to balance additional weight in bank
+	 * cos(bank angle) = W/L = 1/n, n is the load factor
 	 *
+	 * lift is proportional to airspeed^2 so the increase in stall speed is Vsacc = Vs * sqrt(n)
 	 */
+
 	float adjusted_min_airspeed = _param_fw_airspd_min.get();
 
 	if (_airspeed_valid && PX4_ISFINITE(_att_sp.roll_body)) {
@@ -380,88 +424,98 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const
 						  _param_fw_airspd_min.get(), _param_fw_airspd_max.get());
 	}
 
-	// groundspeed undershoot
-	if (!_l1_control.circle_mode()) {
-		/*
-		 * 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.
-		 */
-		const float ground_speed_body = _body_velocity(0);
+	// constrain airspeed setpoint changes with slew rate of 1 m/s/s
+	const float airspeed_setpoint_slew_rate = 1.f;
 
-		if (ground_speed_body < _param_fw_gnd_spd_min.get()) {
-			airspeed_demand += max(_param_fw_gnd_spd_min.get() - ground_speed_body, 0.0f);
-		}
+	// initialize to current airspeed setpoint
+	if (!PX4_ISFINITE(_last_airspeed_setpoint)) {
+		_last_airspeed_setpoint = airspeed_setpoint;
 	}
 
-	// add minimum ground speed undershoot (only non-zero in presence of sufficient wind)
-	// sanity check: limit to range
-	return constrain(airspeed_demand, adjusted_min_airspeed, _param_fw_airspd_max.get());
+	if (dt > FLT_EPSILON) {
+		airspeed_setpoint = constrain(airspeed_setpoint, _last_airspeed_setpoint - airspeed_setpoint_slew_rate * dt,
+					      _last_airspeed_setpoint + airspeed_setpoint_slew_rate * dt);
+		_last_airspeed_setpoint = airspeed_setpoint;
+	}
+
+	return constrain(airspeed_setpoint, adjusted_min_airspeed, _param_fw_airspd_max.get());
+}
+
+FixedwingPositionControl::FW_SPEED_MODE
+FixedwingPositionControl::getSpeedMode()
+{
+	FW_SPEED_MODE_COMMANDED new_mode = static_cast<FW_SPEED_MODE_COMMANDED>(_param_fw_spd_mode_set.get());
+
+	const float altitude_amsl_min = max(_tecs.get_hgt_setpoint() - _param_fw_spdm_alt_er_u.get(),
+					    _local_pos.ref_alt + _param_fw_alt_spdm_min.get());
+	const float altitude_amsl_max = _tecs.get_hgt_setpoint() + _param_fw_spdm_alt_er_o.get();
+
+	const bool altitude_conditions_met = _current_altitude <= altitude_amsl_max
+					     && _current_altitude >= altitude_amsl_min;
+
+	if (!altitude_conditions_met) {
+		// reset timer
+		_time_conditions_not_met = _local_pos.timestamp;
+	}
+
+	// add a 10s timeout after conditions where not met
+	const bool conditions_for_eco_dash_met = hrt_elapsed_time(&_time_conditions_not_met) > 10_s;
+
+	FW_SPEED_MODE speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; //default;
+
+	switch (new_mode) {
+	case NORMAL:
+		speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; //default
+		break;
+
+	case ECO_CRUISE:
+		if (conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) {
+			speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO;
+
+		} else {
+			speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL;
+		}
+
+		break;
+
+	case ECO_FULL:
+		if (conditions_for_eco_dash_met) {
+			speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO;
+
+		} else {
+			speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL;
+		}
+
+		break;
+
+	case DASH_CRUISE:
+		if (conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) {
+			speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH;
+
+		} else {
+			speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL;
+		}
+
+		break;
+
+	case DASH_FULL:
+		if (conditions_for_eco_dash_met) {
+			speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH;
+
+		} else {
+			speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL;
+		}
+
+		break;
+	}
+
+	return speed_mode_current;
 }
 
 void
-FixedwingPositionControl::tecs_status_publish()
+FixedwingPositionControl::resetEcoDashAllowed()
 {
-	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.timestamp = hrt_absolute_time();
-
-	_tecs_status_pub.publish(t);
+	_time_conditions_not_met = _local_pos.timestamp;
 }
 
 void
@@ -612,7 +666,6 @@ FixedwingPositionControl::getManualHeightRateSetpoint()
 		const float climb_rate_target = _param_climbrate_target.get();
 
 		height_rate_setpoint = pitch * climb_rate_target;
-
 	}
 
 	return height_rate_setpoint;
@@ -734,6 +787,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 	/* get circle mode */
 	const bool was_circle_mode = _l1_control.circle_mode();
 
+	FW_SPEED_MODE spd_mode = getSpeedMode();
+
 	/* restore 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());
@@ -784,15 +839,15 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_POSITION:
-		control_auto_position(now, curr_pos, ground_speed, pos_sp_prev, current_sp);
+		control_auto_position(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, spd_mode);
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_LOITER:
-		control_auto_loiter(now, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
+		control_auto_loiter(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next, spd_mode);
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_LAND:
-		control_auto_landing(now, curr_pos, ground_speed, pos_sp_prev, current_sp);
+		control_auto_landing(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_TAKEOFF:
@@ -907,7 +962,7 @@ FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
 				   _param_fw_thr_cruise.get(),
 				   false,
 				   _param_fw_p_lim_min.get(),
-				   tecs_status_s::TECS_MODE_NORMAL,
+				   false,
 				   descend_rate);
 
 	_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
@@ -982,8 +1037,9 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 }
 
 void
-FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Vector2d &curr_pos,
-		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
+FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
+		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
+		const FW_SPEED_MODE spd_mode)
 {
 	const float acc_rad = _l1_control.switch_distance(500.0f);
 	Vector2d curr_wp{0, 0};
@@ -1006,15 +1062,6 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve
 		prev_wp(1) = pos_sp_curr.lon;
 	}
 
-
-	float mission_airspeed = _param_fw_airspd_trim.get();
-
-	if (PX4_ISFINITE(pos_sp_curr.cruising_speed) &&
-	    pos_sp_curr.cruising_speed > 0.1f) {
-
-		mission_airspeed = pos_sp_curr.cruising_speed;
-	}
-
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
 	float tecs_fw_mission_throttle;
@@ -1080,20 +1127,21 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve
 	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 	tecs_update_pitch_throttle(now, position_sp_alt,
-				   calculate_target_airspeed(mission_airspeed, ground_speed),
+				   get_cruise_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt, spd_mode),
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
 				   tecs_fw_mission_throttle,
 				   false,
-				   radians(_param_fw_p_lim_min.get()));
+				   radians(_param_fw_p_lim_min.get()),
+				   spd_mode == FW_SPEED_MODE_ECO);
 }
 
 void
-FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vector2d &curr_pos,
+FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const float dt, 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)
+		const position_setpoint_s &pos_sp_next, const FW_SPEED_MODE spd_mode)
 {
 	Vector2d curr_wp{0, 0};
 	Vector2d prev_wp{0, 0};
@@ -1114,13 +1162,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect
 		prev_wp(1) = pos_sp_curr.lon;
 	}
 
-	float mission_airspeed = _param_fw_airspd_trim.get();
-
-	if (PX4_ISFINITE(pos_sp_curr.cruising_speed) &&
-	    pos_sp_curr.cruising_speed > 0.1f) {
-
-		mission_airspeed = pos_sp_curr.cruising_speed;
-	}
+	float mission_airspeed = pos_sp_curr.cruising_speed;
 
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
@@ -1191,14 +1233,15 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect
 	}
 
 	tecs_update_pitch_throttle(now, alt_sp,
-				   calculate_target_airspeed(mission_airspeed, ground_speed),
+				   get_cruise_airspeed_setpoint(now, mission_airspeed, ground_speed, dt, spd_mode),
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
 				   tecs_fw_mission_throttle,
 				   false,
-				   radians(_param_fw_p_lim_min.get()));
+				   radians(_param_fw_p_lim_min.get()),
+				   spd_mode == FW_SPEED_MODE_ECO);
 }
 
 void
@@ -1261,15 +1304,15 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
 
 		tecs_update_pitch_throttle(now, pos_sp_curr.alt,
-					   calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed),
+					   get_cruise_airspeed_setpoint(now, _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
+							   dt, FW_SPEED_MODE::FW_SPEED_MODE_NORMAL),
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(takeoff_pitch_max_deg),
 					   _param_fw_thr_min.get(),
 					   _param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here?
 					   _param_fw_thr_cruise.get(),
 					   _runway_takeoff.climbout(),
-					   radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())),
-					   tecs_status_s::TECS_MODE_TAKEOFF);
+					   radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())));
 
 		// assign values
 		_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
@@ -1340,15 +1383,14 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 							   takeoff_throttle,
 							   _param_fw_thr_cruise.get(),
 							   true,
-							   radians(_takeoff_pitch_min.get()),
-							   tecs_status_s::TECS_MODE_TAKEOFF);
+							   radians(_takeoff_pitch_min.get()));
 
 				/* limit roll motion to ensure enough lift */
 				_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
 
 			} else {
 				tecs_update_pitch_throttle(now, pos_sp_curr.alt,
-							   calculate_target_airspeed(_param_fw_airspd_trim.get(), ground_speed),
+							   get_cruise_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed, dt, FW_SPEED_MODE::FW_SPEED_MODE_NORMAL),
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(_param_fw_p_lim_max.get()),
 							   _param_fw_thr_min.get(),
@@ -1373,7 +1415,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 }
 
 void
-FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
+FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
 	/* current waypoint (the one currently heading for) */
@@ -1566,7 +1608,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 		const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
 
 		tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel,
-					   calculate_target_airspeed(airspeed_land, ground_speed),
+					   get_cruise_airspeed_setpoint(now, airspeed_land, ground_speed, dt, FW_SPEED_MODE::FW_SPEED_MODE_NORMAL),
 					   radians(_param_fw_lnd_fl_pmin.get()),
 					   radians(_param_fw_lnd_fl_pmax.get()),
 					   0.0f,
@@ -1574,7 +1616,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 					   throttle_land,
 					   false,
 					   _land_motor_lim ? radians(_param_fw_lnd_fl_pmin.get()) : radians(_param_fw_p_lim_min.get()),
-					   _land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND);
+					   true);
 
 		if (!_land_noreturn_vertical) {
 			// just started with the flaring phase
@@ -1636,7 +1678,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 		const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
 
 		tecs_update_pitch_throttle(now, altitude_desired,
-					   calculate_target_airspeed(airspeed_approach, ground_speed),
+					   get_cruise_airspeed_setpoint(now, airspeed_approach, ground_speed, dt, FW_SPEED_MODE::FW_SPEED_MODE_NORMAL),
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(_param_fw_p_lim_max.get()),
 					   _param_fw_thr_min.get(),
@@ -1691,7 +1733,7 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
 				   _param_fw_thr_cruise.get(),
 				   false,
 				   pitch_limit_min,
-				   tecs_status_s::TECS_MODE_NORMAL,
+				   false,
 				   height_rate_sp);
 
 	_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
@@ -1750,7 +1792,7 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
 				   _param_fw_thr_cruise.get(),
 				   false,
 				   pitch_limit_min,
-				   tecs_status_s::TECS_MODE_NORMAL,
+				   false,
 				   height_rate_sp);
 
 	/* heading control */
@@ -1988,21 +2030,25 @@ FixedwingPositionControl::Run()
 
 		case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
 				control_auto_fixed_bank_alt_hold(_local_pos.timestamp);
+				resetEcoDashAllowed();
 				break;
 			}
 
 		case FW_POSCTRL_MODE_AUTO_CLIMBRATE: {
 				control_auto_descend(_local_pos.timestamp);
+				resetEcoDashAllowed();
 				break;
 			}
 
 		case FW_POSCTRL_MODE_MANUAL_POSITION: {
 				control_manual_position(_local_pos.timestamp, curr_pos, ground_speed);
+				resetEcoDashAllowed();
 				break;
 			}
 
 		case FW_POSCTRL_MODE_MANUAL_ALTITUDE: {
 				control_manual_altitude(_local_pos.timestamp, curr_pos, ground_speed);
+				resetEcoDashAllowed();
 				break;
 			}
 
@@ -2013,6 +2059,8 @@ FixedwingPositionControl::Run()
 					reset_takeoff_state();
 				}
 
+				resetEcoDashAllowed();
+
 				_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
 
 				break;
@@ -2123,7 +2171,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 		float pitch_min_rad, float pitch_max_rad,
 		float throttle_min, float throttle_max, float throttle_cruise,
 		bool climbout_mode, float climbout_pitch_min_rad,
-		uint8_t mode, float hgt_rate_sp)
+		bool disable_underspeed_detection, float hgt_rate_sp,
+		bool eco_mode_enabled)
 {
 	const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, 0.01f, 0.05f);
 	_last_tecs_update = now;
@@ -2187,8 +2236,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 	}
 
 	/* No underspeed protection in landing mode */
-	_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND
-					      || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
+	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
 
 	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
 	bool in_air_alt_control = (!_landed &&
@@ -2225,9 +2273,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 				    throttle_min, throttle_max, throttle_cruise,
 				    pitch_min_rad - radians(_param_fw_psp_off.get()),
 				    pitch_max_rad - radians(_param_fw_psp_off.get()),
-				    _param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp);
-
-	tecs_status_publish();
+				    hgt_rate_sp, eco_mode_enabled);
 }
 
 void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp)

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -76,7 +76,6 @@
 #include <uORB/topics/position_controller_landing_status.h>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/tecs_status.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -89,6 +88,7 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/orbit_status.h>
+#include <uORB/topics/wind.h>
 #include <uORB/uORB.h>
 #include <vtol_att_control/vtol_type.h>
 
@@ -152,11 +152,11 @@ private:
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _wind_sub{ORB_ID(wind)};
 
 	uORB::Publication<vehicle_attitude_setpoint_s>		_attitude_sp_pub;
 	uORB::Publication<position_controller_status_s>		_pos_ctrl_status_pub{ORB_ID(position_controller_status)};			///< navigation capabilities publication
-	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};	///< landing status publication
-	uORB::Publication<tecs_status_s>			_tecs_status_pub{ORB_ID(tecs_status)};						///< TECS status publication
+	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};	///< landing status publication					///< TECS status publication
 	uORB::PublicationMulti<orbit_status_s>			_orbit_status_pub{ORB_ID(orbit_status)};
 
 	manual_control_setpoint_s	_manual_control_setpoint {};			///< r/c channel data
@@ -267,6 +267,24 @@ private:
 		FW_POSCTRL_MODE_OTHER
 	} _control_mode_current{FW_POSCTRL_MODE_OTHER};		///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
 
+	enum FW_SPEED_MODE {
+		FW_SPEED_MODE_NORMAL,
+		FW_SPEED_MODE_ECO,
+		FW_SPEED_MODE_DASH,
+	};
+
+	enum FW_SPEED_MODE_COMMANDED {
+		NORMAL,
+		ECO_CRUISE,
+		ECO_FULL,
+		DASH_CRUISE,
+		DASH_FULL
+	};
+
+	hrt_abstime _time_conditions_not_met{0};
+
+	float _last_airspeed_setpoint{NAN};
+
 	param_t _param_handle_airspeed_trans{PARAM_INVALID};
 	float _param_airspeed_trans{NAN};
 
@@ -284,7 +302,6 @@ private:
 
 	void		status_publish();
 	void		landing_status_publish();
-	void		tecs_status_publish();
 
 	void		abort_landing(bool abort);
 
@@ -325,15 +342,19 @@ private:
 	void		control_auto_fixed_bank_alt_hold(const hrt_abstime &now);
 	void		control_auto_descend(const hrt_abstime &now);
 
-	void		control_auto_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
-					      const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
-	void		control_auto_loiter(const hrt_abstime &now, 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);
+	void		control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
+					      const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
+					      const position_setpoint_s &pos_sp_curr, const FW_SPEED_MODE spd_mode);
+	void		control_auto_loiter(const hrt_abstime &now, const float dt, 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,
+					    const FW_SPEED_MODE spd_mode);
 	void		control_auto_takeoff(const hrt_abstime &now, const float dt,  const Vector2d &curr_pos,
 					     const Vector2f &ground_speed,
 					     const position_setpoint_s &pos_sp_prev,
 					     const position_setpoint_s &pos_sp_curr);
-	void		control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
+	void		control_auto_landing(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
+					     const Vector2f &ground_speed,
 					     const position_setpoint_s &pos_sp_prev,
 					     const position_setpoint_s &pos_sp_curr);
 	void		control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
@@ -343,14 +364,19 @@ private:
 	float		get_tecs_thrust();
 
 	float		get_demanded_airspeed();
-	float		calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed);
+	float		get_cruise_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed,
+			const Vector2f &ground_speed, float dt, const FW_SPEED_MODE spd_mode);
 
 	void		reset_takeoff_state(bool force = false);
 	void		reset_landing_state();
 	Vector2f 	get_nav_speed_2d(const Vector2f &ground_speed);
 	void		set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
 
-	void publishOrbitStatus(const position_setpoint_s pos_sp);
+	void 		publishOrbitStatus(const position_setpoint_s pos_sp);
+
+	FW_SPEED_MODE	getSpeedMode();
+	void		update_wind_mode();
+	void		resetEcoDashAllowed();
 
 	/*
 	 * Call TECS : a wrapper function to call the TECS implementation
@@ -359,7 +385,8 @@ private:
 					float pitch_min_rad, float pitch_max_rad,
 					float throttle_min, float throttle_max, float throttle_cruise,
 					bool climbout_mode, float climbout_pitch_min_rad,
-					uint8_t mode = tecs_status_s::TECS_MODE_NORMAL, float hgt_rate_sp = NAN);
+					bool disable_underspeed_detection = false, float hgt_rate_sp = NAN,
+					bool enable_eco_mode = false);
 
 	DEFINE_PARAMETERS(
 
@@ -435,10 +462,19 @@ private:
 
 		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
 
-		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min
+		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
 
+		//Speed mode params
+		(ParamFloat<px4::params::FW_WIND_ARSP_SC>) _param_fw_wind_arsp_sc,
+
+		(ParamInt<px4::params::FW_SPD_MODE_SET>) _param_fw_spd_mode_set,
+		(ParamFloat<px4::params::FW_SPDM_ALT_MIN>) _param_fw_alt_spdm_min,
+		(ParamFloat<px4::params::FW_SPDM_ALT_ER_U>) _param_fw_spdm_alt_er_u,
+		(ParamFloat<px4::params::FW_SPDM_ALT_ER_O>) _param_fw_spdm_alt_er_o,
+		(ParamFloat<px4::params::FW_T_ALT_TC_E>) _param_fw_t_alt_tc_eco,
+		(ParamFloat<px4::params::FW_T_SPDWEIGHT_E>) _param_fw_t_spdweight_eco,
+		(ParamFloat<px4::params::FW_T_CLMB_R_SP_E>) _param_fw_t_clmb_r_sp_eco
 	)
-
 };
 
 #endif // FIXEDWINGPOSITIONCONTROL_HPP_

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -854,3 +854,126 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f);
+
+/**
+ * Wind-based airspeed scaling factor in Eco
+ *
+ * Multiplying this factor with the current absolute wind estimate gives the airspeed offset
+ * added to the setpoint (which is FW_AIRSPD_MIN) in Eco mode (see FW_SPD_MODE_SET). This helps to make the
+ * system more robust against disturbances in high wind.
+ *
+ * setpoint = FW_AIRSPD_MIN + FW_LND_AIRSPD_SC * wind
+ *
+ * @unit norm
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.0f);
+
+/**
+ * FW Speed mode setting
+ *
+ * Setting
+ *
+ * @min 0
+ * @max 4
+ * @value 0 Normal
+ * @value 1 Eco cruise
+ * @value 2 Eco full
+ * @value 3 Dash cruise
+ * @value 4 Dash full
+ * @group FW TECS
+ */
+PARAM_DEFINE_INT32(FW_SPD_MODE_SET, 0);
+
+
+/**
+ * Max altitude undershoot for Eco/Dash
+ *
+ * Eco/Dash mode is disabled if the current altitude overshoots the setpoint by this value.
+ *
+ * @min 1.0
+ * @max 100.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_U, 10.f);
+
+/**
+ * Max altitude overshoot for Eco/Dash
+ *
+ * Eco/Dash mode is disabled if the current altitude undershoots the setpoint by this value.
+ *
+ * @min 1.0
+ * @max 100.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_O, 20.f);
+
+/**
+ * Minimum height above home for Eco/Dash
+ *
+ * Eco/Dash mode can be enabled if above this relative altitude to home.
+ *
+ * @min -200.0
+ * @max 200.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_SPDM_ALT_MIN, 50.f);
+
+/**
+ * Speed <--> Altitude priority in Eco mode
+ *
+ * Overrides FW_T_SPDWEIGHT in Eco mode.
+ *
+ * Set it close to 2 for looser altitude control.
+ * That helps increasing power efficiency by having a smoother throttle setpoint, plus
+ * it enables the vehicle to exploit favorable local uplift conditions to gain some altitude.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 1
+ * @increment 1.0
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT_E, 1.8f);
+
+/**
+ * Eco Mode: Altitude error time constant.
+ *
+ * Overrides FW_T_ALT_TC in Eco mode.
+ *
+ * Increase it for looser altitude control.
+ * That helps increasing power efficiency by having a smoother throttle setpoint, plus
+ * it enables the vehicle to exploit favorable local uplift conditions to gain some altitude.
+ *
+ * @min 2.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_ALT_TC_E, 10.0f);
+
+/**
+ * Eco target climbrate.
+ *
+ * Overrides FW_T_CLMB_R_SP in Eco mode.
+ *
+ * The rate at which the vehicle will climb in autonomous modes to achieve altitude setpoints in Eco mode.
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 15
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP_E, 2.0f);