TECS: Replaced old tecs by cleaned up version.

src/modules/fw_pos_control_l1/CMakeLists.txt

@@ -47,6 +47,5 @@ px4_add_module(
 		runway_takeoff
 		SlewRate
 		tecs
-		tecsnew
 		motion_planning
 	)

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -129,7 +129,6 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get());
 	_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get());
 	_tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get());
-	_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
 	_tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
 	_tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());
 	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
@@ -138,34 +137,13 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get());
 	_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
 	_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
-	_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
-	_tecs.set_heightrate_ff(_param_fw_t_hrate_ff.get());
+	_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.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_speed_derivative_time_constant(_param_tas_rate_time_const.get());
 	_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
 
-	_tecsnew.set_max_climb_rate(_param_fw_t_clmb_max.get());
-	_tecsnew.set_max_sink_rate(_param_fw_t_sink_max.get());
-	_tecsnew.set_speed_weight(_param_fw_t_spdweight.get());
-	_tecsnew.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get());
-	_tecsnew.set_equivalent_airspeed_min(_param_fw_airspd_min.get());
-	_tecsnew.set_equivalent_airspeed_max(_param_fw_airspd_max.get());
-	_tecsnew.set_throttle_damp(_param_fw_t_thr_damp.get());
-	_tecsnew.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());
-	_tecsnew.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
-	_tecsnew.set_throttle_slewrate(_param_fw_thr_slew_max.get());
-	_tecsnew.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
-	_tecsnew.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get());
-	_tecsnew.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
-	_tecsnew.set_pitch_damping(_param_fw_t_ptch_damp.get());
-	_tecsnew.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
-	_tecsnew.set_altitude_rate_ff(_param_fw_t_hrate_ff.get());
-	_tecsnew.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
-	_tecsnew.set_ste_rate_time_const(_param_ste_rate_time_const.get());
-	_tecsnew.set_speed_derivative_time_constant(_param_tas_rate_time_const.get());
-	_tecsnew.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
-
 	int check_ret = PX4_OK;
 
 	// sanity check parameters
@@ -306,7 +284,6 @@ FixedwingPositionControl::airspeed_poll()
 	// update TECS if validity changed
 	if (airspeed_valid != _airspeed_valid) {
 		_tecs.enable_airspeed(airspeed_valid);
-		_tecsnew.enable_airspeed(airspeed_valid);
 		_airspeed_valid = airspeed_valid;
 	}
 }
@@ -391,7 +368,6 @@ FixedwingPositionControl::vehicle_attitude_poll()
 		// load factor due to banking
 		const float load_factor = 1.f / cosf(euler_angles(0));
 		_tecs.set_load_factor(load_factor);
-		_tecsnew.set_load_factor(load_factor);
 	}
 }
 
@@ -485,6 +461,8 @@ FixedwingPositionControl::tecs_status_publish()
 {
 	tecs_status_s t{};
 
+	TECS::DebugOutput debug_output{_tecs.getStatus()};
+
 	switch (_tecs.tecs_mode()) {
 	case TECS::ECL_TECS_MODE_NORMAL:
 		t.mode = tecs_status_s::TECS_MODE_NORMAL;
@@ -503,43 +481,25 @@ FixedwingPositionControl::tecs_status_publish()
 		break;
 	}
 
-	t.altitude_sp = _tecs.hgt_setpoint();
-	t.altitude_filtered = _tecs.vert_pos_state();
+	t.altitude_filtered = debug_output.altitude_sp;
 
-	t.true_airspeed_sp = _tecs.TAS_setpoint_adj();
-	t.true_airspeed_filtered = _tecs.tas_state();
+	t.true_airspeed_filtered = debug_output.true_airspeed_filtered;
 
-	t.height_rate_setpoint = _tecs.hgt_rate_setpoint();
-	t.height_rate = _tecs.vert_vel_state();
+	t.height_rate_setpoint = debug_output.altitude_rate_setpoint;
+	t.height_rate = debug_output.altitude_rate;
 
-	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.true_airspeed_derivative_sp = debug_output.true_airspeed_derivative_control;
+	t.true_airspeed_derivative = debug_output.true_airspeed_derivative;
 
-	t.total_energy_error = _tecs.STE_error();
-	t.total_energy_rate_error = _tecs.STE_rate_error();
+	t.total_energy_rate_error = debug_output.total_energy_rate_error;
 
-	t.energy_distribution_error = _tecs.SEB_error();
-	t.energy_distribution_rate_error = _tecs.SEB_rate_error();
+	t.energy_distribution_rate_error = debug_output.energy_balance_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.total_energy_rate_sp = debug_output.total_energy_rate_sp;
+	t.total_energy_balance_rate_sp = debug_output.energy_balance_rate_sp;
 
 	t.throttle_sp = _tecs.get_throttle_setpoint();
 	t.pitch_sp_rad = _tecs.get_pitch_setpoint();
-	t.throttle_trim = _tecs.get_throttle_trim();
 
 	t.timestamp = hrt_absolute_time();
 
@@ -866,10 +826,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 		_was_in_air = true;
 		_time_went_in_air = now;
 
-		_tecs.resetIntegrals();
-		_tecs.resetTrajectoryGenerators(_current_altitude);
-
-		_tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
 	}
 
 	/* reset flag when airplane landed */
@@ -877,9 +834,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 		_was_in_air = false;
 		_completed_manual_takeoff = false;
 
-		_tecs.resetIntegrals();
-		_tecs.resetTrajectoryGenerators(_current_altitude);
-		_tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
 	}
 }
 
@@ -1124,7 +1079,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 	if (pos_sp_curr.gliding_enabled) {
 		/* enable gliding with this waypoint */
 		_tecs.set_speed_weight(2.0f);
-		_tecsnew.set_speed_weight(2.0f);
 		tecs_fw_thr_min = 0.0;
 		tecs_fw_thr_max = 0.0;
 
@@ -1227,7 +1181,6 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 	if (pos_sp_curr.gliding_enabled) {
 		/* enable gliding with this waypoint */
 		_tecs.set_speed_weight(2.0f);
-		_tecsnew.set_speed_weight(2.0f);
 		tecs_fw_thr_min = 0.0;
 		tecs_fw_thr_max = 0.0;
 
@@ -1315,7 +1268,6 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 	if (pos_sp_curr.gliding_enabled) {
 		/* enable gliding with this waypoint */
 		_tecs.set_speed_weight(2.0f);
-		_tecsnew.set_speed_weight(2.0f);
 		tecs_fw_thr_min = 0.0;
 		tecs_fw_thr_max = 0.0;
 
@@ -1338,8 +1290,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 		// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
 		// landing airspeed and potentially tighter altitude control) already such that we don't
 		// have to do this switch (which can cause significant altitude errors) close to the ground.
-		_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
-		_tecsnew.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
+		_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
 		airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : _param_fw_airspd_min.get();
 		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
 		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;
@@ -1385,8 +1336,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 			_att_sp.roll_body = 0.0f;
 		}
 
-		_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
-		_tecsnew.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
+		_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
 	}
 
 	tecs_update_pitch_throttle(control_interval,
@@ -1522,7 +1472,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 			// throttle is open loop anyway during ground roll, no need to wind up the integrator
 			_tecs.resetIntegrals();
-			_tecsnew.resetIntegrals();
 		}
 
 		tecs_update_pitch_throttle(control_interval,
@@ -1538,7 +1487,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 					   _param_fw_t_clmb_max.get());
 
 		_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
-		_tecsnew.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
 
 		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
 		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust());
@@ -1661,8 +1609,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
 	// Enable tighter altitude control for landings
-	_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
-	_tecsnew.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
+	_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
 
 	const Vector2f local_position{_local_pos.x, _local_pos.y};
 	Vector2f local_land_point = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
@@ -1680,7 +1627,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 	if (airspeed_land < _param_fw_airspd_min.get()) {
 		// adjust underspeed detection bounds for landing airspeed
 		_tecs.set_equivalent_airspeed_min(airspeed_land);
-		_tecsnew.set_equivalent_airspeed_min(airspeed_land);
 		adjusted_min_airspeed = airspeed_land;
 	}
 
@@ -1726,7 +1672,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 		if (!_flare_states.flaring) {
 			_flare_states.flaring = true;
 			_flare_states.start_time = now;
-			_flare_states.initial_height_rate_setpoint = _tecs.hgt_rate_setpoint();
+			_flare_states.initial_height_rate_setpoint = -_local_pos.vz;
 			_flare_states.initial_throttle_setpoint = _att_sp.thrust_body[0];
 			events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info,
 				     "Landing, flaring");
@@ -1904,7 +1850,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 	}
 
 	_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
-	_tecsnew.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
 
 	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
 
@@ -2156,7 +2101,7 @@ FixedwingPositionControl::Run()
 		if (_control_mode.flag_control_manual_enabled) {
 			if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _alt_reset_counter) {
 				// make TECS accept step in altitude and demanded altitude
-				_tecs.handle_alt_step(-_local_pos.delta_z, _current_altitude);
+				_tecs.handle_alt_step(_current_altitude, -_local_pos.vz);
 			}
 
 			// adjust navigation waypoints in position control mode
@@ -2303,9 +2248,7 @@ FixedwingPositionControl::Run()
 
 		// restore nominal 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());
-		_tecsnew.set_speed_weight(_param_fw_t_spdweight.get());
-		_tecsnew.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
+		_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
 
 		// restore lateral-directional guidance parameters (changed in takeoff mode)
 		_npfg.setPeriod(_param_npfg_period.get());
@@ -2371,7 +2314,7 @@ FixedwingPositionControl::Run()
 				_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 				_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 
-				_tecs.reset_state();
+				_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
 
 				break;
 			}
@@ -2548,14 +2491,12 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	}
 
 	// We need an altitude lock to calculate the TECS control
-	if (!(_local_pos.timestamp > 0))
-	{
+	if (!(_local_pos.timestamp > 0)) {
 		_reinitialize_tecs = true;
 	}
 
 	if (_reinitialize_tecs) {
-		_tecs.reset_state();
-		_tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
 		_reinitialize_tecs = false;
 	}
 
@@ -2563,82 +2504,31 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
 
 	if (_landed) {
-		_tecs.resetIntegrals();
-		_tecs.resetTrajectoryGenerators(_current_altitude);
-		_tecsnew.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
 	}
 
 	/* update TECS vehicle state estimates */
-	_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), _current_altitude,
-					     _local_pos.vz);
-
 	const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min,
 					 throttle_max);
 
-	_tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()),
-				    _current_altitude,
-				    alt_sp,
-				    airspeed_sp,
-				    _airspeed,
-				    _eas2tas,
-				    climbout_mode,
-				    climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
-				    throttle_min,
-				    throttle_max,
-				    throttle_trim_comp,
-				    pitch_min_rad - radians(_param_fw_psp_off.get()),
-				    pitch_max_rad - radians(_param_fw_psp_off.get()),
-				    desired_max_climbrate,
-				    desired_max_sinkrate,
-				    hgt_rate_sp);
-
-	_tecsnew.update(_pitch - radians(_param_fw_psp_off.get()),
-			_current_altitude,
-			alt_sp,
-			airspeed_sp,
-			_airspeed,
-			_eas2tas,
-			climbout_mode,
-			climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
-			throttle_min,
-			throttle_max,
-			throttle_trim_comp,
-			pitch_min_rad - radians(_param_fw_psp_off.get()),
-			pitch_max_rad - radians(_param_fw_psp_off.get()),
-			desired_max_climbrate,
-			desired_max_sinkrate,
-			_body_acceleration(0),
-			-_local_pos.vz,
-			hgt_rate_sp);
-
-	newTECS::TECS::DebugOutput new_status{_tecsnew.getStatus()};
-
-	printf("Compare Airspeed Filter: \n ");
-	printf("\t\t raw\t old \t new \n");
-	printf("Airspeed: \t %.2f \t %.2f \t %.2f \n",(double)(_airspeed*_eas2tas), (double)_tecs.tas_state(), (double)new_status.true_airspeed_filtered);
-	printf("Airspeed_deriv:\t %.2f \t %.2f \t %.2f \n", (double)_body_acceleration(0), (double)_tecs.speed_derivative(), (double)new_status.true_airspeed_derivative);
-
-	printf("Compare Altitude reference filter: \n ");
-	printf("\t\t\t raw\t\t old \t\t new \n");
-	printf("Altitude Ref:\t\t %.2f \t %.2f \t %.2f \n",(double)alt_sp, (double)_tecs.hgt_setpoint(), (double)new_status.altitude_sp);
-	printf("Alt rate from Alt:\t %.2f \t\t %.2f \t\t %.2f \n",0.0, (double)_tecs.hgt_rate_from_hgt_setpoint(), (double)new_status.altitude_rate);
-	printf("Altitude Rate Ref:\t %.2f \t\t %.2f \t\t %.2f \n",(double)hgt_rate_sp, (double)_tecs.smooth_hgt_rate_setpoint(), (double)new_status.altitude_rate_setpoint);
-
-	printf("Compare Tecs control: \n ");
-	printf("\t\t old \t\t new \n");
-	printf("alt_rate_ref:\t %.2f \t %.2f \n", (double)_tecs.hgt_rate_setpoint(), (double)new_status.altitude_rate_control);
-	printf("speed_rate_ref:\t %.2f \t %.2f \n", (double)_tecs.TAS_rate_setpoint(), (double)new_status.true_airspeed_derivative_control);
-	printf("ste_rate_error:\t %.2f \t %.2f \n", (double)_tecs.STE_rate_error(), (double)new_status.total_energy_rate_error);
-	printf("ste_rate_sp:\t %.2f \t %.2f \n", (double)_tecs.STE_rate_setpoint(), (double)new_status.total_energy_rate_sp);
-	printf("seb_rate_error:\t %.2f \t %.2f \n", (double)_tecs.SEB_rate_error(), (double)new_status.energy_balance_rate_error);
-	printf("seb_rate_sp:\t %.2f \t %.2f \n", (double)_tecs.SEB_rate_setpoint(), (double)new_status.energy_balance_rate_sp);
-
-	printf("Compare Tecs output: \n ");
-	printf("\t\t old \t\t new \n");
-	printf("pitch control:\t %.2f \t %.2f \n", (double)_tecs.get_pitch_setpoint(), (double)_tecsnew.get_pitch_setpoint());
-	printf("throttle control:\t %.2f \t %.2f \n", (double)_tecs.get_throttle_setpoint(), (double)_tecsnew.get_throttle_setpoint());
-
-
+	_tecs.update(_pitch - radians(_param_fw_psp_off.get()),
+		     _current_altitude,
+		     alt_sp,
+		     airspeed_sp,
+		     _airspeed,
+		     _eas2tas,
+		     climbout_mode,
+		     climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
+		     throttle_min,
+		     throttle_max,
+		     throttle_trim_comp,
+		     pitch_min_rad - radians(_param_fw_psp_off.get()),
+		     pitch_max_rad - radians(_param_fw_psp_off.get()),
+		     desired_max_climbrate,
+		     desired_max_sinkrate,
+		     _body_acceleration(0),
+		     -_local_pos.vz,
+		     hgt_rate_sp);
 
 	tecs_status_publish();
 }

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -58,7 +58,6 @@
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
 #include <lib/npfg/npfg.hpp>
 #include <lib/tecs/TECS.hpp>
-#include <lib/tecs/TECSnew.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/slew_rate/SlewRate.hpp>
@@ -389,7 +388,6 @@ private:
 
 	// total energy control system - airspeed / altitude control
 	TECS _tecs;
-	newTECS::TECS _tecsnew;
 
 	bool _reinitialize_tecs{true};
 	bool _tecs_is_running{false};