fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 12:43:33 +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/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+1 -2
View File
@@ -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;
src/modules/commander/accelerometer_calibration.cpp
+1 -5
View File
@@ -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 {
src/modules/commander/gyro_calibration.cpp
+1 -3
View File
@@ -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..");