fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 05:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use isAllFinite() in all places that check finiteness on entire vectors or matrices

Browse Source
This commit is contained in:
Matthias Grob
2022-10-19 19:09:20 +02:00
parent 93de9567a5
commit 5ca28dd6dc
38 changed files with 128 additions and 252 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/terrain_estimation/terrain_estimator.cpp
+1 -17
View File
@@ -169,23 +169,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct sensor
}
// reinitialise filter if we find bad data
bool reinit = false;
for (int i = 0; i < n_x; i++) {
if (!PX4_ISFINITE(_x(i))) {
reinit = true;
}
}
for (int i = 0; i < n_x; i++) {
for (int j = 0; j < n_x; j++) {
if (!PX4_ISFINITE(_P(i, j))) {
reinit = true;
}
}
}
if (reinit) {
if (!_x.isAllFinite() || !_P.isAllFinite()) {
_x.zero();
_P.setZero();
_P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f;