fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 15:53: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/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+1 -1
View File
@@ -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)
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+3 -9
View File
@@ -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;
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
+1 -5
View File
@@ -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;