fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

WindEstimator: avoid division by 0

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-10-21 11:45:37 +02:00
parent c0754cf324
commit b38bf23d6e
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/wind_estimator/WindEstimator.cpp
View File

@ -166,7 +166,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
// compute state observation matrix H
const float HH0 = airspeed_predicted_raw;
const float HH1 = _state(INDEX_TAS_SCALE) / HH0;
const float HH1 = _state(INDEX_TAS_SCALE) / math::max(HH0, 0.1f);
matrix::Matrix<float, 1, 3> H_tas;
H_tas(0, 0) = HH1 * (-v_n + _state(INDEX_W_N));

4
src/lib/wind_estimator/WindEstimator.hpp
View File

@ -69,7 +69,7 @@ public:
matrix::Vector2f get_wind() { return matrix::Vector2f{_state(INDEX_W_N), _state(INDEX_W_E)}; }
// invert scale (CAS = IAS * scale), protect agains division by 0, constrain to [0.1, 10]
float get_tas_scale() { return math::constrain(1.f / (_state(INDEX_TAS_SCALE) + 0.0001f), 0.1f, 10.f); }
float get_tas_scale() { return 1.f / math::constrain(_state(INDEX_TAS_SCALE), 0.1f, 10.0f); }
float get_tas_scale_var() { return _P(2, 2); }
float get_tas_innov() { return _tas_innov; }
float get_tas_innov_var() { return _tas_innov_var; }
@ -84,7 +84,7 @@ public:
void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; }
void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; }
void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; }
void set_scale_init(float scale_init) {_scale_init = math::constrain(1.f / (scale_init + 0.0001f), 0.1f, 10.f); }
void set_scale_init(float scale_init) {_scale_init = 1.f / math::constrain(scale_init, 0.1f, 10.f); }
void set_disable_tas_scale_estimate(bool disable_tas_scale_estimate) {_disable_tas_scale_estimate = disable_tas_scale_estimate; }
private: