From 2190f66096a3df847114beb82ade1e1ce67d2344 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 18 Oct 2021 20:28:44 -0400
Subject: [PATCH] sensors/vehicle_magnetometer: add SENS_MAG_AUTOCAL to enable
 initial auto cal

---
 src/modules/sensors/sensor_params_mag.c              | 12 ++++++++++++
 .../vehicle_magnetometer/VehicleMagnetometer.cpp     |  2 +-
 .../vehicle_magnetometer/VehicleMagnetometer.hpp     |  3 ++-
 3 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/src/modules/sensors/sensor_params_mag.c b/src/modules/sensors/sensor_params_mag.c
index 091e37b1da..929aa97e57 100644
--- a/src/modules/sensors/sensor_params_mag.c
+++ b/src/modules/sensors/sensor_params_mag.c
@@ -105,3 +105,15 @@ PARAM_DEFINE_FLOAT(SENS_MAG_RATE, 50.0f);
  * @group Sensors
  */
 PARAM_DEFINE_INT32(SENS_MAG_MODE, 1);
+
+/**
+ * Magnetometer auto calibration
+ *
+ * Automatically initialize magnetometer calibration from bias estimate if available.
+ *
+ * @boolean
+ *
+ * @category system
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(SENS_MAG_AUTOCAL, 0);
diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
index 2e9cbea69b..68e2361661 100644
--- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
@@ -183,7 +183,7 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
 					_calibration_estimator_bias[mag_index] = bias;
 
 					// set initial mag calibration if disarmed, mag uncalibrated, and valid estimated bias available
-					if (!_armed && mag_bias_est.stable[mag_index]
+					if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index]
 					    && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) {
 
 						if (_calibration[mag_index].calibration_index() < 0) {
diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
index 62f4dbfa9a..90486dee74 100644
--- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
+++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
@@ -171,7 +171,8 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
 		(ParamBool<px4::params::SENS_MAG_MODE>) _param_sens_mag_mode,
-		(ParamFloat<px4::params::SENS_MAG_RATE>) _param_sens_mag_rate
+		(ParamFloat<px4::params::SENS_MAG_RATE>) _param_sens_mag_rate,
+		(ParamBool<px4::params::SENS_MAG_AUTOCAL>) _param_sens_mag_autocal
 	)
 };
 }; // namespace sensors