create new mag_bias_estimator module

Co-authored-by: Daniel Agar <daniel@agar.ca>

msg/CMakeLists.txt

@@ -98,6 +98,7 @@ set(msg_files
 	log_message.msg
 	logger_status.msg
 	mag_worker_data.msg
+	magnetometer_bias_estimate.msg
 	manual_control_setpoint.msg
 	manual_control_switches.msg
 	mavlink_log.msg

msg/magnetometer_bias_estimate.msg

@@ -0,0 +1,7 @@
+uint64 timestamp                # time since system start (microseconds)
+
+float32[4] bias_x		# estimated X-bias of all the sensors
+float32[4] bias_y		# estimated Y-bias of all the sensors
+float32[4] bias_z		# estimated Z-bias of all the sensors
+
+bool[4] valid			# true if the estimator has converged

src/lib/CMakeLists.txt

@@ -45,12 +45,12 @@ add_subdirectory(controllib)
 add_subdirectory(conversion)
 add_subdirectory(crypto)
 add_subdirectory(drivers)
+add_subdirectory(field_sensor_bias_estimator)
 add_subdirectory(geo)
 add_subdirectory(hysteresis)
 add_subdirectory(l1)
 add_subdirectory(landing_slope)
 add_subdirectory(led)
-add_subdirectory(magnetometer_bias_estimator)
 add_subdirectory(mathlib)
 add_subdirectory(mixer)
 add_subdirectory(mixer_module)

src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp

@@ -59,6 +59,7 @@ public:
 	// Set initial states
 	void setField(const matrix::Vector3f &field) { _field_prev = field; }
 	void setBias(const matrix::Vector3f &bias) { _state_bias = bias; }
+	void setLearningGain(float learning_gain) { _learning_gain = learning_gain; }
 
 	/**
 	 * Update the estimator and extract updated biases.
@@ -70,7 +71,7 @@ public:
 	{
 		const matrix::Vector3f field_pred = _field_prev + (-gyro % (_field_prev - _state_bias)) * dt;
 		const matrix::Vector3f field_innov = field - field_pred;
-		_state_bias += GAIN_BIAS * (-gyro % field_innov);
+		_state_bias += _learning_gain * (-gyro % field_innov);
 		_field_prev = field;
 	}
 
@@ -78,10 +79,8 @@ public:
 	const matrix::Vector3f &getBias() { return _state_bias; }
 
 private:
-	// gains
-	static constexpr float GAIN_BIAS = 100.f;
-
 	// states
 	matrix::Vector3f _field_prev{};
 	matrix::Vector3f _state_bias{};
+	float _learning_gain{20.f};
 };

src/lib/field_sensor_bias_estimator/FieldSensorBiasEstimatorTest.cpp

@@ -31,14 +31,20 @@
  *
  ****************************************************************************/
 
+/**
+ * Test code for the Field Sensor Bias Estimator
+ * Run this test only using make tests TESTFILTER=FieldSensorBiasEstimator
+ */
+
 #include <gtest/gtest.h>
 #include <FieldSensorBiasEstimator.hpp>
 
 using namespace matrix;
 
-TEST(MagnetometerBiasEstimatorTest, AllZeroCase)
+TEST(MagnetometerBiasEstimatorTest, constantZRotation)
 {
 	FieldSensorBiasEstimator field_sensor_bias_estimator;
+	field_sensor_bias_estimator.setLearningGain(100.f);
 	const Vector3f virtual_gyro = Vector3f(0.f, 0.f, 0.1f);
 	Vector3f virtual_unbiased_mag = Vector3f(0.9f, 0.f, 1.79f); // taken from SITL jmavsim initialization
 	const Vector3f virtual_bias(0.2f, -0.4f, 0.5f);

src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.cpp

@@ -1,104 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file MagnetometerBiasEstimator.cpp
- */
-
-#include "MagnetometerBiasEstimator.hpp"
-
-#include <px4_posix.h>
-#include <drivers/drv_mag.h>
-#include <mathlib/mathlib.h>
-
-using namespace matrix;
-using namespace time_literals;
-
-MagnetometerBiasEstimator::MagnetometerBiasEstimator(const matrix::Dcmf &board_rotation) :
-	ModuleParams(nullptr),
-	_board_rotation(board_rotation)
-{}
-
-void MagnetometerBiasEstimator::extractBias(vehicle_magnetometer_s &mag_raw, const sensor_combined_s &gyro_raw)
-{
-	// fill in vectors
-	Vector3f gyro(gyro_raw.gyro_rad);
-	Vector3f mag(mag_raw.magnetometer_ga);
-
-	// prepare delta time in seconds
-	hrt_abstime time_stamp_current = hrt_absolute_time();
-	float dt  = math::min((time_stamp_current - _time_stamp_last_loop), hrt_abstime(500_ms)) / 1e6f;
-	_time_stamp_last_loop = time_stamp_current;
-
-	_field_sensor_bias_estimator.updateEstimate(gyro, mag, dt);
-	const Vector3f &bias = _field_sensor_bias_estimator.getBias();
-
-	// save the bias to the parameters to apply it given the right circumstances
-	const bool longer_than_10_sec = (time_stamp_current - _time_stamp_last_save) > hrt_abstime(10_s);
-	const bool bias_significant = bias.norm_squared() > (0.01f * 0.01f);
-
-	_actuator_armed_sub.update();
-
-	if (!_actuator_armed_sub.get().armed && bias_significant && longer_than_10_sec) {
-		saveBias(bias);
-		_time_stamp_last_save = time_stamp_current;
-	}
-
-	(mag - bias).copyTo(mag_raw.magnetometer_ga);
-}
-
-void MagnetometerBiasEstimator::saveBias(const matrix::Vector3f &bias)
-{
-	// estimated bias is in body frame, but the driver needs sensor frame
-	const Vector3f transformed_bias = _board_rotation.transpose() * bias;
-
-	// get current calibration
-	updateParams();
-	Vector3f calibration_bias(_param_cal_mag0_xoff.get(),
-				  _param_cal_mag0_yoff.get(),
-				  _param_cal_mag0_zoff.get());
-
-	// estimated bias comes on top of existing calibration
-	calibration_bias += transformed_bias;
-
-	// save new calibration
-	_param_cal_mag0_xoff.set(calibration_bias(0));
-	_param_cal_mag0_yoff.set(calibration_bias(1));
-	_param_cal_mag0_zoff.set(calibration_bias(2));
-	_param_cal_mag0_xoff.commit();
-	_param_cal_mag0_yoff.commit();
-	_param_cal_mag0_zoff.commit();
-
-	// reset internal bias to zero because from now the driver will correct
-	_field_sensor_bias_estimator.setBias(Vector3f());
-}

src/lib/magnetometer_bias_estimator/MagnetometerBiasEstimator.hpp

@@ -1,86 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file MagnetometerBiasEstimator.hpp
- *
- * Runs the FieldSensorBiasEstimator with PX4 magnetometer and gyroscope data and saves the result to the calibration parameters.
- *
- * @author Matthias Grob	<maetugr@gmail.com>
- */
-
-#pragma once
-
-#include <matrix/matrix/math.hpp>
-#include <lib/conversion/rotation.h>
-#include <px4_module_params.h>
-#include <drivers/drv_hrt.h>
-
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_magnetometer.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/actuator_armed.h>
-
-#include <FieldSensorBiasEstimator.hpp>
-
-class MagnetometerBiasEstimator : public ModuleParams
-{
-public:
-	MagnetometerBiasEstimator(const matrix::Dcmf &board_rotation);
-	~MagnetometerBiasEstimator() = default;
-
-	/**
-	 * Update the estimator and extract updated magnetometer biases.
-	 * @param mag_raw struct containing the magnetometer data to operate on (gets adjusted with current estimate)
-	 * @param raw struct containing the gyroscope data to use
-	 */
-	void extractBias(vehicle_magnetometer_s &mag_raw, const sensor_combined_s &gyro_raw);
-
-private:
-	void updateEstimate(const matrix::Vector3f &gyro, const matrix::Vector3f &mag, const float dt);
-	void saveBias(const matrix::Vector3f &bias);
-
-	FieldSensorBiasEstimator _field_sensor_bias_estimator{};
-	const matrix::Dcmf &_board_rotation;
-
-	hrt_abstime _time_stamp_last_loop{0};
-	hrt_abstime _time_stamp_last_save{0};
-
-	uORB::SubscriptionData<actuator_armed_s> _actuator_armed_sub{ORB_ID(actuator_armed)};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::CAL_MAG0_XOFF>) _param_cal_mag0_xoff,
-		(ParamFloat<px4::params::CAL_MAG0_YOFF>) _param_cal_mag0_yoff,
-		(ParamFloat<px4::params::CAL_MAG0_ZOFF>) _param_cal_mag0_zoff
-	)
-};

src/modules/logger/logged_topics.cpp

@@ -69,6 +69,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("hover_thrust_estimate", 100);
 	add_topic("input_rc", 500);
 	add_topic("internal_combustion_engine_status", 10);
+	add_topic("magnetometer_bias_estimate", 200);
 	add_topic("manual_control_setpoint", 200);
 	add_topic("manual_control_switches");
 	add_topic("mission_result");

src/modules/mag_bias_estimator/CMakeLists.txt

@@ -30,14 +30,14 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-
-add_subdirectory(FieldSensorBiasEstimator)
-
-px4_add_library(MagnetometerBiasEstimator
-	MagnetometerBiasEstimator.cpp
+px4_add_module(
+	MODULE modules__mag_bias_estimator
+	MAIN mag_bias_estimator
+	COMPILE_FLAGS
+		#-DDEBUG_BUILD
+	SRCS
+		MagBiasEstimator.cpp
+		MagBiasEstimator.hpp
+	DEPENDS
+		px4_work_queue
 )
-target_include_directories(MagnetometerBiasEstimator
-	PUBLIC
-	${CMAKE_CURRENT_SOURCE_DIR}
-)
-target_link_libraries(MagnetometerBiasEstimator PUBLIC FieldSensorBiasEstimator)

src/modules/mag_bias_estimator/MagBiasEstimator.cpp

@@ -0,0 +1,275 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "MagBiasEstimator.hpp"
+
+using namespace time_literals;
+using matrix::Vector3f;
+
+namespace mag_bias_estimator
+{
+
+MagBiasEstimator::MagBiasEstimator() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+}
+
+MagBiasEstimator::~MagBiasEstimator()
+{
+	perf_free(_cycle_perf);
+}
+
+int MagBiasEstimator::task_spawn(int argc, char *argv[])
+{
+	MagBiasEstimator *obj = new MagBiasEstimator();
+
+	if (!obj) {
+		PX4_ERR("alloc failed");
+		return -1;
+	}
+
+	_object.store(obj);
+	_task_id = task_id_is_work_queue;
+
+	/* Schedule a cycle to start things. */
+	obj->start();
+
+	return 0;
+}
+
+void MagBiasEstimator::start()
+{
+	ScheduleOnInterval(20_ms); // 50 Hz
+}
+
+void MagBiasEstimator::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			if (_arming_state != vehicle_status.arming_state) {
+				_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+
+				// reset on any arming state change
+				for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
+					_reset_field_estimator[mag_index] = true;
+				}
+
+				_arming_state = vehicle_status.arming_state;
+			}
+		}
+	}
+
+	// only run when disarmed
+	if (_armed) {
+		return;
+	}
+
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+
+		for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
+			const auto calibration_count = _calibration[mag_index].calibration_count();
+			_calibration[mag_index].ParametersUpdate();
+
+			if (calibration_count != _calibration[mag_index].calibration_count()) {
+				_reset_field_estimator[mag_index] = true;
+			}
+
+			_bias_estimator[mag_index].setLearningGain(_param_mbe_learn_gain.get());
+		}
+	}
+
+	if (_vehicle_status_flags_sub.updated()) {
+		vehicle_status_flags_s vehicle_status_flags;
+
+		if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) {
+
+			// do nothing during regular sensor calibration
+			_system_calibrating = vehicle_status_flags.condition_calibration_enabled;
+			_system_sensors_initialized = vehicle_status_flags.condition_system_sensors_initialized
+						      && vehicle_status_flags.condition_system_hotplug_timeout;
+		}
+	}
+
+	if (_system_calibrating || !_system_sensors_initialized) {
+		return;
+	}
+
+	perf_begin(_cycle_perf);
+
+	Vector3f angular_velocity{};
+
+	{
+		// Assume a constant angular velocity during two mag samples
+		vehicle_angular_velocity_s vehicle_angular_velocity;
+
+		if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
+			angular_velocity = Vector3f{vehicle_angular_velocity.xyz};
+		}
+	}
+
+	for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
+		sensor_mag_s sensor_mag;
+
+		while (_sensor_mag_subs[mag_index].update(&sensor_mag)) {
+
+			// apply existing mag calibration
+			_calibration[mag_index].set_device_id(sensor_mag.device_id, sensor_mag.is_external);
+			const Vector3f raw_mag{sensor_mag.x, sensor_mag.y, sensor_mag.z};
+			const Vector3f mag_calibrated = _calibration[mag_index].Correct(raw_mag);
+
+			float dt = (sensor_mag.timestamp_sample - _timestamp_last_update[mag_index]) * 1e-6f;
+			_timestamp_last_update[mag_index] = sensor_mag.timestamp_sample;
+
+			if (dt < 0.001f || dt > 0.2f) {
+				_reset_field_estimator[mag_index] = true;
+			}
+
+			if (_reset_field_estimator[mag_index]) {
+				// reset
+				_bias_estimator[mag_index].setBias(Vector3f{});
+				_bias_estimator[mag_index].setField(mag_calibrated);
+
+				_reset_field_estimator[mag_index] = false;
+				_valid[mag_index] = false;
+
+			} else {
+				const Vector3f bias_prev = _bias_estimator[mag_index].getBias();
+
+				_bias_estimator[mag_index].updateEstimate(angular_velocity, mag_calibrated, dt);
+
+				const Vector3f &bias = _bias_estimator[mag_index].getBias();
+				const Vector3f bias_rate = (bias - bias_prev) / dt;
+
+				Vector3f fitness;
+				fitness(0) = fabsf(angular_velocity(0)) / fmaxf(fabsf(bias_rate(1)) + fabsf(bias_rate(2)), 0.02f);
+				fitness(1) = fabsf(angular_velocity(1)) / fmaxf(fabsf(bias_rate(0)) + fabsf(bias_rate(2)), 0.02f);
+				fitness(2) = fabsf(angular_velocity(2)) / fmaxf(fabsf(bias_rate(0)) + fabsf(bias_rate(1)), 0.02f);
+
+				if (!PX4_ISFINITE(bias(0)) || !PX4_ISFINITE(bias(1)) || !PX4_ISFINITE(bias(2)) || bias.longerThan(5.f)) {
+					_reset_field_estimator[mag_index] = true;
+					_valid[mag_index] = false;
+
+				} else {
+					const bool bias_significant = bias.longerThan(0.04f);
+					const bool has_converged = fitness(0) > 20.f || fitness(1) > 20.f || fitness(2) > 20.f;
+
+					if (bias_significant && has_converged) {
+						_valid[mag_index] = true;
+					}
+				}
+			}
+		}
+	}
+
+	publishMagBiasEstimate();
+
+	perf_end(_cycle_perf);
+}
+
+void MagBiasEstimator::publishMagBiasEstimate()
+{
+	magnetometer_bias_estimate_s mag_bias_est{};
+
+	for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
+		const Vector3f &bias = _bias_estimator[mag_index].getBias();
+
+		mag_bias_est.timestamp = hrt_absolute_time();
+		mag_bias_est.bias_x[mag_index] = bias(0);
+		mag_bias_est.bias_y[mag_index] = bias(1);
+		mag_bias_est.bias_z[mag_index] = bias(2);
+		mag_bias_est.valid[mag_index] = _valid[mag_index];
+	}
+
+
+	_magnetometer_bias_estimate_pub.publish(mag_bias_est);
+}
+
+int MagBiasEstimator::print_status()
+{
+	for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
+		if (_calibration[mag_index].device_id() != 0) {
+
+			_calibration[mag_index].PrintStatus();
+
+			const Vector3f &bias = _bias_estimator[mag_index].getBias();
+
+			PX4_INFO("%d (%" PRIu32 ") bias: [% 05.3f % 05.3f % 05.3f]",
+				 mag_index, _calibration[mag_index].device_id(),
+				 (double)bias(0),
+				 (double)bias(1),
+				 (double)bias(2));
+		}
+	}
+
+	return 0;
+}
+
+int MagBiasEstimator::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Online magnetometer bias estimator.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("mag_bias_estimator", "system");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int mag_bias_estimator_main(int argc, char *argv[])
+{
+	return MagBiasEstimator::main(argc, argv);
+}
+
+} // namespace load_mon

src/modules/mag_bias_estimator/MagBiasEstimator.hpp

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/sensor_calibration/Magnetometer.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/magnetometer_bias_estimate.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_mag.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_status_flags.h>
+
+namespace mag_bias_estimator
+{
+
+class MagBiasEstimator : public ModuleBase<MagBiasEstimator>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	MagBiasEstimator();
+	~MagBiasEstimator() override;
+
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[])
+	{
+		return print_usage("unknown command");
+	}
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	void start();
+
+private:
+	void Run() override;
+	void publishMagBiasEstimate();
+
+	static constexpr int MAX_SENSOR_COUNT = 4;
+
+	FieldSensorBiasEstimator _bias_estimator[MAX_SENSOR_COUNT];
+	hrt_abstime _timestamp_last_update[MAX_SENSOR_COUNT] {};
+
+	uORB::SubscriptionMultiArray<sensor_mag_s, MAX_SENSOR_COUNT> _sensor_mag_subs{ORB_ID::sensor_mag};
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
+
+	uORB::Publication<magnetometer_bias_estimate_s> _magnetometer_bias_estimate_pub{ORB_ID(magnetometer_bias_estimate)};
+
+	calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
+
+	bool _reset_field_estimator[MAX_SENSOR_COUNT] {};
+	bool _valid[MAX_SENSOR_COUNT] {};
+
+	bool _armed{false};
+	bool _system_calibrating{false};
+	bool _system_sensors_initialized{false};
+	uint8_t _arming_state{0};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MBE_LEARN_GAIN>) _param_mbe_learn_gain
+	)
+};
+
+} // namespace mag_bias_estimator

src/modules/mag_bias_estimator/params.c

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Enable online mag bias calibration
+ *
+ * This enables continuous calibration of the magnetometers
+ * before takeoff using gyro data.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group Magnetometer Bias Estimator
+ */
+PARAM_DEFINE_INT32(MBE_ENABLE, 1);
+
+/**
+ * Mag bias estimator learning gain
+ *
+ * Increase to make the estimator more responsive
+ * Decrease to make the estimator more robust to noise
+ *
+ * @min 0.1
+ * @max 100
+ * @increment 0.1
+ * @decimal 1
+ * @group Magnetometer Bias Estimator
+ */
+PARAM_DEFINE_FLOAT(MBE_LEARN_GAIN, 0.4f);