From b06b46b2245784280496e95e8c38a857f70039ed Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 26 May 2021 08:41:47 +0300 Subject: [PATCH] log raw airspeed derivative, pitch setpoint and airspeed innovation for TECS Signed-off-by: RomanBapst --- msg/tecs_status.msg | 3 +++ src/lib/tecs/TECS.cpp | 6 +++--- src/lib/tecs/TECS.hpp | 3 +++ src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp | 3 +++ 4 files changed, 12 insertions(+), 3 deletions(-) diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index 7e4fe08228..74c47e1a2c 100644 --- a/msg/tecs_status.msg +++ b/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 diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 26695120fb..494070c7e0 100644 --- a/src/lib/tecs/TECS.cpp +++ b/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 diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 67ffd93a7d..8e527c796f 100644 --- a/src/lib/tecs/TECS.hpp +++ b/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 diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index c438351d92..cc5e1794ac 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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();