log raw airspeed derivative, pitch setpoint and airspeed innovation for TECS

Signed-off-by: RomanBapst <bapstroman@gmail.com>

msg/tecs_status.msg

@@ -17,6 +17,8 @@ float32 true_airspeed_sp
 float32 true_airspeed_filtered
 float32 true_airspeed_derivative_sp
 float32 true_airspeed_derivative
+float32 true_airspeed_derivative_raw
+float32 true_airspeed_innovation
 
 float32 total_energy_error
 float32 energy_distribution_error
@@ -37,5 +39,6 @@ float32 throttle_integ
 float32 pitch_integ
 
 float32 throttle_sp
+float32 pitch_sp_rad
 
 uint8 mode

src/lib/tecs/TECS.cpp

@@ -133,8 +133,8 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
 	// Obtain a smoothed TAS estimate using a second order complementary filter
 
 	// Update TAS rate state
-	float tas_error = (_EAS * EAS2TAS) - _tas_state;
-	float tas_rate_state_input = tas_error * _tas_estimate_freq * _tas_estimate_freq;
+	_tas_innov = (_EAS * EAS2TAS) - _tas_state;
+	float tas_rate_state_input = _tas_innov * _tas_estimate_freq * _tas_estimate_freq;
 
 	// limit integrator input to prevent windup
 	if (_tas_state < 3.1f) {
@@ -143,7 +143,7 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
 
 	// Update TAS state
 	_tas_rate_state = _tas_rate_state + tas_rate_state_input * dt;
-	float tas_state_input = _tas_rate_state + _tas_rate_raw + tas_error * _tas_estimate_freq * 1.4142f;
+	float tas_state_input = _tas_rate_state + _tas_rate_raw + _tas_innov * _tas_estimate_freq * 1.4142f;
 	_tas_state = _tas_state + tas_state_input * dt;
 
 	// Limit the TAS state to a minimum of 3 m/s

src/lib/tecs/TECS.hpp

@@ -144,6 +144,7 @@ public:
 
 	float TAS_setpoint_adj() { return _TAS_setpoint_adj; }
 	float tas_state() { return _tas_state; }
+	float getTASInnovation() { return _tas_innov; }
 
 	float hgt_rate_setpoint() { return _hgt_rate_setpoint; }
 	float vert_vel_state() { return _vert_vel_state; }
@@ -151,6 +152,7 @@ public:
 	float get_EAS_setpoint() { return _EAS_setpoint; };
 	float TAS_rate_setpoint() { return _TAS_rate_setpoint; }
 	float speed_derivative() { return _tas_rate_filtered; }
+	float speed_derivative_raw() { return _tas_rate_raw; }
 
 	float STE_error() { return _STE_error; }
 	float STE_rate_error() { return _STE_rate_error; }
@@ -235,6 +237,7 @@ private:
 	float _vert_pos_state{0.0f};					///< complimentary filter state - height (m)
 	float _tas_rate_state{0.0f};					///< complimentary filter state - true airspeed first derivative (m/sec**2)
 	float _tas_state{0.0f};						///< complimentary filter state - true airspeed (m/sec)
+	float _tas_innov{0.0f};						///< complimentary filter true airspeed innovation (m/sec)
 
 	// controller states
 	float _throttle_integ_state{0.0f};				///< throttle integrator state

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -398,6 +398,8 @@ FixedwingPositionControl::tecs_status_publish()
 	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.total_energy_error = _tecs.STE_error();
 	t.total_energy_rate_error = _tecs.STE_rate_error();
@@ -419,6 +421,7 @@ FixedwingPositionControl::tecs_status_publish()
 	t.pitch_integ = _tecs.pitch_integ_state();
 
 	t.throttle_sp = _tecs.get_throttle_setpoint();
+	t.pitch_sp_rad = _tecs.get_pitch_setpoint();
 
 	t.timestamp = hrt_absolute_time();