Improve fixed wing trim throttle compensation & geneal refactoring

ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane

@@ -29,7 +29,7 @@ param set-default FW_SPOILERS_LND 0.4
 
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
-param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_TRIM 0.25
 
 param set-default FW_T_CLMB_MAX 8
 param set-default FW_T_SINK_MAX 2.7

ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer

@@ -27,7 +27,7 @@ param set-default FW_RR_P 0.3
 
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
-param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_TRIM 0.25
 
 param set-default FW_T_ALT_TC 2
 param set-default FW_T_CLMB_MAX 8

ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider

@@ -5,5 +5,5 @@
 
 . ${R}etc/init.d-posix/airframes/1030_plane
 
-param set-default FW_THR_CRUISE 0.0
+param set-default FW_THR_TRIM 0.0
 param set-default RWTO_TKOFF 0

ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol

@@ -57,7 +57,7 @@ param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
-param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_TRIM 0.25
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
 param set-default FW_T_CLMB_MAX 8

ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter

@@ -51,7 +51,7 @@ param set-default FW_PSP_OFF 2
 param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_P 0.2
-param set-default FW_THR_CRUISE 0.33
+param set-default FW_THR_TRIM 0.33
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
 param set-default FW_T_ALT_TC 2

ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor

@@ -62,7 +62,7 @@ param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
-param set-default FW_THR_CRUISE 0.38
+param set-default FW_THR_TRIM 0.38
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
 param set-default FW_T_CLMB_MAX 8

ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop

@@ -16,7 +16,7 @@ param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
-param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_TRIM 0.25
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
 param set-default FW_T_ALT_TC 2

ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1

@@ -41,7 +41,7 @@ param set-default FW_R_LIM 30
 param set-default FW_MAN_P_MAX 30.0
 param set-default FW_MAN_R_MAX 30.0
 
-param set-default FW_THR_CRUISE 0.8
+param set-default FW_THR_TRIM 0.8
 param set-default FW_THR_IDLE 0
 param set-default COM_DISARM_PRFLT 0
 

ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad

@@ -43,7 +43,7 @@ param set-default MPC_YAWRAUTO_MAX 40
 param set-default FW_PR_I 0.02
 param set-default FW_RR_FF 0.6
 param set-default FW_RR_I 0.01
-param set-default FW_THR_CRUISE 0.75
+param set-default FW_THR_TRIM 0.75
 
 param set-default VT_ARSP_BLEND 6
 param set-default VT_ARSP_TRANS 12

ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger

@@ -14,7 +14,7 @@
 . ${R}etc/init.d/rc.vtol_defaults
 
 
-param set-default FW_THR_CRUISE 65
+param set-default FW_THR_TRIM 65
 param set-default FW_RR_FF 0.6
 
 param set-default MIS_YAW_TMT 10

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -53,7 +53,7 @@ param set-default FW_T_CLMB_MAX 3
 param set-default FW_T_SINK_MAX 3
 param set-default FW_T_SINK_MIN 1
 param set-default FW_T_VERT_ACC 6
-param set-default FW_THR_CRUISE 0.70
+param set-default FW_THR_TRIM 0.70
 param set-default FW_THR_SLEW_MAX 1
 param set-default FW_MAN_P_MAX 30
 param set-default FW_P_LIM_MAX 15

ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark

@@ -39,7 +39,7 @@ param set-default FW_RLL_TO_YAW_FF 0.1
 param set-default FW_RR_P 0.08
 param set-default FW_R_LIM 45
 param set-default FW_R_RMAX 50
-param set-default FW_THR_CRUISE 0.65
+param set-default FW_THR_TRIM 0.65
 param set-default FW_THR_MIN 0.3
 param set-default FW_THR_SLEW_MAX 0.6
 param set-default FW_T_HRATE_FF 0

ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2

@@ -37,7 +37,7 @@ param set-default SENS_BOARD_ROT 8
 param set-default FW_AIRSPD_MAX 20
 param set-default FW_AIRSPD_MIN 7
 param set-default FW_AIRSPD_TRIM 13
-param set-default FW_THR_CRUISE 0.8
+param set-default FW_THR_TRIM 0.8
 
 param set-default FW_MAN_P_MAX 25
 param set-default FW_MAN_R_MAX 25

ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2

@@ -32,7 +32,7 @@ param set-default SENS_BOARD_ROT 4
 param set-default FW_AIRSPD_MAX 20
 param set-default FW_AIRSPD_MIN 7
 param set-default FW_AIRSPD_TRIM 13
-param set-default FW_THR_CRUISE 0.8
+param set-default FW_THR_TRIM 0.8
 
 param set-default FW_MAN_P_MAX 25
 param set-default FW_MAN_R_MAX 25

ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha

@@ -40,7 +40,7 @@ param set-default FW_RR_P 0.013
 param set-default FW_P_RMAX_NEG 70
 param set-default FW_P_RMAX_POS 70
 param set-default FW_R_RMAX 70
-param set-default FW_THR_CRUISE 0.55
+param set-default FW_THR_TRIM 0.55
 
 param set-default LNDFW_AIRSPD_MAX 6
 param set-default LNDFW_XYACC_MAX 4

msg/position_setpoint.msg

@@ -40,6 +40,7 @@ int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
 float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
 
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
-float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint)
+bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
+float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover
 
 bool disable_weather_vane   # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind

msg/tecs_status.msg

@@ -40,5 +40,6 @@ float32 pitch_integ
 
 float32 throttle_sp
 float32 pitch_sp_rad
+float32 throttle_trim	# estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions
 
 uint8 mode

src/lib/parameters/param_translation.cpp

@@ -237,5 +237,14 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2022-07-07: translate FW_THR_CRUISE->FW_THR_TRIM
+	{
+		if (strcmp("FW_THR_CRUISE", node->name) == 0) {
+			strcpy(node->name, "FW_THR_TRIM");
+			PX4_INFO("copying %s -> %s", "FW_THR_CRUISE", "FW_THR_TRIM");
+			return true;
+		}
+	}
+
 	return false;
 }

src/lib/tecs/TECS.cpp

@@ -116,9 +116,9 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
 	_TAS_max   = _equivalent_airspeed_max * EAS2TAS;
 	_TAS_min   = _equivalent_airspeed_min * EAS2TAS;
 
-	// If airspeed measurements are not being used, fix the airspeed estimate to the nominal cruise airspeed
+	// If airspeed measurements are not being used, fix the airspeed estimate to the nominal trim airspeed
 	if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) {
-		_EAS = _equivalent_airspeed_cruise;
+		_EAS = _equivalent_airspeed_trim;
 
 	} else {
 		_EAS = equivalent_airspeed;
@@ -170,7 +170,13 @@ void TECS::_update_speed_setpoint()
 	_TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
 
 	// calculate the demanded true airspeed rate of change based on first order response of true airspeed error
-	_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, velRateMin, velRateMax);
+	// if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints
+	if (airspeed_sensor_enabled()) {
+		_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, velRateMin, velRateMax);
+
+	} else {
+		_TAS_rate_setpoint = 0.0f;
+	}
 
 }
 
@@ -242,11 +248,11 @@ void TECS::_update_energy_estimates()
 	_SKE_rate = _tas_state * _tas_rate_filtered;// kinetic energy rate of change
 }
 
-void TECS::_update_throttle_setpoint(const float throttle_cruise)
+void TECS::_update_throttle_setpoint()
 {
 	// Calculate demanded rate of change of total energy, respecting vehicle limits.
 	// We will constrain the value below.
-	float STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint;
+	_STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint;
 
 	// Calculate the total energy rate error, applying a first order IIR filter
 	// to reduce the effect of accelerometer noise
@@ -264,24 +270,26 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise)
 		// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
 		// Assume induced drag scales linearly with normal load factor.
 		// The additional normal load factor is given by (1/cos(bank angle) - 1)
-		STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (_load_factor - 1.f);
+		_STE_rate_setpoint += _load_factor_correction * (_load_factor - 1.f);
 
-		STE_rate_setpoint = constrain(STE_rate_setpoint, _STE_rate_min, _STE_rate_max);
+		_STE_rate_setpoint = constrain(_STE_rate_setpoint, _STE_rate_min, _STE_rate_max);
 
-		// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
+		// Calculate a predicted throttle from the demanded rate of change of energy, using the trim throttle
 		// as the starting point. Assume:
 		// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
-		// Specific total energy rate = 0 at cruise throttle
+		// Specific total energy rate = 0 at trim throttle
 		// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
 		float throttle_predicted = 0.0f;
 
-		if (STE_rate_setpoint >= 0) {
-			// throttle is between cruise and maximum
-			throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - throttle_cruise);
+		if (_STE_rate_setpoint >= 0) {
+			// throttle is between trim and maximum
+			throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max -
+					     _throttle_trim);
 
 		} else {
-			// throttle is between cruise and minimum
-			throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - throttle_cruise);
+			// throttle is between trim and minimum
+			throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min -
+					     _throttle_trim);
 
 		}
 
@@ -482,7 +490,7 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat
 	}
 }
 
-void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout,
+void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altitude, float pitch_min_climbout,
 			      float EAS2TAS)
 {
 	if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initialized) {
@@ -494,7 +502,7 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
 		_tas_state = _EAS * EAS2TAS;
 		_throttle_integ_state =  0.0f;
 		_pitch_integ_state = 0.0f;
-		_last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);;
+		_last_throttle_setpoint = (_in_air ? throttle_trim : 0.0f);;
 		_last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max);
 		_pitch_setpoint_unc = _last_pitch_setpoint;
 		_TAS_setpoint_last = _EAS * EAS2TAS;
@@ -553,7 +561,8 @@ 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 throttle_min, float throttle_max, float throttle_trim,
+				 float pitch_limit_min, float pitch_limit_max,
 				 float target_climbrate, float target_sinkrate, float hgt_rate_sp)
 {
 	// Calculate the time since last update (seconds)
@@ -566,9 +575,10 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	_pitch_setpoint_max = pitch_limit_max;
 	_pitch_setpoint_min = pitch_limit_min;
 	_climbout_mode_active = climb_out_setpoint;
+	_throttle_trim = throttle_trim;
 
 	// Initialize selected states and variables as required
-	_initialize_states(pitch, throttle_cruise, baro_altitude, pitch_min_climbout, eas_to_tas);
+	_initialize_states(pitch, throttle_trim, baro_altitude, pitch_min_climbout, eas_to_tas);
 
 	// Don't run TECS control algorithms when not in flight
 	if (!_in_air) {
@@ -600,7 +610,7 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	_update_energy_estimates();
 
 	// Calculate the throttle demand
-	_update_throttle_setpoint(throttle_cruise);
+	_update_throttle_setpoint();
 
 	// Calculate the pitch demand
 	_update_pitch_setpoint();

src/lib/tecs/TECS.hpp

@@ -89,12 +89,13 @@ class TECS
 	 */
 	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 throttle_min, float throttle_setpoint_max, float throttle_trim,
 				   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 get_throttle_trim() { return _throttle_trim; }
 
 	void reset_state() { _states_initialized = false; }
 
@@ -120,7 +121,7 @@ class TECS
 
 	void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
 	void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
-	void set_equivalent_airspeed_cruise(float airspeed) { _equivalent_airspeed_cruise = airspeed; }
+	void set_equivalent_airspeed_trim(float airspeed) { _equivalent_airspeed_trim = airspeed; }
 
 	void set_pitch_damping(float damping) { _pitch_damping_gain = damping; }
 	void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; }
@@ -140,7 +141,6 @@ class TECS
 
 	void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; }
 
-
 	// TECS status
 	uint64_t timestamp() { return _pitch_update_timestamp; }
 	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
@@ -175,7 +175,7 @@ class TECS
 
 	float STE_rate() { return _SPE_rate + _SKE_rate; }
 
-	float STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; }
+	float STE_rate_setpoint() { return _STE_rate_setpoint; }
 
 	float SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; }
 
@@ -223,15 +223,15 @@ class TECS
 	float _integrator_gain_throttle{0.0f};				///< integrator gain used by the throttle demand calculation
 	float _integrator_gain_pitch{0.0f};				///< integrator gain used by the pitch demand calculation
 	float _vert_accel_limit{0.0f};					///< magnitude of the maximum vertical acceleration allowed (m/sec**2)
-	float _load_factor{0.0f};					///< additional normal load factor
+	float _load_factor{1.0f};					///< additional normal load factor
 	float _load_factor_correction{0.0f};				///< gain from normal load factor increase to total energy rate demand (m**2/sec**3)
 	float _pitch_speed_weight{1.0f};				///< speed control weighting used by pitch demand calculation
 	float _height_error_gain{0.2f};					///< height error inverse time constant [1/s]
 	float _height_setpoint_gain_ff{0.0f};				///< gain from height demand derivative to demanded climb rate
 	float _airspeed_error_gain{0.1f};				///< airspeed error inverse time constant [1/s]
 	float _equivalent_airspeed_min{3.0f};				///< equivalent airspeed demand lower limit (m/sec)
 	float _equivalent_airspeed_max{30.0f};				///< equivalent airspeed demand upper limit (m/sec)
-	float _equivalent_airspeed_cruise{15.0f};			///< equivalent cruise airspeed for airspeed less mode (m/sec)
+	float _equivalent_airspeed_trim{15.0f};			///< equivalent cruise airspeed for airspeed less mode (m/sec)
 	float _throttle_slewrate{0.0f};					///< throttle demand slew rate limit (1/sec)
 	float _STE_rate_time_const{0.1f};				///< filter time constant for specific total energy rate (damping path) (s)
 	float _speed_derivative_time_const{0.01f};			///< speed derivative filter time constant (s)
@@ -272,6 +272,7 @@ class TECS
 	float _STE_rate_min{0.0f};					///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3)
 	float _throttle_setpoint_max{0.0f};				///< normalised throttle upper limit
 	float _throttle_setpoint_min{0.0f};				///< normalised throttle lower limit
+	float _throttle_trim{0.0f};					///< throttle required to fly level at _EAS_setpoint, compensated for air density and vehicle weight
 	float _pitch_setpoint_max{0.5f};				///< pitch demand upper limit (rad)
 	float _pitch_setpoint_min{-0.5f};				///< pitch demand lower limit (rad)
 
@@ -280,6 +281,7 @@ class TECS
 	float _SKE_setpoint{0.0f};					///< specific kinetic energy demand (m**2/sec**2)
 	float _SPE_rate_setpoint{0.0f};					///< specific potential energy rate demand (m**2/sec**3)
 	float _SKE_rate_setpoint{0.0f};					///< specific kinetic energy rate demand (m**2/sec**3)
+	float _STE_rate_setpoint{0.0f};					///< specific total energy rate demand (m**s/sec**3)
 	float _SPE_estimate{0.0f};					///< specific potential energy estimate (m**2/sec**2)
 	float _SKE_estimate{0.0f};					///< specific kinetic energy estimate (m**2/sec**2)
 	float _SPE_rate{0.0f};						///< specific potential energy rate estimate (m**2/sec**3)
@@ -337,7 +339,7 @@ class TECS
 	/**
 	 * Update throttle setpoint
 	 */
-	void _update_throttle_setpoint(float throttle_cruise);
+	void _update_throttle_setpoint();
 
 	/**
 	 * Detect an uncommanded descent