Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

TECS: remove height rate complementary filter

Browse Source
This commit is contained in:
Daniel Agar
2019-11-22 10:19:48 -05:00
committed by Paul Riseborough
parent 8f70a10565
commit 250d5b8acc
2 changed files with 5 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tecs/tecs.cpp
+4 -46
View File
@@ -56,7 +56,7 @@ static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a fil
*/
void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat,
const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air,
float altitude, bool vz_valid, float vz, float az)
float altitude, float vz)
{
// calculate the time lapsed since the last update
uint64_t now = ecl_absolute_time();
@@ -74,16 +74,6 @@ void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &ro
}
if (reset_altitude) {
_vert_pos_state = altitude;
if (vz_valid) {
_vert_vel_state = -vz;
} else {
_vert_vel_state = 0.0f;
}
_vert_accel_state = 0.0f;
_states_initalized = false;
}
@@ -92,40 +82,9 @@ void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &ro
_in_air = in_air;
// Generate the height and climb rate state estimates
if (vz_valid) {
// Set the velocity and position state to the the INS data
_vert_vel_state = -vz;
_vert_pos_state = altitude;
} else {
// Get height acceleration
float hgt_ddot_mea = -az;
// If we have no vertical INS data, estimate the vertical velocity using a complementary filter
// Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega
// Reference Paper: Optimising the Gains of the Baro-Inertial Vertical Channel
// Widnall W.S, Sinha P.K, AIAA Journal of Guidance and Control, 78-1307R
float omega2 = _hgt_estimate_freq * _hgt_estimate_freq;
float hgt_err = altitude - _vert_pos_state;
float vert_accel_input = hgt_err * omega2 * _hgt_estimate_freq;
_vert_accel_state = _vert_accel_state + vert_accel_input * dt;
float vert_vel_input = _vert_accel_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f;
_vert_vel_state = _vert_vel_state + vert_vel_input * dt;
float vert_pos_input = _vert_vel_state + hgt_err * _hgt_estimate_freq * 3.0f;
// If more than 1 second has elapsed since last update then reset the position state
// to the measured height
if (reset_altitude) {
_vert_pos_state = altitude;
} else {
_vert_pos_state = _vert_pos_state + vert_pos_input * dt;
}
}
// Set the velocity and position state to the the INS data
_vert_vel_state = -vz;
_vert_pos_state = altitude;
// Update and average speed rate of change if airspeed is being measured
if (ISFINITE(airspeed) && airspeed_sensor_enabled()) {
@@ -521,7 +480,6 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initalized) {
// On first time through or when not using TECS of if there has been a large time slip,
// states must be reset to allow filters to a clean start
_vert_accel_state = 0.0f;
_vert_vel_state = 0.0f;
_vert_pos_state = baro_altitude;
_tas_rate_state = 0.0f;
tecs/tecs.h
+1 -5
View File
@@ -75,7 +75,7 @@ public:
*/
void update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat,
const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air,
float altitude, bool vz_valid, float vz, float az);
float altitude, float vz);
/**
* Update the control loop calculations
@@ -108,7 +108,6 @@ public:
void set_max_sink_rate(float sink_rate) { _max_sink_rate = sink_rate; }
void set_max_climb_rate(float climb_rate) { _max_climb_rate = climb_rate; }
void set_height_comp_filter_omega(float omega) { _hgt_estimate_freq = omega; }
void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; }
void set_heightrate_p(float heightrate_p) { _height_error_gain = heightrate_p; }
@@ -169,7 +168,6 @@ public:
// reset height states
_vert_pos_state = altitude;
_vert_accel_state = 0.0f;
_vert_vel_state = 0.0f;
}
@@ -183,7 +181,6 @@ private:
uint64_t _pitch_update_timestamp{0}; ///< last timestamp of the pitch function call
// controller parameters
float _hgt_estimate_freq{0.0f}; ///< cross-over frequency of the height rate complementary filter (rad/sec)
float _tas_estimate_freq{0.0f}; ///< cross-over frequency of the true airspeed complementary filter (rad/sec)
float _max_climb_rate{2.0f}; ///< climb rate produced by max allowed throttle (m/sec)
float _min_sink_rate{1.0f}; ///< sink rate produced by min allowed throttle (m/sec)
@@ -208,7 +205,6 @@ private:
float _pitch_setpoint{0.0f}; ///< pitch angle demand (radians)
// complimentary filter states
float _vert_accel_state{0.0f}; ///< complimentary filter state - height second derivative (m/sec**2)
float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec)
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)