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

src/lib/avoidance/ObstacleAvoidance.cpp

@@ -89,8 +89,7 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 		PX4_WARN("Obstacle Avoidance system failed, loitering");
 		_publishVehicleCmdDoLoiter();
 
-		if (!PX4_ISFINITE(_failsafe_position(0)) || !PX4_ISFINITE(_failsafe_position(1))
-		    || !PX4_ISFINITE(_failsafe_position(2))) {
+		if (!_failsafe_position.isAllFinite()) {
 			// save vehicle position when entering failsafe
 			_failsafe_position = _position;
 		}

src/lib/mathlib/math/Functions.hpp

@@ -292,7 +292,7 @@ inline bool isFinite(const float &value)
 
 inline bool isFinite(const matrix::Vector3f &value)
 {
-	return PX4_ISFINITE(value(0)) && PX4_ISFINITE(value(1)) && PX4_ISFINITE(value(2));
+	return value.isAllFinite();
 }
 
 } /* namespace math */

src/lib/motion_planning/PositionSmoothing.cpp

@@ -172,8 +172,8 @@ const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &posi
 	// If a velocity is specified, that is used as a feedforward to track the position setpoint
 	// (ie. it assumes the position setpoint is moving at the specified velocity)
 	// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
-	auto &target = waypoints[1];
-	const bool xy_target_valid = PX4_ISFINITE(target(0)) && PX4_ISFINITE(target(1));
+	const Vector3f &target = waypoints[1];
+	const bool xy_target_valid = Vector2f(target).isAllFinite();
 	const bool z_target_valid = PX4_ISFINITE(target(2));
 
 	Vector3f velocity_setpoint = feedforward_velocity_setpoint;
@@ -269,8 +269,7 @@ void PositionSmoothing::_generateTrajectory(
 	float delta_time,
 	PositionSmoothingSetpoints &out_setpoints)
 {
-	if (!PX4_ISFINITE(velocity_setpoint(0)) || !PX4_ISFINITE(velocity_setpoint(1))
-	    || !PX4_ISFINITE(velocity_setpoint(2))) {
+	if (!velocity_setpoint.isAllFinite()) {
 		return;
 	}
 

src/lib/sensor_calibration/Accelerometer.cpp

@@ -111,7 +111,7 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
 bool Accelerometer::set_offset(const Vector3f &offset)
 {
 	if (Vector3f(_offset - offset).longerThan(0.01f)) {
-		if (PX4_ISFINITE(offset(0)) && PX4_ISFINITE(offset(1)) && PX4_ISFINITE(offset(2))) {
+		if (offset.isAllFinite()) {
 			_offset = offset;
 			_calibration_count++;
 			return true;
@@ -124,9 +124,7 @@ bool Accelerometer::set_offset(const Vector3f &offset)
 bool Accelerometer::set_scale(const Vector3f &scale)
 {
 	if (Vector3f(_scale - scale).longerThan(0.01f)) {
-		if ((scale(0) > 0.f) && (scale(1) > 0.f) && (scale(2) > 0.f) &&
-		    PX4_ISFINITE(scale(0)) && PX4_ISFINITE(scale(1)) && PX4_ISFINITE(scale(2))) {
-
+		if (scale.isAllFinite() && (scale(0) > 0.f) && (scale(1) > 0.f) && (scale(2) > 0.f)) {
 			_scale = scale;
 			_calibration_count++;
 			return true;

src/lib/sensor_calibration/Gyroscope.cpp

@@ -111,7 +111,7 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
 bool Gyroscope::set_offset(const Vector3f &offset)
 {
 	if (Vector3f(_offset - offset).longerThan(0.01f) || (_calibration_count == 0)) {
-		if (PX4_ISFINITE(offset(0)) && PX4_ISFINITE(offset(1)) && PX4_ISFINITE(offset(2))) {
+		if (offset.isAllFinite()) {
 			_offset = offset;
 			_calibration_count++;
 			return true;

src/lib/sensor_calibration/Magnetometer.cpp

@@ -71,7 +71,7 @@ void Magnetometer::set_device_id(uint32_t device_id)
 bool Magnetometer::set_offset(const Vector3f &offset)
 {
 	if (Vector3f(_offset - offset).longerThan(0.01f)) {
-		if (PX4_ISFINITE(offset(0)) && PX4_ISFINITE(offset(1)) && PX4_ISFINITE(offset(2))) {
+		if (offset.isAllFinite()) {
 			_offset = offset;
 			_calibration_count++;
 			return true;
@@ -84,9 +84,7 @@ bool Magnetometer::set_offset(const Vector3f &offset)
 bool Magnetometer::set_scale(const Vector3f &scale)
 {
 	if (Vector3f(_scale.diag() - scale).longerThan(0.01f)) {
-		if ((scale(0) > 0.f) && (scale(1) > 0.f) && (scale(2) > 0.f) &&
-		    PX4_ISFINITE(scale(0)) && PX4_ISFINITE(scale(1)) && PX4_ISFINITE(scale(2))) {
-
+		if (scale.isAllFinite() && (scale(0) > 0.f) && (scale(1) > 0.f) && (scale(2) > 0.f)) {
 			_scale(0, 0) = scale(0);
 			_scale(1, 1) = scale(1);
 			_scale(2, 2) = scale(2);
@@ -102,7 +100,7 @@ bool Magnetometer::set_scale(const Vector3f &scale)
 bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal)
 {
 	if (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiagonal).longerThan(0.01f)) {
-		if (PX4_ISFINITE(offdiagonal(0)) && PX4_ISFINITE(offdiagonal(1)) && PX4_ISFINITE(offdiagonal(2))) {
+		if (offdiagonal.isAllFinite()) {
 
 			_scale(0, 1) = offdiagonal(0);
 			_scale(1, 0) = offdiagonal(0);

src/lib/terrain_estimation/terrain_estimator.cpp

@@ -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;