remove in air vs landed knowledge from TECS

- create integral and trajectory generator reset methods
- always run TECS unless in rotary-wing mode (or in transition)
- constantly reset TECS integrals and trajectory generators when landed

src/lib/tecs/TECS.cpp

@@ -56,7 +56,7 @@ static constexpr float DT_MAX = 1.0f;	///< max value of _dt allowed before a fil
  * which is used by the airspeed complimentary filter.
  */
 void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward,
-		bool altitude_lock, bool in_air, float altitude, float vz)
+		bool altitude_lock, float altitude, float vz)
 {
 	// calculate the time lapsed since the last update
 	uint64_t now = hrt_absolute_time();
@@ -69,7 +69,7 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float
 		reset_altitude = true;
 	}
 
-	if (!altitude_lock || !in_air) {
+	if (!altitude_lock) {
 		reset_altitude = true;
 	}
 
@@ -80,8 +80,6 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float
 	_state_update_timestamp = now;
 	_EAS = equivalent_airspeed;
 
-	_in_air = in_air;
-
 	// Set the velocity and position state to the the INS data
 	_vert_vel_state = -vz;
 	_vert_pos_state = altitude;
@@ -97,11 +95,6 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float
 		_tas_rate_raw = 0.0f;
 		_tas_rate_filtered = 0.0f;
 	}
-
-	if (!_in_air) {
-		_states_initialized = false;
-	}
-
 }
 
 void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equivalent_airspeed, float EAS2TAS)
@@ -124,8 +117,8 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
 		_EAS = equivalent_airspeed;
 	}
 
-	// If first time through or not flying, reset airspeed states
+	// If first time through reset airspeed states
-	if (_speed_update_timestamp == 0 || !_in_air) {
+	if (_speed_update_timestamp == 0) {
 		_tas_rate_state = 0.0f;
 		_tas_state = (_EAS * EAS2TAS);
 	}
@@ -493,16 +486,14 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat
 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) {
+	if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_states_initialized) {
 		// On first time through or when not using TECS of if there has been a large time slip,
 		// states must be reset to allow filters to a clean start
 		_vert_vel_state = 0.0f;
 		_vert_pos_state = baro_altitude;
 		_tas_rate_state = 0.0f;
 		_tas_state = _EAS * EAS2TAS;
-		_throttle_integ_state =  0.0f;
+		_last_throttle_setpoint = throttle_trim;
-		_pitch_integ_state = 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;
@@ -512,13 +503,13 @@ void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altit
 		_STE_rate_error = 0.0f;
 		_hgt_setpoint = baro_altitude;
 
+		resetIntegrals();
+
 		if (_dt > DT_MAX || _dt < DT_MIN) {
 			_dt = DT_DEFAULT;
 		}
 
-		_alt_control_traj_generator.reset(0, 0, baro_altitude);
+		resetTrajectoryGenerators(baro_altitude);
-		_velocity_control_traj_generator.reset(0.0f, 0.0f, baro_altitude);
-
 
 	} else if (_climbout_mode_active) {
 		// During climbout use the lower pitch angle limit specified by the
@@ -580,11 +571,6 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	// Initialize selected states and variables as required
 	_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) {
-		return;
-	}
-
 	_updateTrajectoryGenerationConstraints();
 
 	// Update the true airspeed state estimate

src/lib/tecs/TECS.hpp

@@ -81,7 +81,6 @@ public:
 	 * Must be called at 50Hz or greater
 	 */
 	void update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, bool altitude_lock,
-					    bool in_air,
 					    float altitude, float vz);
 
 	/**
@@ -99,6 +98,23 @@ public:
 
 	void reset_state() { _states_initialized = false; }
 
+	void resetIntegrals()
+	{
+		_throttle_integ_state =  0.0f;
+		_pitch_integ_state = 0.0f;
+	}
+
+	/**
+	 * @brief Resets the altitude and height rate control trajectory generators to the input altitude
+	 *
+	 * @param altitude Vehicle altitude (AMSL) [m]
+	 */
+	void resetTrajectoryGenerators(const float altitude)
+	{
+		_alt_control_traj_generator.reset(0, 0, altitude);
+		_velocity_control_traj_generator.reset(0.0f, 0.0f, altitude);
+	}
+
 	enum ECL_TECS_MODE {
 		ECL_TECS_MODE_NORMAL = 0,
 		ECL_TECS_MODE_UNDERSPEED,
@@ -307,8 +323,7 @@ private:
 	bool _uncommanded_descent_recovery{false};			///< true when a continuous descent caused by an unachievable airspeed demand has been detected
 	bool _climbout_mode_active{false};				///< true when in climbout mode
 	bool _airspeed_enabled{false};					///< true when airspeed use has been enabled
-	bool _states_initialized{false};					///< true when TECS states have been iniitalized
+	bool _states_initialized{false};				///< true when TECS states have been iniitalized
-	bool _in_air{false};						///< true when the vehicle is flying
 
 	/**
 	 * Update the airspeed internal state using a second order complementary filter

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -855,11 +855,17 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 		_was_in_air = true;
 		_time_went_in_air = now;
 		_takeoff_ground_alt = _current_altitude; // XXX: is this really a good idea?
+
+		_tecs.resetIntegrals();
+		_tecs.resetTrajectoryGenerators(_current_altitude);
 	}
 
 	/* reset flag when airplane landed */
 	if (_landed) {
 		_was_in_air = false;
+
+		_tecs.resetIntegrals();
+		_tecs.resetTrajectoryGenerators(_current_altitude);
 	}
 }
 
@@ -2176,7 +2182,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 float
 FixedwingPositionControl::get_tecs_pitch()
 {
-	if (_is_tecs_running) {
+	if (_tecs_is_running) {
 		return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get());
 	}
 
@@ -2187,7 +2193,7 @@ FixedwingPositionControl::get_tecs_pitch()
 float
 FixedwingPositionControl::get_tecs_thrust()
 {
-	if (_is_tecs_running) {
+	if (_tecs_is_running) {
 		return min(_tecs.get_throttle_setpoint(), 1.f);
 	}
 
@@ -2567,14 +2573,13 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		bool climbout_mode, float climbout_pitch_min_rad,
 		bool disable_underspeed_detection, float hgt_rate_sp)
 {
-	// do not run TECS if we are not in air
+	_tecs_is_running = true;
-	bool run_tecs = !_landed;
 
 	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
 	// (it should also not run during VTOL blending because airspeed is too low still)
 	if (_vehicle_status.is_vtol) {
 		if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) {
-			run_tecs = false;
+			_tecs_is_running = false;
 		}
 
 		if (_vehicle_status.in_transition_mode) {
@@ -2607,9 +2612,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		}
 	}
 
-	_is_tecs_running = run_tecs;
+	if (!_tecs_is_running) {
-
-	if (!run_tecs) {
 		// next time we run TECS we should reinitialize states
 		_reinitialize_tecs = true;
 		return;
@@ -2623,16 +2626,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	/* No underspeed protection in landing mode */
 	_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 */
+	if (_landed) {
-	bool in_air_alt_control = (!_landed &&
+		_tecs.resetIntegrals();
-				   (_control_mode.flag_control_auto_enabled ||
+		_tecs.resetTrajectoryGenerators(_current_altitude);
-				    _control_mode.flag_control_velocity_enabled ||
+	}
-				    _control_mode.flag_control_altitude_enabled ||
-				    _control_mode.flag_control_climb_rate_enabled));
 
 	/* update TECS vehicle state estimates */
-	_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control,
+	_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), _current_altitude,
-					     _current_altitude, _local_pos.vz);
+					     _local_pos.vz);
 
 	const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min,
 					 throttle_max);

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -302,7 +302,7 @@ private:
 	matrix::Vector3f _body_velocity{};
 
 	bool _reinitialize_tecs{true};
-	bool _is_tecs_running{false};
+	bool _tecs_is_running{false};
 
 	hrt_abstime _time_last_tecs_update{0}; // [us]