fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 05:10: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/lib/sensor_calibration/Magnetometer.cpp
+3 -5
View File
@@ -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);