Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 30 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-30 11:06:17 +03: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 // Update and average speed rate of change if airspeed is being measured
if (PX4_ISFINITE(equivalent_airspeed) && airspeed_sensor_enabled()) { if (PX4_ISFINITE(equivalent_airspeed) && airspeed_sensor_enabled()) {
_tas_rate_raw = speed_deriv_forward;
// Apply some noise filtering // Apply some noise filtering
_TAS_rate_filter.update(speed_deriv_forward); _TAS_rate_filter.update(speed_deriv_forward);
_speed_derivative = _TAS_rate_filter.getState(); _tas_rate_filtered = _TAS_rate_filter.getState();
} else { } else {
_speed_derivative = 0.0f; _tas_rate_raw = 0.0f;
_tas_rate_filtered = 0.0f;
} }
if (!_in_air) { if (!_in_air) {
@@ -141,7 +143,7 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
// Update TAS state // Update TAS state
_tas_rate_state = _tas_rate_state + tas_rate_state_input * dt; _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; _tas_state = _tas_state + tas_state_input * dt;
// Limit the TAS state to a minimum of 3 m/s // 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) // Calculate specific energy rates in units of (m**2/sec**3)
_SPE_rate = _vert_vel_state * CONSTANTS_ONE_G; // potential energy rate of change _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) void TECS::_update_throttle_setpoint(const float throttle_cruise)
src/lib/tecs/TECS.hpp
+3 -2
View File
@@ -149,7 +149,7 @@ public:
float get_EAS_setpoint() { return _EAS_setpoint; }; float get_EAS_setpoint() { return _EAS_setpoint; };
float TAS_rate_setpoint() { return _TAS_rate_setpoint; } float TAS_rate_setpoint() { return _TAS_rate_setpoint; }
float speed_derivative() { return _speed_derivative; } float speed_derivative() { return _tas_rate_filtered; }
float STE_error() { return _STE_error; } float STE_error() { return _STE_error; }
float STE_rate_error() { return _STE_rate_error; } float STE_rate_error() { return _STE_rate_error; }
@@ -239,7 +239,7 @@ private:
float _pitch_integ_state{0.0f}; ///< pitch integrator state (rad) float _pitch_integ_state{0.0f}; ///< pitch integrator state (rad)
float _last_throttle_setpoint{0.0f}; ///< throttle demand rate limiter state (1/sec) float _last_throttle_setpoint{0.0f}; ///< throttle demand rate limiter state (1/sec)
float _last_pitch_setpoint{0.0f}; ///< pitch demand rate limiter state (rad/sec) float _last_pitch_setpoint{0.0f}; ///< pitch demand rate limiter state (rad/sec)
float _speed_derivative{0.0f}; ///< rate of change of speed along X axis (m/sec**2) float _tas_rate_filtered{0.0f}; ///< low pass filtered rate of change of speed along X axis (m/sec**2)
// speed demand calculations // speed demand calculations
float _EAS{0.0f}; ///< equivalent airspeed (m/sec) float _EAS{0.0f}; ///< equivalent airspeed (m/sec)
@@ -250,6 +250,7 @@ private:
float _EAS_setpoint{0.0f}; ///< Equivalent airspeed demand (m/sec) float _EAS_setpoint{0.0f}; ///< Equivalent airspeed demand (m/sec)
float _TAS_setpoint_adj{0.0f}; ///< true airspeed demand tracked by the TECS algorithm (m/sec) float _TAS_setpoint_adj{0.0f}; ///< true airspeed demand tracked by the TECS algorithm (m/sec)
float _TAS_rate_setpoint{0.0f}; ///< true airspeed rate demand tracked by the TECS algorithm (m/sec**2) float _TAS_rate_setpoint{0.0f}; ///< true airspeed rate demand tracked by the TECS algorithm (m/sec**2)
float _tas_rate_raw{0.0f}; ///< true airspeed rate, calculated as inertial acceleration in body X direction
// height demand calculations // height demand calculations
float _hgt_setpoint{0.0f}; ///< demanded height tracked by the TECS algorithm (m) float _hgt_setpoint{0.0f}; ///< demanded height tracked by the TECS algorithm (m)