TECS: improve initialization

-remove external init, and instead always (but only) init when dt is too large
-init the controller params correctly

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

src/lib/tecs/TECS.cpp

@@ -628,8 +628,24 @@ void TECSControl::resetIntegrals()
 	_throttle_integ_state = 0.0f;
 }
 
+void TECS::initControlParams(float target_climbrate, float target_sinkrate, float eas_to_tas, float pitch_limit_max,
+			     float pitch_limit_min, float throttle_min, float throttle_setpoint_max, float throttle_trim)
+{
+	// Update parameters from input
+	// Reference model
+	_reference_param.target_climbrate = target_climbrate;
+	_reference_param.target_sinkrate = target_sinkrate;
+	// Control
+	_control_param.tas_min = eas_to_tas * _equivalent_airspeed_min;
+	_control_param.pitch_max = pitch_limit_max;
+	_control_param.pitch_min = pitch_limit_min;
+	_control_param.throttle_trim = throttle_trim;
+	_control_param.throttle_max = throttle_setpoint_max;
+	_control_param.throttle_min = throttle_min;
+}
+
 void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed,
-		      const float eas_to_tas)
+		      float eas_to_tas)
 {
 	// Init subclasses
 	TECSAltitudeReferenceModel::AltitudeReferenceState current_state{.alt = altitude,
@@ -649,15 +665,6 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 						.tas_rate = 0.0f};
 
 	_control.initialize(control_setpoint, control_input, _control_param, _control_flag);
-
-	_debug_status.control = _control.getDebugOutput();
-	_debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed;
-	_debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate;
-	_debug_status.altitude_reference = _altitude_reference_model.getAltitudeReference().alt;
-	_debug_status.height_rate_reference = _altitude_reference_model.getAltitudeReference().alt_rate;
-	_debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect();
-
-	_update_timestamp = hrt_absolute_time();
 }
 
 void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
@@ -670,17 +677,8 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	const hrt_abstime now(hrt_absolute_time());
 	const float dt = static_cast<float>((now - _update_timestamp)) / 1_s;
 
-	// Update parameters from input
+	initControlParams(target_climbrate, target_sinkrate, eas_to_tas, pitch_limit_max, pitch_limit_min, throttle_min,
-	// Reference model
+			  throttle_setpoint_max, throttle_trim);
-	_reference_param.target_climbrate = target_climbrate;
-	_reference_param.target_sinkrate = target_sinkrate;
-	// Control
-	_control_param.tas_min = eas_to_tas * _equivalent_airspeed_min;
-	_control_param.pitch_max = pitch_limit_max;
-	_control_param.pitch_min = pitch_limit_min;
-	_control_param.throttle_trim = throttle_trim;
-	_control_param.throttle_max = throttle_setpoint_max;
-	_control_param.throttle_min = throttle_min;
 
 	if (dt < DT_MIN) {
 		// Update intervall too small, do not update. Assume constant states/output in this case.
@@ -697,7 +695,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 				.equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas};
 
 		_airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled);
-		const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
 
 		// Update Reference model submodule
 		const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
@@ -712,20 +709,19 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 
 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,
-							.tas = eas_to_tas * eas.speed,
+							.tas = eas_to_tas * _airspeed_filter.getState().speed,
-							.tas_rate = eas_to_tas * eas.speed_rate};
+							.tas_rate = eas_to_tas * _airspeed_filter.getState().speed_rate};
 
 		_control.update(dt, control_setpoint, control_input, _control_param, _control_flag);
-
-		// Update time stamps
-		_update_timestamp = now;
-
-		_debug_status.control = _control.getDebugOutput();
-		_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
-		_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
-		_debug_status.altitude_reference = control_setpoint.altitude_reference.alt;
-		_debug_status.height_rate_reference = control_setpoint.altitude_reference.alt_rate;
-		_debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect();
 	}
+
+	_debug_status.control = _control.getDebugOutput();
+	_debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed;
+	_debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate;
+	_debug_status.altitude_reference = _altitude_reference_model.getAltitudeReference().alt;
+	_debug_status.height_rate_reference = _altitude_reference_model.getAltitudeReference().alt_rate;
+	_debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect();
+
+	_update_timestamp = now;
 }
 

src/lib/tecs/TECS.hpp

@@ -88,8 +88,11 @@ public:
 	 * @brief Initialize filter
 	 *
 	 * @param[in] equivalent_airspeed is the equivalent airspeed in [m/s].
+	 * @param[in] equivalent_airspeed_trim is the equivalent airspeed trim (vehicle setting) in [m/s].
+	 * @param[in] airspeed_sensor_available boolean if the airspeed sensor is available.
 	 */
-	void initialize(float equivalent_airspeed);
+	void initialize(float equivalent_airspeed, const float equivalent_airspeed_trim,
+			const bool airspeed_sensor_available);
 
 	/**
 	 * @brief Update filter
@@ -579,12 +582,6 @@ public:
 		    float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate,
 		    float target_sinkrate, float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
 
-	/**
-	 * @brief Initialize the control loop
-	 *
-	 */
-	void initialize(float altitude, float altitude_rate, float equivalent_airspeed, float eas_to_tas);
-
 	void resetIntegrals()
 	{
 		_control.resetIntegrals();
@@ -648,6 +645,20 @@ public:
 	float get_underspeed_ratio() { return _control.getRatioUndersped(); }
 
 private:
+	/**
+	 * @brief Initialize the control parameters
+	 *
+	 */
+	void initControlParams(float target_climbrate, float target_sinkrate, float eas_to_tas, float pitch_limit_max,
+			       float pitch_limit_min, float throttle_min, float throttle_setpoint_max, float throttle_trim);
+
+	/**
+	 * @brief Initialize the control loop
+	 *
+	 */
+	void initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed,
+			float eas_to_tas);
+
 	TECSControl 			_control;			///< Control submodule.
 	TECSAirspeedFilter 		_airspeed_filter;		///< Airspeed filter submodule.
 	TECSAltitudeReferenceModel 	_altitude_reference_model;	///< Setpoint reference model submodule.

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -777,21 +777,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 void
 FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 {
-	/* save time when airplane is in air */
-	if (!_was_in_air && !_landed) {
-		_was_in_air = true;
-		_time_went_in_air = now;
-
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
-	}
-
 	/* reset flag when airplane landed */
 	if (_landed) {
-		_was_in_air = false;
 		_completed_manual_takeoff = false;
-
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
 	}
+
 }
 
 void
@@ -2420,9 +2410,6 @@ FixedwingPositionControl::Run()
 
 		case FW_POSCTRL_MODE_OTHER: {
 				_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
-
-				_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
-
 				break;
 			}
 
@@ -2569,32 +2556,16 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	}
 
 	if (!_tecs_is_running) {
-		// next time we run TECS we should reinitialize states
-		_reinitialize_tecs = true;
 		return;
 	}
 
-	// We need an altitude lock to calculate the TECS control
-	if (_local_pos.timestamp == 0) {
-		_reinitialize_tecs = true;
-	}
-
-	if (_reinitialize_tecs) {
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
-		_reinitialize_tecs = false;
-	}
-
-	/* No underspeed protection in landing mode */
-	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
-
-	if (_landed) {
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
-	}
-
 	/* update TECS vehicle state estimates */
 	const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min,
 						throttle_max, airspeed_sp, _air_density);
 
+	/* No underspeed protection in landing mode */
+	_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;

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -270,12 +270,6 @@ private:
 
 	bool _landed{true};
 
-	// indicates whether the plane was in the air in the previous interation
-	bool _was_in_air{false};
-
-	// [us] time at which the plane went in the air
-	hrt_abstime _time_went_in_air{0};
-
 	// MANUAL MODES
 
 	// indicates whether we have completed a manual takeoff in a position control mode
@@ -388,10 +382,7 @@ private:
 	// total energy control system - airspeed / altitude control
 	TECS _tecs;
 
-	bool _reinitialize_tecs{true};
 	bool _tecs_is_running{false};
-	hrt_abstime _time_last_tecs_update{0}; // [us]
-
 	// VTOL / TRANSITION
 
 	float _airspeed_after_transition{0.0f};