From 5ca28dd6dc7b665aadfd78c6901b066076584123 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 18 Oct 2022 16:57:01 +0200 Subject: [PATCH] Use isAllFinite() in all places that check finiteness on entire vectors or matrices --- src/lib/avoidance/ObstacleAvoidance.cpp | 3 +- src/lib/mathlib/math/Functions.hpp | 2 +- src/lib/motion_planning/PositionSmoothing.cpp | 7 ++- src/lib/sensor_calibration/Accelerometer.cpp | 6 +-- src/lib/sensor_calibration/Gyroscope.cpp | 2 +- src/lib/sensor_calibration/Magnetometer.cpp | 8 ++-- .../terrain_estimation/terrain_estimator.cpp | 18 +------- .../airspeed_selector/AirspeedValidator.cpp | 3 +- .../attitude_estimator_q_main.cpp | 8 +--- .../checks/estimatorCheck.cpp | 3 +- .../commander/accelerometer_calibration.cpp | 6 +-- src/modules/commander/gyro_calibration.cpp | 4 +- src/modules/ekf2/EKF/estimator_interface.h | 2 +- src/modules/ekf2/EKF/optflow_fusion.cpp | 2 +- src/modules/ekf2/EKF/optical_flow_control.cpp | 3 +- src/modules/ekf2/EKF2.cpp | 24 +++------- .../tasks/Auto/FlightTaskAuto.cpp | 17 +++---- .../FlightTaskAutoFollowTarget.cpp | 16 ++----- .../TargetEstimator.cpp | 16 +++---- .../TargetEstimator.hpp | 4 +- .../tasks/Failsafe/FlightTaskFailsafe.cpp | 6 +-- .../FlightTaskManualAcceleration.cpp | 6 +-- .../FlightTaskManualPosition.cpp | 19 +++----- .../tasks/Orbit/FlightTaskOrbit.cpp | 7 +-- .../tasks/Utility/StickAccelerationXY.cpp | 4 +- .../tasks/Utility/Sticks.cpp | 7 +-- .../FixedwingPositionControl.cpp | 9 ++-- src/modules/gimbal/output.cpp | 6 +-- .../LandingTargetEstimator.cpp | 3 +- .../BlockLocalPositionEstimator.cpp | 14 +++--- .../mag_bias_estimator/MagBiasEstimator.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 45 ++++++++----------- .../MulticopterPositionControl.cpp | 31 ++++++------- .../PositionControl/PositionControl.cpp | 5 +-- .../VehicleAcceleration.cpp | 2 +- .../VehicleAngularVelocity.cpp | 12 ++--- .../VehicleOpticalFlow.cpp | 6 +-- .../simulator_mavlink/SimulatorMavlink.cpp | 42 +++++++---------- 38 files changed, 128 insertions(+), 252 deletions(-) diff --git a/src/lib/avoidance/ObstacleAvoidance.cpp b/src/lib/avoidance/ObstacleAvoidance.cpp index 067f6fbafd..e69c812475 100644 --- a/src/lib/avoidance/ObstacleAvoidance.cpp +++ b/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; } diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index b4e1162817..b81459ff67 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/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 */ diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index 5f555c43c2..a5c68f04d1 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/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; } diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index b738dc862e..ac7c74d1dc 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/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; diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index c953e98592..e81c1903c9 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/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; diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index 4ea0e6982e..efb2b8c1b2 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/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); diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index cae05bda9b..be0fdecc75 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/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; diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 4c64f615be..275c9be931 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -130,8 +130,7 @@ AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, const matrix::Vec _scale_check_TAS(segment_index) = airspeed_true_raw; // run check if all segments are filled - if (PX4_ISFINITE(_scale_check_groundspeed.norm_squared())) { - + if (_scale_check_groundspeed.isAllFinite()) { float ground_speed_sum = 0.f; float TAS_sum = 0.f; diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index ad5db582e4..89ff104279 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -441,9 +441,7 @@ bool AttitudeEstimatorQ::init_attitude_q() _q.normalize(); - if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && - PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) && - _q.length() > 0.95f && _q.length() < 1.05f) { + if (_q.isAllFinite() && _q.length() > 0.95f && _q.length() < 1.05f) { _initialized = true; } else { @@ -552,9 +550,7 @@ bool AttitudeEstimatorQ::update(float dt) // Normalize quaternion _q.normalize(); - if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && - PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) { - + if (!_q.isAllFinite()) { // Reset quaternion to last good state _q = q_last; _rates.zero(); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 4d1ff833e8..1d9843a9af 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -752,8 +752,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f _vehicle_angular_velocity_sub.copy(&angular_velocity); const bool condition_angular_velocity_time_valid = angular_velocity.timestamp != 0 && now < angular_velocity.timestamp + 1_s; - const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0]) - && PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]); + const bool condition_angular_velocity_finite = matrix::Vector3f(angular_velocity.xyz).isAllFinite(); const bool angular_velocity_invalid = !condition_angular_velocity_time_valid || !condition_angular_velocity_finite; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index eedf0b3233..63d04957af 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -558,11 +558,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) calibration::Accelerometer calibration{arp.device_id}; - if (!calibrated || (offset.norm() > CONSTANTS_ONE_G) - || !PX4_ISFINITE(offset(0)) - || !PX4_ISFINITE(offset(1)) - || !PX4_ISFINITE(offset(2))) { - + if (!calibrated || (offset.norm() > CONSTANTS_ONE_G) || !offset.isAllFinite()) { PX4_ERR("accel %d quick calibrate failed", accel_index); } else { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index a32a0511cf..0ebcaebfc1 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -219,9 +219,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) /* maximum allowable calibration error */ static constexpr float maxoff = math::radians(0.6f); - if (!PX4_ISFINITE(worker_data.offset[0](0)) || - !PX4_ISFINITE(worker_data.offset[0](1)) || - !PX4_ISFINITE(worker_data.offset[0](2)) || + if (!worker_data.offset[0].isAllFinite() || fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) { calibration_log_critical(mavlink_log_pub, "motion, retrying.."); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 1f84f348c2..412b865c1a 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -121,7 +121,7 @@ public: void set_vehicle_at_rest(bool at_rest) { _control_status.flags.vehicle_at_rest = at_rest; } // return true if the attitude is usable - bool attitude_valid() const { return PX4_ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; } + bool attitude_valid() const { return _output_new.quat_nominal.isAllFinite() && _control_status.flags.tilt_align; } // get vehicle landed status data bool get_in_air_status() const { return _control_status.flags.in_air; } diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 90d9960cb7..fc3b210175 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -346,7 +346,7 @@ void Ekf::fuseOptFlow() bool Ekf::calcOptFlowBodyRateComp() { bool is_body_rate_comp_available = false; - const bool use_flow_sensor_gyro = PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2)); + const bool use_flow_sensor_gyro = _flow_sample_delayed.gyro_xyz.isAllFinite(); if (use_flow_sensor_gyro) { diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index fdd740a6b1..0545f24f65 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -88,8 +88,7 @@ void Ekf::controlOpticalFlowFusion() const bool is_body_rate_comp_available = calcOptFlowBodyRateComp(); // don't allow invalid flow gyro_xyz to propagate - if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) { - + if (!_flow_sample_delayed.gyro_xyz.isAllFinite()) { _flow_sample_delayed.gyro_xyz.zero(); } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 2a8b2a86aa..b5ef696ad1 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1741,7 +1741,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo // if error estimates are unavailable, use parameter defined defaults // check for valid velocity data - if (PX4_ISFINITE(ev_odom.velocity[0]) && PX4_ISFINITE(ev_odom.velocity[1]) && PX4_ISFINITE(ev_odom.velocity[2])) { + if (Vector3f(ev_odom.velocity).isAllFinite()) { bool velocity_valid = true; switch (ev_odom.velocity_frame) { @@ -1770,11 +1770,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo const float evv_noise_var = sq(_param_ekf2_evv_noise.get()); // velocity measurement error from ev_data or parameters - if (!_param_ekf2_ev_noise_md.get() && - PX4_ISFINITE(ev_odom.velocity_variance[0]) && - PX4_ISFINITE(ev_odom.velocity_variance[1]) && - PX4_ISFINITE(ev_odom.velocity_variance[2])) { - + if (!_param_ekf2_ev_noise_md.get() && Vector3f(ev_odom.velocity_variance).isAllFinite()) { ev_data.velVar(0) = fmaxf(evv_noise_var, ev_odom.velocity_variance[0]); ev_data.velVar(1) = fmaxf(evv_noise_var, ev_odom.velocity_variance[1]); ev_data.velVar(2) = fmaxf(evv_noise_var, ev_odom.velocity_variance[2]); @@ -1788,8 +1784,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo } // check for valid position data - if (PX4_ISFINITE(ev_odom.position[0]) && PX4_ISFINITE(ev_odom.position[1]) && PX4_ISFINITE(ev_odom.position[2])) { - + if (Vector3f(ev_odom.position).isAllFinite()) { bool position_valid = true; // switch (ev_odom.pose_frame) { @@ -1814,11 +1809,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo const float evp_noise_var = sq(_param_ekf2_evp_noise.get()); // position measurement error from ev_data or parameters - if (!_param_ekf2_ev_noise_md.get() && - PX4_ISFINITE(ev_odom.position_variance[0]) && - PX4_ISFINITE(ev_odom.position_variance[1]) && - PX4_ISFINITE(ev_odom.position_variance[2]) - ) { + if (!_param_ekf2_ev_noise_md.get() && Vector3f(ev_odom.position_variance).isAllFinite()) { ev_data.posVar(0) = fmaxf(evp_noise_var, ev_odom.position_variance[0]); ev_data.posVar(1) = fmaxf(evp_noise_var, ev_odom.position_variance[1]); ev_data.posVar(2) = fmaxf(evp_noise_var, ev_odom.position_variance[2]); @@ -1832,8 +1823,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo } // check for valid orientation data - if ((PX4_ISFINITE(ev_odom.q[0]) && PX4_ISFINITE(ev_odom.q[1]) - && PX4_ISFINITE(ev_odom.q[2]) && PX4_ISFINITE(ev_odom.q[3])) + if ((Quatf(ev_odom.q).isAllFinite()) && ((fabsf(ev_odom.q[0]) > 0.f) || (fabsf(ev_odom.q[1]) > 0.f) || (fabsf(ev_odom.q[2]) > 0.f) || (fabsf(ev_odom.q[3]) > 0.f)) ) { @@ -1896,9 +1886,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) .quality = optical_flow.quality, }; - if (PX4_ISFINITE(optical_flow.pixel_flow[0]) && - PX4_ISFINITE(optical_flow.pixel_flow[1]) && - flow.dt < 1) { + if (Vector2f(optical_flow.pixel_flow).isAllFinite() && flow.dt < 1) { // Save sensor limits reported by the optical flow sensor _ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance, diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 440a112a1a..19b7298fef 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -97,12 +97,7 @@ bool FlightTaskAuto::updateInitialize() // require valid reference and valid target ret = ret && _evaluateGlobalReference() && _evaluateTriplets(); // require valid position - ret = ret && PX4_ISFINITE(_position(0)) - && PX4_ISFINITE(_position(1)) - && PX4_ISFINITE(_position(2)) - && PX4_ISFINITE(_velocity(0)) - && PX4_ISFINITE(_velocity(1)) - && PX4_ISFINITE(_velocity(2)); + ret = ret && _position.isAllFinite() && _velocity.isAllFinite(); return ret; } @@ -360,7 +355,7 @@ bool FlightTaskAuto::_evaluateTriplets() if (!PX4_ISFINITE(_sub_triplet_setpoint.get().current.lat) || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.lon)) { // No position provided in xy. Lock position - if (!PX4_ISFINITE(_lock_position_xy(0))) { + if (!_lock_position_xy.isAllFinite()) { tmp_target(0) = _lock_position_xy(0) = _position(0); tmp_target(1) = _lock_position_xy(1) = _position(1); @@ -388,9 +383,7 @@ bool FlightTaskAuto::_evaluateTriplets() const bool prev_next_validity_changed = (_prev_was_valid != _sub_triplet_setpoint.get().previous.valid) || (_next_was_valid != _sub_triplet_setpoint.get().next.valid); - if (PX4_ISFINITE(_triplet_target(0)) - && PX4_ISFINITE(_triplet_target(1)) - && PX4_ISFINITE(_triplet_target(2)) + if (_triplet_target.isAllFinite() && fabsf(_triplet_target(0) - tmp_target(0)) < 0.001f && fabsf(_triplet_target(1) - tmp_target(1)) < 0.001f && fabsf(_triplet_target(2) - tmp_target(2)) < 0.001f @@ -402,7 +395,7 @@ bool FlightTaskAuto::_evaluateTriplets() _triplet_target = tmp_target; _target_acceptance_radius = _sub_triplet_setpoint.get().current.acceptance_radius; - if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) { + if (!Vector2f(_triplet_target).isAllFinite()) { // Horizontal target is not finite. _triplet_target(0) = _position(0); _triplet_target(1) = _position(1); @@ -536,7 +529,7 @@ void FlightTaskAuto::_set_heading_from_mode() break; } - if (PX4_ISFINITE(v.norm_squared())) { + if (v.isAllFinite()) { // We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance // radius, lock yaw to current yaw. // This prevents excessive yawing. diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp index e682638a00..4699f1349f 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp @@ -67,8 +67,7 @@ bool FlightTaskAutoFollowTarget::activate(const trajectory_setpoint_s &last_setp { bool ret = FlightTask::activate(last_setpoint); - if (!PX4_ISFINITE(_position_setpoint(0)) || !PX4_ISFINITE(_position_setpoint(1)) - || !PX4_ISFINITE(_position_setpoint(2))) { + if (!_position_setpoint.isAllFinite()) { _position_setpoint = _position; } @@ -128,18 +127,12 @@ void FlightTaskAutoFollowTarget::updateTargetPositionVelocityFilter(const follow const Vector3f pos_ned_est{follow_target_estimator.pos_est}; const Vector3f vel_ned_est{follow_target_estimator.vel_est}; - const bool filtered_target_pos_is_finite = PX4_ISFINITE(_target_position_velocity_filter.getState()(0)) - && PX4_ISFINITE(_target_position_velocity_filter.getState()(1)) - && PX4_ISFINITE(_target_position_velocity_filter.getState()(2)); - const bool filtered_target_vel_is_finite = PX4_ISFINITE(_target_position_velocity_filter.getRate()(0)) - && PX4_ISFINITE(_target_position_velocity_filter.getRate()(1)) - && PX4_ISFINITE(_target_position_velocity_filter.getRate()(2)); - const bool target_estimator_timed_out = ((follow_target_estimator.timestamp - _last_valid_target_estimator_timestamp) > TARGET_ESTIMATOR_TIMEOUT_US); _last_valid_target_estimator_timestamp = follow_target_estimator.timestamp; - if (!filtered_target_pos_is_finite || !filtered_target_vel_is_finite || target_estimator_timed_out) { + if (!_target_position_velocity_filter.getState().isAllFinite() + || !_target_position_velocity_filter.getRate().isAllFinite() || target_estimator_timed_out) { _target_position_velocity_filter.reset(pos_ned_est, vel_ned_est); } else { @@ -364,8 +357,7 @@ bool FlightTaskAutoFollowTarget::update() const Vector2f orbit_total_accel = orbit_radial_accel + orbit_tangential_accel; // Position + Velocity + Acceleration setpoint - if (PX4_ISFINITE(drone_desired_position(0)) && PX4_ISFINITE(drone_desired_position(1)) - && PX4_ISFINITE(drone_desired_position(2))) { + if (drone_desired_position.isAllFinite()) { if (fabsf(drone_desired_position(2) - _position(2)) < ALT_ACCEPTANCE_THRESHOLD) { // Drone is close enough to the altitude target : Apply Horizontal + Velocity Control _position_setpoint = drone_desired_position; diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp index 880be632d1..c667594b10 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp @@ -210,12 +210,8 @@ bool TargetEstimator::measurement_can_be_fused(const Vector3f ¤t_measureme const Vector3f &previous_measurement, uint64_t last_fusion_timestamp, float min_delta_t) const { - const bool measurement_valid = PX4_ISFINITE(current_measurement(0)) && PX4_ISFINITE(current_measurement(1)) - && PX4_ISFINITE(current_measurement(2)); - const bool sensor_data_changed = Vector3f(current_measurement - previous_measurement).longerThan(2.0f * FLT_EPSILON) - || !PX4_ISFINITE(previous_measurement(0)) || !PX4_ISFINITE(previous_measurement(1)) - || !PX4_ISFINITE(previous_measurement(2)); + || !previous_measurement.isAllFinite(); // This is required as a throttle const bool fusion_old_enough = hrt_absolute_time() - last_fusion_timestamp > @@ -225,7 +221,7 @@ bool TargetEstimator::measurement_can_be_fused(const Vector3f ¤t_measureme const bool fusion_too_old = hrt_absolute_time() - last_fusion_timestamp > 2 * min_delta_t * 1000; - return measurement_valid && fusion_old_enough && (sensor_data_changed || fusion_too_old); + return current_measurement.isAllFinite() && fusion_old_enough && (sensor_data_changed || fusion_too_old); // return measurement_valid; } @@ -242,7 +238,7 @@ void TargetEstimator::measurement_update(follow_target_s follow_target) if (_last_follow_target_timestamp == 0) { _filter_states.pos_ned_est = pos_measured; - if (PX4_ISFINITE(vel_measured(0)) && PX4_ISFINITE(vel_measured(1)) && PX4_ISFINITE(vel_measured(2))) { + if (vel_measured.isAllFinite()) { _filter_states.vel_ned_est = vel_measured; } else { @@ -316,11 +312,11 @@ void TargetEstimator::prediction_update(float deltatime) const Vector3f vel_ned_est_prev = _filter_states.vel_ned_est; const Vector3f acc_ned_est_prev = _filter_states.acc_ned_est; - if (PX4_ISFINITE(vel_ned_est_prev(0)) && PX4_ISFINITE(vel_ned_est_prev(1)) && PX4_ISFINITE(vel_ned_est_prev(2))) { + if (vel_ned_est_prev.isAllFinite()) { _filter_states.pos_ned_est += deltatime * vel_ned_est_prev + 0.5f * acc_ned_est_prev * deltatime * deltatime; } - if (PX4_ISFINITE(acc_ned_est_prev(0)) && PX4_ISFINITE(acc_ned_est_prev(1)) && PX4_ISFINITE(acc_ned_est_prev(2))) { + if (acc_ned_est_prev.isAllFinite()) { _filter_states.vel_ned_est += deltatime * acc_ned_est_prev; } } @@ -329,7 +325,7 @@ Vector3 TargetEstimator::get_lat_lon_alt_est() const { Vector3 lat_lon_alt{(double)NAN, (double)NAN, (double)NAN}; - if (PX4_ISFINITE(_filter_states.pos_ned_est(0)) && PX4_ISFINITE(_filter_states.pos_ned_est(0))) { + if (Vector2f(_filter_states.pos_ned_est).isAllFinite()) { _reference_position.reproject(_filter_states.pos_ned_est(0), _filter_states.pos_ned_est(1), lat_lon_alt(0), lat_lon_alt(1)); lat_lon_alt(2) = -(double)_filter_states.pos_ned_est(2) + (double)_vehicle_local_position.ref_alt; diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp index 78f43f563b..4dc22e455a 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp @@ -91,9 +91,7 @@ struct filter_states_s { */ bool is_finite() { - return PX4_ISFINITE(pos_ned_est(0)) && PX4_ISFINITE(pos_ned_est(1)) && PX4_ISFINITE(pos_ned_est(2)) && - PX4_ISFINITE(vel_ned_est(0)) && PX4_ISFINITE(vel_ned_est(1)) && PX4_ISFINITE(vel_ned_est(2)) && - PX4_ISFINITE(acc_ned_est(0)) && PX4_ISFINITE(acc_ned_est(1)) && PX4_ISFINITE(acc_ned_est(2)); + return pos_ned_est.isAllFinite() && vel_ned_est.isAllFinite() && acc_ned_est.isAllFinite(); } /** diff --git a/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp b/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp index e90d3a723a..24174e51f7 100644 --- a/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp +++ b/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp @@ -51,13 +51,13 @@ bool FlightTaskFailsafe::update() { bool ret = FlightTask::update(); - if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) { + if (matrix::Vector2f(_position).isAllFinite()) { // stay at current position setpoint _velocity_setpoint(0) = _velocity_setpoint(1) = 0.f; _acceleration_setpoint(0) = _acceleration_setpoint(1) = 0.f; - } else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) { - // don't move along xy + } else if (matrix::Vector2f(_velocity).isAllFinite()) { + // don't move horizontally _position_setpoint(0) = _position_setpoint(1) = NAN; _acceleration_setpoint(0) = _acceleration_setpoint(1) = NAN; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index ec4ed9e277..c1d1edaff4 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -45,14 +45,14 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se _stick_acceleration_xy.resetPosition(); - if (PX4_ISFINITE(last_setpoint.velocity[0]) && PX4_ISFINITE(last_setpoint.velocity[1])) { + if (Vector2f(last_setpoint.velocity).isAllFinite()) { _stick_acceleration_xy.resetVelocity(Vector2f(last_setpoint.velocity)); } else { _stick_acceleration_xy.resetVelocity(_velocity.xy()); } - if (PX4_ISFINITE(last_setpoint.acceleration[0]) && PX4_ISFINITE(last_setpoint.acceleration[1])) { + if (Vector2f(last_setpoint.acceleration).isAllFinite()) { _stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration)); } @@ -80,7 +80,7 @@ bool FlightTaskManualAcceleration::update() _yaw_setpoint = NAN; // only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN) - if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) { + if (Vector2f(_position_setpoint).isAllFinite()) { // vehicle is steady _yawspeed_setpoint += _weathervane.getWeathervaneYawrate(); } diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index f0b5eb31e7..0cfe1d4d67 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -45,10 +45,7 @@ bool FlightTaskManualPosition::updateInitialize() { bool ret = FlightTaskManualAltitude::updateInitialize(); // require valid position / velocity in xy - return ret && PX4_ISFINITE(_position(0)) - && PX4_ISFINITE(_position(1)) - && PX4_ISFINITE(_velocity(0)) - && PX4_ISFINITE(_velocity(1)); + return ret && Vector2f(_position).isAllFinite() && Vector2f(_velocity).isAllFinite(); } bool FlightTaskManualPosition::activate(const trajectory_setpoint_s &last_setpoint) @@ -112,16 +109,14 @@ void FlightTaskManualPosition::_updateXYlock() const bool apply_brake = Vector2f(_velocity_setpoint).length() < FLT_EPSILON; const bool stopped = (_param_mpc_hold_max_xy.get() < FLT_EPSILON || vel_xy_norm < _param_mpc_hold_max_xy.get()); - if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) { - _position_setpoint(0) = _position(0); - _position_setpoint(1) = _position(1); + if (apply_brake && stopped && !Vector2f(_position_setpoint).isAllFinite()) { + _position_setpoint.xy() = _position.xy(); - } else if (PX4_ISFINITE(_position_setpoint(0)) && apply_brake) { + } else if (Vector2f(_position_setpoint).isAllFinite() && apply_brake) { // Position is locked but check if a reset event has happened. // We will shift the setpoints. if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counter) { - _position_setpoint(0) = _position(0); - _position_setpoint(1) = _position(1); + _position_setpoint.xy() = _position.xy(); _reset_counter = _sub_vehicle_local_position.get().xy_reset_counter; } @@ -144,8 +139,8 @@ void FlightTaskManualPosition::_updateSetpoints() if (_weathervane.isActive()) { _yaw_setpoint = NAN; - // only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN) - if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) { + // only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. aren't NAN) + if (Vector2f(_position_setpoint).isAllFinite()) { // vehicle is steady _yawspeed_setpoint += _weathervane.getWeathervaneYawrate(); } diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 0203402995..b1a0e70ffb 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -169,12 +169,7 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint) _slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get())); // need a valid position and velocity - ret = ret && PX4_ISFINITE(_position(0)) - && PX4_ISFINITE(_position(1)) - && PX4_ISFINITE(_position(2)) - && PX4_ISFINITE(_velocity(0)) - && PX4_ISFINITE(_velocity(1)) - && PX4_ISFINITE(_velocity(2)); + ret = ret && _position.isAllFinite() && _velocity.isAllFinite(); Vector3f pos_prev{last_setpoint.position}; Vector3f vel_prev{last_setpoint.velocity}; diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index eceaa6da39..2a33ae80c5 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -177,7 +177,7 @@ void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration) void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt) { const bool moving = _velocity_setpoint.norm_squared() > FLT_EPSILON; - const bool position_locked = PX4_ISFINITE(_position_setpoint(0)) || PX4_ISFINITE(_position_setpoint(1)); + const bool position_locked = Vector2f(_position_setpoint).isAllFinite(); // lock position if (!moving && !position_locked) { @@ -189,7 +189,7 @@ void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector _position_setpoint.setNaN(); // avoid velocity setpoint jump caused by ignoring remaining position error - if (PX4_ISFINITE(vel_sp_feedback(0)) && PX4_ISFINITE(vel_sp_feedback(1))) { + if (vel_sp_feedback.isAllFinite()) { _velocity_setpoint = vel_sp_feedback; } } diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp index 84ab3ae15a..f43eff4fba 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp @@ -64,12 +64,7 @@ bool Sticks::checkAndUpdateStickInputs() _positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get()); // valid stick inputs are required - const bool valid_sticks = PX4_ISFINITE(_positions(0)) - && PX4_ISFINITE(_positions(1)) - && PX4_ISFINITE(_positions(2)) - && PX4_ISFINITE(_positions(3)); - - _input_available = valid_sticks; + _input_available = _positions.isAllFinite(); } else { failsafe_flags_s failsafe_flags; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 84e1b3017b..b353ad9437 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -2135,8 +2135,7 @@ FixedwingPositionControl::Run() _pos_sp_triplet.current.lon = static_cast(NAN); _pos_sp_triplet.current.alt = NAN; - if (PX4_ISFINITE(trajectory_setpoint.position[0]) && PX4_ISFINITE(trajectory_setpoint.position[1]) - && PX4_ISFINITE(trajectory_setpoint.position[2])) { + if (Vector3f(trajectory_setpoint.position).isAllFinite()) { if (_global_local_proj_ref.isInitialized()) { double lat; double lon; @@ -2150,16 +2149,14 @@ FixedwingPositionControl::Run() } - if (PX4_ISFINITE(trajectory_setpoint.velocity[0]) && PX4_ISFINITE(trajectory_setpoint.velocity[1]) - && PX4_ISFINITE(trajectory_setpoint.velocity[2])) { + if (Vector3f(trajectory_setpoint.velocity).isAllFinite()) { valid_setpoint = true; _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; _pos_sp_triplet.current.vx = trajectory_setpoint.velocity[0]; _pos_sp_triplet.current.vy = trajectory_setpoint.velocity[1]; _pos_sp_triplet.current.vz = trajectory_setpoint.velocity[2]; - if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1]) - && PX4_ISFINITE(trajectory_setpoint.acceleration[2])) { + if (Vector3f(trajectory_setpoint.acceleration).isAllFinite()) { Vector2f velocity_sp_2d(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); Vector2f normalized_velocity_sp_2d = velocity_sp_2d.normalized(); Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]); diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index d72ffd6a07..c43c3dfaa8 100644 --- a/src/modules/gimbal/output.cpp +++ b/src/modules/gimbal/output.cpp @@ -213,12 +213,12 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) float dt = math::constrain((t - _last_update) * 1.e-6f, 0.001f, 1.f); - const bool q_setpoint_valid = PX4_ISFINITE(_q_setpoint[0]) && PX4_ISFINITE(_q_setpoint[1]) - && PX4_ISFINITE(_q_setpoint[2]) && PX4_ISFINITE(_q_setpoint[3]); + const matrix::Quatf q_setpoint(_q_setpoint); + const bool q_setpoint_valid = q_setpoint.isAllFinite(); matrix::Eulerf euler_gimbal{}; if (q_setpoint_valid) { - euler_gimbal = matrix::Quatf{_q_setpoint}; + euler_gimbal = q_setpoint; } for (int i = 0; i < 3; ++i) { diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index ef22a14bcc..7fd5f15cfb 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -263,8 +263,7 @@ void LandingTargetEstimator::_update_topics() return; } - if (!PX4_ISFINITE((float)_uwbDistance.position[0]) || !PX4_ISFINITE((float)_uwbDistance.position[1]) || - !PX4_ISFINITE((float)_uwbDistance.position[2])) { + if (!matrix::Vector3f(_uwbDistance.position).isAllFinite()) { PX4_WARN("Position is corrupt!"); return; } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index bf85965c23..508a542754 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -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 &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; diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp index d8fc118b0e..4ef331a071 100644 --- a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp @@ -194,7 +194,7 @@ void MagBiasEstimator::Run() const Vector3f &bias = _bias_estimator[mag_index].getBias(); const Vector3f bias_rate = (bias - bias_prev) / dt; - if (!PX4_ISFINITE(bias(0)) || !PX4_ISFINITE(bias(1)) || !PX4_ISFINITE(bias(2)) || bias.longerThan(5.f)) { + if (!bias.isAllFinite() || bias.longerThan(5.f)) { _reset_field_estimator[mag_index] = true; _valid[mag_index] = false; _time_valid[mag_index] = 0; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0f396421a9..f07fe302e8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -792,10 +792,10 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) sensor_optical_flow.integration_timespan_us = flow.integration_time_us; sensor_optical_flow.quality = flow.quality; - if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) { - sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro; - sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro; - sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro; + const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro); + + if (integrated_gyro.isAllFinite()) { + integrated_gyro.copyTo(sensor_optical_flow.delta_angle); sensor_optical_flow.delta_angle_available = true; } @@ -838,10 +838,10 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) sensor_optical_flow.integration_timespan_us = flow.integration_time_us; sensor_optical_flow.quality = flow.quality; - if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) { - sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro; - sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro; - sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro; + const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro); + + if (integrated_gyro.isAllFinite()) { + integrated_gyro.copyTo(sensor_optical_flow.delta_angle); sensor_optical_flow.delta_angle_available = true; } @@ -1340,16 +1340,16 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) odom.timestamp_sample = _mavlink_timesync.sync_stamp(odom_in.time_usec); + const matrix::Vector3f odom_in_p(odom_in.x, odom_in.y, odom_in.z); + // position x/y/z (m) - if (PX4_ISFINITE(odom_in.x) && PX4_ISFINITE(odom_in.y) && PX4_ISFINITE(odom_in.z)) { + if (odom_in_p.isAllFinite()) { // frame_id: Coordinate frame of reference for the pose data. switch (odom_in.frame_id) { case MAV_FRAME_LOCAL_NED: // NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; - odom.position[0] = odom_in.x; - odom.position[1] = odom_in.y; - odom.position[2] = odom_in.z; + odom_in_p.copyTo(odom.position); break; case MAV_FRAME_LOCAL_ENU: @@ -1363,9 +1363,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) case MAV_FRAME_LOCAL_FRD: // FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD; - odom.position[0] = odom_in.x; - odom.position[1] = odom_in.y; - odom.position[2] = odom_in.z; + odom_in_p.copyTo(odom.position); break; case MAV_FRAME_LOCAL_FLU: @@ -1408,10 +1406,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) } // q: the quaternion of the ODOMETRY msg represents a rotation from body frame to a local frame - if (PX4_ISFINITE(odom_in.q[0]) - && PX4_ISFINITE(odom_in.q[1]) - && PX4_ISFINITE(odom_in.q[2]) - && PX4_ISFINITE(odom_in.q[3])) { + if (matrix::Quatf(odom_in.q).isAllFinite()) { odom.q[0] = odom_in.q[0]; odom.q[1] = odom_in.q[1]; @@ -1428,16 +1423,16 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) } } + const matrix::Vector3f odom_in_v(odom_in.vx, odom_in.vy, odom_in.vz); + // velocity vx/vy/vz (m/s) - if (PX4_ISFINITE(odom_in.vx) && PX4_ISFINITE(odom_in.vy) && PX4_ISFINITE(odom_in.vz)) { + if (odom_in_v.isAllFinite()) { // child_frame_id: Coordinate frame of reference for the velocity in free space (twist) data. switch (odom_in.child_frame_id) { case MAV_FRAME_LOCAL_NED: // NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED; - odom.velocity[0] = odom_in.vx; - odom.velocity[1] = odom_in.vy; - odom.velocity[2] = odom_in.vz; + odom_in_v.copyTo(odom.velocity); break; case MAV_FRAME_LOCAL_ENU: @@ -1451,9 +1446,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) case MAV_FRAME_LOCAL_FRD: // FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD; - odom.velocity[0] = odom_in.vx; - odom.velocity[1] = odom_in.vy; - odom.velocity[2] = odom_in.vz; + odom_in_v.copyTo(odom.velocity); break; case MAV_FRAME_LOCAL_FLU: diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 4744e7e4d7..039bef28fd 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -268,15 +268,14 @@ PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehic { PositionControlStates states; + const Vector2f position_xy(vehicle_local_position.x, vehicle_local_position.y); + // only set position states if valid and finite - if (PX4_ISFINITE(vehicle_local_position.x) && PX4_ISFINITE(vehicle_local_position.y) - && vehicle_local_position.xy_valid) { - states.position(0) = vehicle_local_position.x; - states.position(1) = vehicle_local_position.y; + if (vehicle_local_position.xy_valid && position_xy.isAllFinite()) { + states.position.xy() = position_xy; } else { - states.position(0) = NAN; - states.position(1) = NAN; + states.position(0) = states.position(1) = NAN; } if (PX4_ISFINITE(vehicle_local_position.z) && vehicle_local_position.z_valid) { @@ -286,18 +285,16 @@ PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehic states.position(2) = NAN; } - if (PX4_ISFINITE(vehicle_local_position.vx) && PX4_ISFINITE(vehicle_local_position.vy) - && vehicle_local_position.v_xy_valid) { - states.velocity(0) = vehicle_local_position.vx; - states.velocity(1) = vehicle_local_position.vy; - states.acceleration(0) = _vel_x_deriv.update(vehicle_local_position.vx); - states.acceleration(1) = _vel_y_deriv.update(vehicle_local_position.vy); + const Vector2f velocity_xy(vehicle_local_position.vx, vehicle_local_position.vy); + + if (vehicle_local_position.v_xy_valid && velocity_xy.isAllFinite()) { + states.velocity.xy() = velocity_xy; + states.acceleration(0) = _vel_x_deriv.update(velocity_xy(0)); + states.acceleration(1) = _vel_y_deriv.update(velocity_xy(1)); } else { - states.velocity(0) = NAN; - states.velocity(1) = NAN; - states.acceleration(0) = NAN; - states.acceleration(1) = NAN; + states.velocity(0) = states.velocity(1) = NAN; + states.acceleration(0) = states.acceleration(1) = NAN; // reset derivatives to prevent acceleration spikes when regaining velocity _vel_x_deriv.reset(); @@ -599,7 +596,7 @@ trajectory_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const trajectory_setpoint_s failsafe_setpoint = PositionControl::empty_trajectory_setpoint; failsafe_setpoint.timestamp = now; - if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) { + if (Vector2f(states.velocity).isAllFinite()) { // don't move along xy failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = 0.f; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 5abbc2fbce..10aa105d06 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -117,10 +117,7 @@ bool PositionControl::update(const float dt) } // There has to be a valid output acceleration and thrust setpoint otherwise something went wrong - valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2)); - valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2)); - - return valid; + return valid && _acc_sp.isAllFinite() && _thr_sp.isAllFinite(); } void PositionControl::_positionControl() diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index c2507eb20f..af2f948f3a 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -234,7 +234,7 @@ void VehicleAcceleration::Run() while (_sensor_sub.update(&sensor_data)) { const Vector3f accel_raw{sensor_data.x, sensor_data.y, sensor_data.z}; - if (math::isFinite(accel_raw)) { + if (accel_raw.isAllFinite()) { // Apply calibration and filter // - calibration offsets, scale factors, and thermal scale (if available) // - estimated in run bias (if available) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index a49b32d7d2..f5e98a5c25 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -499,10 +499,7 @@ Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const // start with last valid vehicle body frame angular velocity and compute equivalent raw data (for current sensor selection) Vector3f angular_velocity_uncalibrated{_calibration.Uncorrect(_angular_velocity + _bias)}; - if (PX4_ISFINITE(angular_velocity_uncalibrated(0)) - && PX4_ISFINITE(angular_velocity_uncalibrated(1)) - && PX4_ISFINITE(angular_velocity_uncalibrated(2))) { - + if (angular_velocity_uncalibrated.isAllFinite()) { return angular_velocity_uncalibrated; } } @@ -517,10 +514,7 @@ Vector3f VehicleAngularVelocity::GetResetAngularAcceleration() const // start with last valid vehicle body frame angular acceleration and compute equivalent raw data (for current sensor selection) Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration}; - if (PX4_ISFINITE(angular_acceleration(0)) - && PX4_ISFINITE(angular_acceleration(1)) - && PX4_ISFINITE(angular_acceleration(2))) { - + if (angular_acceleration.isAllFinite()) { return angular_acceleration; } } @@ -849,7 +843,7 @@ void VehicleAngularVelocity::Run() sensor_gyro_s sensor_data; while (_sensor_sub.update(&sensor_data)) { - if (PX4_ISFINITE(sensor_data.x) && PX4_ISFINITE(sensor_data.y) && PX4_ISFINITE(sensor_data.z)) { + if (Vector3f(sensor_data.x, sensor_data.y, sensor_data.z).isAllFinite()) { if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) { _timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz; diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp index 371ae74661..87f356781e 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -125,11 +125,7 @@ void VehicleOpticalFlow::Run() // delta angle // - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available - if (sensor_optical_flow.delta_angle_available - && PX4_ISFINITE(sensor_optical_flow.delta_angle[0]) - && PX4_ISFINITE(sensor_optical_flow.delta_angle[1]) - && PX4_ISFINITE(sensor_optical_flow.delta_angle[2]) - ) { + if (sensor_optical_flow.delta_angle_available && Vector3f(sensor_optical_flow.delta_angle).isAllFinite()) { // passthrough integrated gyro if available _delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle}; _delta_angle_available = true; diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 450b782848..1517d7ed5a 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -634,15 +634,15 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg) odom.timestamp_sample = hrt_absolute_time(); // _mavlink_timesync.sync_stamp(odom_in.time_usec); // position x/y/z (m) - if (PX4_ISFINITE(odom_in.x) && PX4_ISFINITE(odom_in.y) && PX4_ISFINITE(odom_in.z)) { + matrix::Vector3f odom_in_p(odom_in.x, odom_in.y, odom_in.z); + + if (odom_in_p.isAllFinite()) { // frame_id: Coordinate frame of reference for the pose data. switch (odom_in.frame_id) { case MAV_FRAME_LOCAL_NED: // NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; - odom.position[0] = odom_in.x; - odom.position[1] = odom_in.y; - odom.position[2] = odom_in.z; + odom_in_p.copyTo(odom.position); break; case MAV_FRAME_LOCAL_ENU: @@ -656,9 +656,7 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg) case MAV_FRAME_LOCAL_FRD: // FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD; - odom.position[0] = odom_in.x; - odom.position[1] = odom_in.y; - odom.position[2] = odom_in.z; + odom_in_p.copyTo(odom.position); break; case MAV_FRAME_LOCAL_FLU: @@ -701,11 +699,7 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg) } // q: the quaternion of the ODOMETRY msg represents a rotation from body frame to a local frame - if (PX4_ISFINITE(odom_in.q[0]) - && PX4_ISFINITE(odom_in.q[1]) - && PX4_ISFINITE(odom_in.q[2]) - && PX4_ISFINITE(odom_in.q[3])) { - + if (matrix::Quatf(odom_in.q).isAllFinite()) { odom.q[0] = odom_in.q[0]; odom.q[1] = odom_in.q[1]; odom.q[2] = odom_in.q[2]; @@ -722,15 +716,15 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg) } // velocity vx/vy/vz (m/s) - if (PX4_ISFINITE(odom_in.vx) && PX4_ISFINITE(odom_in.vy) && PX4_ISFINITE(odom_in.vz)) { + matrix::Vector3f odom_in_v(odom_in.vx, odom_in.vy, odom_in.vz); + + if (odom_in_v.isAllFinite()) { // child_frame_id: Coordinate frame of reference for the velocity in free space (twist) data. switch (odom_in.child_frame_id) { case MAV_FRAME_LOCAL_NED: // NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED; - odom.velocity[0] = odom_in.vx; - odom.velocity[1] = odom_in.vy; - odom.velocity[2] = odom_in.vz; + odom_in_v.copyTo(odom.velocity); break; case MAV_FRAME_LOCAL_ENU: @@ -744,9 +738,7 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg) case MAV_FRAME_LOCAL_FRD: // FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD; - odom.velocity[0] = odom_in.vx; - odom.velocity[1] = odom_in.vy; - odom.velocity[2] = odom_in.vz; + odom_in_v.copyTo(odom.velocity); break; case MAV_FRAME_LOCAL_FLU: @@ -762,9 +754,7 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg) case MAV_FRAME_BODY_FRD: // FRD local tangent frame (x: Forward, y: Right, z: Down) with origin that travels with vehicle. odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD; - odom.velocity[0] = odom_in.vx; - odom.velocity[1] = odom_in.vy; - odom.velocity[2] = odom_in.vz; + odom_in_v.copyTo(odom.velocity); break; default: @@ -862,10 +852,10 @@ void SimulatorMavlink::handle_message_optical_flow(const mavlink_message_t *msg) sensor_optical_flow.integration_timespan_us = flow.integration_time_us; sensor_optical_flow.quality = flow.quality; - if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) { - sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro; - sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro; - sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro; + matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro); + + if (integrated_gyro.isAllFinite()) { + integrated_gyro.copyTo(sensor_optical_flow.delta_angle); sensor_optical_flow.delta_angle_available = true; }