From bd1c575ce86bb0dcc6fb3a46e466684b564ff43b Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 9 Mar 2021 09:27:21 -0500
Subject: [PATCH] lib/sensor_calibration: add simple offset and scale sanity
 checks

---
 src/lib/sensor_calibration/Accelerometer.cpp | 18 ++++----
 src/lib/sensor_calibration/Gyroscope.cpp     |  9 ++--
 src/lib/sensor_calibration/Magnetometer.cpp  | 44 ++++++++++++--------
 3 files changed, 42 insertions(+), 29 deletions(-)

diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp
index 107ab51307..32fc62b53e 100644
--- a/src/lib/sensor_calibration/Accelerometer.cpp
+++ b/src/lib/sensor_calibration/Accelerometer.cpp
@@ -126,10 +126,11 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
 bool Accelerometer::set_offset(const Vector3f &offset)
 {
 	if (Vector3f(_offset - offset).longerThan(0.01f)) {
-		_offset = offset;
-
-		_calibration_count++;
-		return true;
+		if (PX4_ISFINITE(offset(0)) && PX4_ISFINITE(offset(1)) && PX4_ISFINITE(offset(2))) {
+			_offset = offset;
+			_calibration_count++;
+			return true;
+		}
 	}
 
 	return false;
@@ -138,10 +139,13 @@ bool Accelerometer::set_offset(const Vector3f &offset)
 bool Accelerometer::set_scale(const Vector3f &scale)
 {
 	if (Vector3f(_scale - scale).longerThan(0.01f)) {
-		_scale = scale;
+		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))) {
 
-		_calibration_count++;
-		return true;
+			_scale = scale;
+			_calibration_count++;
+			return true;
+		}
 	}
 
 	return false;
diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp
index a1326101eb..a198bda9f1 100644
--- a/src/lib/sensor_calibration/Gyroscope.cpp
+++ b/src/lib/sensor_calibration/Gyroscope.cpp
@@ -126,10 +126,11 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
 bool Gyroscope::set_offset(const Vector3f &offset)
 {
 	if (Vector3f(_offset - offset).longerThan(0.01f)) {
-		_offset = offset;
-
-		_calibration_count++;
-		return true;
+		if (PX4_ISFINITE(offset(0)) && PX4_ISFINITE(offset(1)) && PX4_ISFINITE(offset(2))) {
+			_offset = offset;
+			_calibration_count++;
+			return true;
+		}
 	}
 
 	return false;
diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp
index 4c23721902..57fc73c244 100644
--- a/src/lib/sensor_calibration/Magnetometer.cpp
+++ b/src/lib/sensor_calibration/Magnetometer.cpp
@@ -86,10 +86,11 @@ void Magnetometer::set_external(bool external)
 bool Magnetometer::set_offset(const Vector3f &offset)
 {
 	if (Vector3f(_offset - offset).longerThan(0.01f)) {
-		_offset = offset;
-
-		_calibration_count++;
-		return true;
+		if (PX4_ISFINITE(offset(0)) && PX4_ISFINITE(offset(1)) && PX4_ISFINITE(offset(2))) {
+			_offset = offset;
+			_calibration_count++;
+			return true;
+		}
 	}
 
 	return false;
@@ -98,12 +99,16 @@ bool Magnetometer::set_offset(const Vector3f &offset)
 bool Magnetometer::set_scale(const Vector3f &scale)
 {
 	if (Vector3f(_scale.diag() - scale).longerThan(0.01f)) {
-		_scale(0, 0) = scale(0);
-		_scale(1, 1) = scale(1);
-		_scale(2, 2) = scale(2);
+		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))) {
 
-		_calibration_count++;
-		return true;
+			_scale(0, 0) = scale(0);
+			_scale(1, 1) = scale(1);
+			_scale(2, 2) = scale(2);
+
+			_calibration_count++;
+			return true;
+		}
 	}
 
 	return false;
@@ -112,17 +117,20 @@ 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)) {
-		_scale(0, 1) = offdiagonal(0);
-		_scale(1, 0) = offdiagonal(0);
+		if (PX4_ISFINITE(offdiagonal(0)) && PX4_ISFINITE(offdiagonal(1)) && PX4_ISFINITE(offdiagonal(2))) {
 
-		_scale(0, 2) = offdiagonal(1);
-		_scale(2, 0) = offdiagonal(1);
+			_scale(0, 1) = offdiagonal(0);
+			_scale(1, 0) = offdiagonal(0);
 
-		_scale(1, 2) = offdiagonal(2);
-		_scale(2, 1) = offdiagonal(2);
+			_scale(0, 2) = offdiagonal(1);
+			_scale(2, 0) = offdiagonal(1);
 
-		_calibration_count++;
-		return true;
+			_scale(1, 2) = offdiagonal(2);
+			_scale(2, 1) = offdiagonal(2);
+
+			_calibration_count++;
+			return true;
+		}
 	}
 
 	return false;
@@ -275,7 +283,7 @@ void Magnetometer::PrintStatus()
 	}
 
 #if defined(DEBUG_BUILD)
-	_scale.print()
+	_scale.print();
 #endif // DEBUG_BUILD
 }