fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-12 14:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2021-05-31 23:56:23 +02:00
committed by Silvan Fuhrer
parent 806b462935
commit b06b46b224
4 changed files with 12 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+3 -3
View File
@@ -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