From 712353ea56d4512a811236978231dd1d5a019677 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 24 Dec 2021 15:07:29 -0500
Subject: [PATCH] gyro_calibration: increase required samples (to be more
 conservative)

 - make sure variance is checked across all available sensors
---
 src/modules/gyro_calibration/CMakeLists.txt    |  1 +
 .../gyro_calibration/GyroCalibration.cpp       | 18 +++++++++++++++---
 2 files changed, 16 insertions(+), 3 deletions(-)

diff --git a/src/modules/gyro_calibration/CMakeLists.txt b/src/modules/gyro_calibration/CMakeLists.txt
index 5148eab2ce..f8f73f1df0 100644
--- a/src/modules/gyro_calibration/CMakeLists.txt
+++ b/src/modules/gyro_calibration/CMakeLists.txt
@@ -35,6 +35,7 @@ px4_add_module(
 	MODULE modules__gyro_calibration
 	MAIN gyro_calibration
 	COMPILE_FLAGS
+		#-DDEBUG_BUILD
 	SRCS
 		GyroCalibration.cpp
 		GyroCalibration.hpp
diff --git a/src/modules/gyro_calibration/GyroCalibration.cpp b/src/modules/gyro_calibration/GyroCalibration.cpp
index 169652a3e9..a1d74d1f3d 100644
--- a/src/modules/gyro_calibration/GyroCalibration.cpp
+++ b/src/modules/gyro_calibration/GyroCalibration.cpp
@@ -200,21 +200,23 @@ void GyroCalibration::Run()
 	bool sufficient_samples = false;
 
 	for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
-		if (_gyro_calibration[gyro].device_id() != 0) {
+		if ((_gyro_calibration[gyro].device_id() != 0) && _gyro_mean[gyro].valid()) {
 			// periodically check variance
-			if ((_gyro_mean[gyro].count() % 100 == 0)) {
+			if (_gyro_mean[gyro].count() % 100 == 0) {
 				PX4_DEBUG("gyro %d (%" PRIu32 ") variance, [%.9f, %.9f, %.9f] %.9f", gyro, _gyro_calibration[gyro].device_id(),
 					  (double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2),
 					  (double)_gyro_mean[gyro].variance().length());
 
 				if (_gyro_mean[gyro].variance().longerThan(0.001f)) {
 					// reset all
+					PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all",
+						  gyro, (double)_gyro_mean[gyro].variance().length());
 					Reset();
 					return;
 				}
 			}
 
-			if (_gyro_mean[gyro].count() > 3000) {
+			if (_gyro_mean[gyro].count() > 5000) {
 				sufficient_samples = true;
 
 			} else {
@@ -231,6 +233,16 @@ void GyroCalibration::Run()
 
 		for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
 			if (_gyro_calibration[gyro].device_id() != 0) {
+
+				// check variance again before saving
+				if (_gyro_mean[gyro].variance().longerThan(0.001f)) {
+					// reset all
+					PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all",
+						  gyro, (double)_gyro_mean[gyro].variance().length());
+					Reset();
+					return;
+				}
+
 				_gyro_calibration[gyro].set_calibration_index(gyro);
 
 				const Vector3f old_offset{_gyro_calibration[gyro].offset()};