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

tecs: use raw inertial acceleration for true airspeed complementary filter

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2021-03-31 20:12:40 -04:00
committed by Daniel Agar
parent 9dd6bef7f9
commit b66ae5f2d4
2 changed files with 9 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+6 -4
View File
@@ -88,12 +88,14 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float
// Update and average speed rate of change if airspeed is being measured
if (PX4_ISFINITE(equivalent_airspeed) && airspeed_sensor_enabled()) {
_tas_rate_raw = speed_deriv_forward;
// Apply some noise filtering
_TAS_rate_filter.update(speed_deriv_forward);
_speed_derivative = _TAS_rate_filter.getState();
_tas_rate_filtered = _TAS_rate_filter.getState();
} else {
_speed_derivative = 0.0f;
_tas_rate_raw = 0.0f;
_tas_rate_filtered = 0.0f;
}
if (!_in_air) {
@@ -141,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 + _speed_derivative + tas_error * _tas_estimate_freq * 1.4142f;
float tas_state_input = _tas_rate_state + _tas_rate_raw + tas_error * _tas_estimate_freq * 1.4142f;
_tas_state = _tas_state + tas_state_input * dt;
// Limit the TAS state to a minimum of 3 m/s
@@ -260,7 +262,7 @@ void TECS::_update_energy_estimates()
// Calculate specific energy rates in units of (m**2/sec**3)
_SPE_rate = _vert_vel_state * CONSTANTS_ONE_G; // potential energy rate of change
_SKE_rate = _tas_state * _speed_derivative;// kinetic energy rate of change
_SKE_rate = _tas_state * _tas_rate_filtered;// kinetic energy rate of change
}
void TECS::_update_throttle_setpoint(const float throttle_cruise)