TECS: Add new tecs library in parallel to old tecs in the position control library for comparison.

src/lib/tecs/CMakeLists.txt

@@ -36,4 +36,10 @@ px4_add_library(tecs
 	TECS.hpp
 )
 
+px4_add_library(tecsnew
+	TECSnew.cpp
+	TECSnew.hpp
+)
+
 target_link_libraries(tecs PRIVATE geo)
+target_link_libraries(tecsnew PRIVATE geo)

src/lib/tecs/TECS.cpp

@@ -204,6 +204,7 @@ void TECS::runAltitudeControllerSmoothVelocity(float alt_sp_amsl_m, float target
 	_alt_control_traj_generator.updateTraj(_dt);
 
 	_hgt_setpoint = _alt_control_traj_generator.getCurrentPosition();
+	_hgt_rate_from_hgt_ref = _alt_control_traj_generator.getCurrentVelocity();
 	_hgt_rate_setpoint = (_hgt_setpoint - alt_amsl) * _height_error_gain + _height_setpoint_gain_ff *
 			     _alt_control_traj_generator.getCurrentVelocity();
 	_hgt_rate_setpoint = math::constrain(_hgt_rate_setpoint, -_max_sink_rate, _max_climb_rate);
@@ -480,6 +481,7 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat
 		_velocity_control_traj_generator.setCurrentPositionEstimate(altitude_amsl);
 		_velocity_control_traj_generator.update(_dt, height_rate_sp);
 		_hgt_rate_setpoint = _velocity_control_traj_generator.getCurrentVelocity();
+		_smooth_hgt_rate_setpoint = _hgt_rate_setpoint;
 		altitude_sp_amsl = _velocity_control_traj_generator.getCurrentPosition();
 		control_altitude = PX4_ISFINITE(altitude_sp_amsl);
 

src/lib/tecs/TECS.hpp

@@ -162,6 +162,8 @@ public:
 	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
 
 	float hgt_setpoint() { return _hgt_setpoint; }
+	float hgt_rate_from_hgt_setpoint() { return _hgt_rate_from_hgt_ref;};
+	float smooth_hgt_rate_setpoint() {return _smooth_hgt_rate_setpoint;};
 	float vert_pos_state() { return _vert_pos_state; }
 
 	float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
@@ -182,6 +184,9 @@ public:
 	float SEB_error() { return _SEB_error; }
 	float SEB_rate_error() { return _SEB_rate_error; }
 
+	float SPE_rate() {return _SPE_rate;};
+	float SKE_rate() {return _SKE_rate;};
+
 	float throttle_integ_state() { return _throttle_integ_state; }
 	float pitch_integ_state() { return _pitch_integ_state; }
 
@@ -283,6 +288,8 @@ private:
 
 	// height demand calculations
 	float _hgt_setpoint{0.0f};					///< demanded height tracked by the TECS algorithm (m)
+	float _hgt_rate_from_hgt_ref{0.0f};
+	float _smooth_hgt_rate_setpoint{0.0f};
 	float _hgt_rate_setpoint{0.0f};					///< demanded climb rate tracked by the TECS algorithm
 
 	// vehicle physical limits

src/lib/tecs/TECSnew.cpp

@@ -48,6 +48,8 @@ using math::min;
 
 // TODO there seems to be an inconsistent definition of IAS/CAS/EAS/TAS
 // TODO Recheck the timing.
+namespace newTECS
+{
 void TECSAirspeedFilter::initialize(const float equivalent_airspeed)
 {
 
@@ -711,3 +713,5 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		_tecs_mode = ECL_TECS_MODE_NORMAL;
 	}
 }
+}
+

src/lib/tecs/TECSnew.hpp

@@ -49,6 +49,8 @@
 #include <motion_planning/VelocitySmoothing.hpp>
 #include <motion_planning/ManualVelocitySmoothingZ.hpp>
 
+namespace newTECS
+{
 class TECSAirspeedFilter {
 public:
 	/**
@@ -645,4 +647,5 @@ private:
 	 */
 	void _detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas, float tas_setpoint);
 };
+}