fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 14:20:35 +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-18 16:57:01 +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/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+5 -9
View File
@@ -581,9 +581,8 @@ void BlockLocalPositionEstimator::publishLocalPos()
}
// publish local position
if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& PX4_ISFINITE(_x(X_vz))) {
if (Vector3f(_x(X_x), _x(X_y), _x(X_z)).isAllFinite() &&
Vector3f(_x(X_vx), _x(X_vy), _x(X_vz)).isAllFinite()) {
_pub_lpos.get().timestamp_sample = _timeStamp;
_pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY;
@@ -644,10 +643,8 @@ void BlockLocalPositionEstimator::publishOdom()
const Vector<float, n_x> &xLP = _xLowPass.getState();
// publish vehicle odometry
if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& PX4_ISFINITE(_x(X_vz))) {
if (Vector3f(_x(X_x), _x(X_y), _x(X_z)).isAllFinite() &&
Vector3f(_x(X_vx), _x(X_vy), _x(X_vz)).isAllFinite()) {
_pub_odom.get().timestamp_sample = _timeStamp;
_pub_odom.get().pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
@@ -795,8 +792,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
}
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
PX4_ISFINITE(xLP(X_vz))) {
Vector3f(xLP(X_vx), xLP(X_vy), xLP(X_vz)).isAllFinite()) {
_pub_gpos.get().timestamp_sample = _timeStamp;
_pub_gpos.get().lat = lat;
_pub_gpos.get().lon = lon;