add library for current- or thrust-based mag compensation

src/lib/CMakeLists.txt

@@ -62,3 +62,4 @@ add_subdirectory(terrain_estimation)
 add_subdirectory(tunes)
 add_subdirectory(version)
 add_subdirectory(weather_vane)
+add_subdirectory(mag_compensation)

src/lib/mag_compensation/CMakeLists.txt

@@ -0,0 +1,34 @@
+############################################################################
+#
+#   Copyright (c) 2019 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.
+#
+############################################################################
+
+px4_add_library(mag_compensation MagCompensation.cpp)

src/lib/mag_compensation/MagCompensation.cpp

@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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 MagCompensation.cpp
+ *
+ */
+
+#include "MagCompensation.hpp"
+#include <mathlib/mathlib.h>
+
+MagCompensator::MagCompensator(ModuleParams *parent):
+	ModuleParams(parent)
+{
+
+}
+
+void MagCompensator::calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f &param_vect)
+{
+	if (_armed) {
+		int type = _param_mag_compensation_type.get();
+
+		if (type == CompensationType::THROTTLE) {
+			mag = mag + param_vect * _throttle; //for param [gauss]
+
+		} else if (type == CompensationType::CURRENT) {
+			mag = mag + param_vect * _battery_current * 0.001; //for param [gauss/kA]
+		}
+	}
+}

src/lib/mag_compensation/MagCompensation.hpp

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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 MagCompensation.hpp
+ * @author Roman Bapst <roman@auterion.com>
+ *
+ *  Library for magnetometer data compensation.
+ *
+ */
+
+#pragma once
+
+#include <px4_platform_common/module_params.h>
+#include <matrix/matrix/math.hpp>
+
+class MagCompensator : public ModuleParams
+{
+public:
+	MagCompensator(ModuleParams *parent);
+
+	~MagCompensator() = default;
+
+	void update_armed_flag(bool armed) { _armed = armed; }
+
+	void update_throttle(float throttle) { _throttle = throttle; }
+
+	void update_current(float battery_current) { _battery_current = battery_current; }
+
+	void calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f &param_vect);
+
+private:
+
+	enum CompensationType {
+		DISABLED = 0,
+		THROTTLE,
+		CURRENT
+	};
+
+	float _throttle{0};
+	float _battery_current{0};
+	bool _armed{false};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_compensation_type
+	)
+
+};

src/lib/mag_compensation/mag_compensation_params.c

@@ -0,0 +1,43 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+/**
+ * Type of magnetometer compensation
+ *
+ * @value 0 Disabled
+ * @value 1 Throttle-based compensation
+ * @value 2 Current-based compensation
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0);

src/modules/sensors/CMakeLists.txt

@@ -57,4 +57,5 @@ px4_add_module(
 		vehicle_angular_velocity
 		vehicle_air_data
 		vehicle_imu
+		mag_compensation
 	)

src/modules/sensors/parameters.cpp

@@ -58,6 +58,22 @@ void initialize_parameter_handles(ParameterHandles &parameter_handles)
 	parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
 	parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
 
+	/* mag compensation */
+	parameter_handles.mag_comp_paramX[0] = param_find("CAL_MAG0_XCOMP");
+	parameter_handles.mag_comp_paramX[1] = param_find("CAL_MAG1_XCOMP");
+	parameter_handles.mag_comp_paramX[2] = param_find("CAL_MAG2_XCOMP");
+	parameter_handles.mag_comp_paramX[3] = param_find("CAL_MAG3_XCOMP");
+
+	parameter_handles.mag_comp_paramY[0] = param_find("CAL_MAG0_YCOMP");
+	parameter_handles.mag_comp_paramY[1] = param_find("CAL_MAG1_YCOMP");
+	parameter_handles.mag_comp_paramY[2] = param_find("CAL_MAG2_YCOMP");
+	parameter_handles.mag_comp_paramY[3] = param_find("CAL_MAG3_YCOMP");
+
+	parameter_handles.mag_comp_paramZ[0] = param_find("CAL_MAG0_ZCOMP");
+	parameter_handles.mag_comp_paramZ[1] = param_find("CAL_MAG1_ZCOMP");
+	parameter_handles.mag_comp_paramZ[2] = param_find("CAL_MAG2_ZCOMP");
+	parameter_handles.mag_comp_paramZ[3] = param_find("CAL_MAG3_ZCOMP");
+
 	parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
 	parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
 	parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
@@ -104,6 +120,21 @@ void update_parameters(const ParameterHandles &parameter_handles, Parameters &pa
 	param_get(parameter_handles.board_offset[1], &(parameters.board_offset[1]));
 	param_get(parameter_handles.board_offset[2], &(parameters.board_offset[2]));
 
+	param_get(parameter_handles.mag_comp_paramX[0], &(parameters.mag_comp_paramX[0]));
+	param_get(parameter_handles.mag_comp_paramX[1], &(parameters.mag_comp_paramX[1]));
+	param_get(parameter_handles.mag_comp_paramX[2], &(parameters.mag_comp_paramX[2]));
+	param_get(parameter_handles.mag_comp_paramX[3], &(parameters.mag_comp_paramX[3]));
+
+	param_get(parameter_handles.mag_comp_paramY[0], &(parameters.mag_comp_paramY[0]));
+	param_get(parameter_handles.mag_comp_paramY[1], &(parameters.mag_comp_paramY[1]));
+	param_get(parameter_handles.mag_comp_paramY[2], &(parameters.mag_comp_paramY[2]));
+	param_get(parameter_handles.mag_comp_paramY[3], &(parameters.mag_comp_paramY[3]));
+
+	param_get(parameter_handles.mag_comp_paramZ[0], &(parameters.mag_comp_paramZ[0]));
+	param_get(parameter_handles.mag_comp_paramZ[1], &(parameters.mag_comp_paramZ[1]));
+	param_get(parameter_handles.mag_comp_paramZ[2], &(parameters.mag_comp_paramZ[2]));
+	param_get(parameter_handles.mag_comp_paramZ[3], &(parameters.mag_comp_paramZ[3]));
+
 	param_get(parameter_handles.air_cmodel, &parameters.air_cmodel);
 	param_get(parameter_handles.air_tube_length, &parameters.air_tube_length);
 	param_get(parameter_handles.air_tube_diameter_mm, &parameters.air_tube_diameter_mm);

src/modules/sensors/parameters.h

@@ -56,6 +56,11 @@ struct Parameters {
 
 	float board_offset[3];
 
+	//parameters for current/throttle-based mag compensation
+	float mag_comp_paramX[4];
+	float mag_comp_paramY[4];
+	float mag_comp_paramZ[4];
+
 	int32_t air_cmodel;
 	float air_tube_length;
 	float air_tube_diameter_mm;
@@ -71,6 +76,10 @@ struct ParameterHandles {
 
 	param_t board_offset[3];
 
+	param_t mag_comp_paramX[4];
+	param_t mag_comp_paramY[4];
+	param_t mag_comp_paramZ[4];
+
 	param_t air_cmodel;
 	param_t air_tube_length;
 	param_t air_tube_diameter_mm;

src/modules/sensors/sensor_params_mag0.c

@@ -138,3 +138,39 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0f);
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0f);
+
+/**
+* Coefficient describing linear relationship between
+* X component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG0_XCOMP, 0.0f);
+
+/**
+* Coefficient describing linear relationship between
+* Y component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG0_YCOMP, 0.0f);
+
+/**
+* Coefficient describing linear relationship between
+* Z component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG0_ZCOMP, 0.0f);

src/modules/sensors/sensor_params_mag1.c

@@ -138,3 +138,39 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0f);
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0f);
+
+/**
+* Coefficient describing linear relationship between
+* X component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG1_XCOMP, 0.0f);
+
+/**
+* Coefficient describing linear relationship between
+* Y component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG1_YCOMP, 0.0f);
+
+/**
+* Coefficient describing linear relationship between
+* Z component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG1_ZCOMP, 0.0f);

src/modules/sensors/sensor_params_mag2.c

@@ -138,3 +138,39 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0f);
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0f);
+
+/**
+* Coefficient describing linear relationship between
+* X component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG2_XCOMP, 0.0f);
+
+/**
+* Coefficient describing linear relationship between
+* Y component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG2_YCOMP, 0.0f);
+
+/**
+* Coefficient describing linear relationship between
+* Z component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG2_ZCOMP, 0.0f);

src/modules/sensors/sensor_params_mag3.c

@@ -138,3 +138,39 @@ PARAM_DEFINE_FLOAT(CAL_MAG3_YSCALE, 1.0f);
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(CAL_MAG3_ZSCALE, 1.0f);
+
+/**
+* Coefficient describing linear relationship between
+* X component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG3_XCOMP, 0.0f);
+
+/**
+* Coefficient describing linear relationship between
+* Y component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG3_YCOMP, 0.0f);
+
+/**
+* Coefficient describing linear relationship between
+* Z component of magnetometer in body frame axis
+* and either current or throttle depending on value of CAL_MAG_COMP_TYP
+* Unit for throttle-based compensation is [G] and
+* for current-based compensation [G/kA]
+*
+* @category system
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(CAL_MAG3_ZCOMP, 0.0f);

src/modules/sensors/sensors.cpp

@@ -69,6 +69,7 @@
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_magnetometer.h>
+#include <uORB/topics/battery_status.h>
 
 #include "parameters.h"
 #include "voted_sensors_update.h"
@@ -124,6 +125,7 @@ private:
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};				/**< notification of parameter updates */
 	uORB::Subscription	_vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
 	uORB::Subscription	_vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+	uORB::Subscription	_battery_status_sub{ORB_ID(battery_status)};		/**< vehicle control mode subscription */
 
 	uORB::Publication<airspeed_s>			_airspeed_pub{ORB_ID(airspeed)};			/**< airspeed */
 	uORB::Publication<sensor_combined_s>		_sensor_pub{ORB_ID(sensor_combined)};			/**< combined sensor data topic */
@@ -464,6 +466,19 @@ void Sensors::Run()
 
 		if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
 			_armed = vcontrol_mode.flag_armed;
+			_voted_sensors_update.update_mag_comp_armed(_armed);
+		}
+
+		if (_actuator_ctrl_0_sub.updated()) {
+			actuator_controls_s controls {};
+			_actuator_ctrl_0_sub.copy(&controls);
+			_voted_sensors_update.update_mag_comp_throttle(controls.control[actuator_controls_s::INDEX_THROTTLE]);
+		}
+
+		if (_battery_status_sub.updated()) {
+			battery_status_s bat_stat {};
+			_battery_status_sub.copy(&bat_stat);
+			_voted_sensors_update.update_mag_comp_current(bat_stat.current_a);
 		}
 	}
 

src/modules/sensors/voted_sensors_update.cpp

@@ -52,7 +52,7 @@ using namespace sensors;
 using namespace matrix;
 
 VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled)
-	: _parameters(parameters), _hil_enabled(hil_enabled)
+	: ModuleParams(nullptr), _parameters(parameters), _hil_enabled(hil_enabled), _mag_compensator(this)
 {
 	for (unsigned i = 0; i < 3; i++) {
 		_corrections.gyro_scale_0[i] = 1.0f;
@@ -121,6 +121,8 @@ void VotedSensorsUpdate::parametersUpdate()
 		_mag_rotation[topic_instance] = _board_rotation;
 	}
 
+	updateParams();
+
 	/* set offset parameters to new values */
 	bool failed = false;
 
@@ -397,6 +399,7 @@ void VotedSensorsUpdate::parametersUpdate()
 			/* if the calibration is for this device, apply it */
 			if ((uint32_t)device_id == _mag_device_id[topic_instance]) {
 				_mag.enabled[topic_instance] = (device_enabled == 1);
+				_mag.power_compensation[topic_instance] = {_parameters.mag_comp_paramX[i], _parameters.mag_comp_paramY[i], _parameters.mag_comp_paramZ[i]};
 
 				// the mags that were published after the initial parameterUpdate
 				// would be given the priority even if disabled. Reset it to 0 in this case
@@ -679,6 +682,10 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
 			}
 
 			Vector3f vect(mag_report.x, mag_report.y, mag_report.z);
+
+			//throttle-/current-based mag compensation
+			_mag_compensator.calculate_mag_corrected(vect, _mag.power_compensation[uorb_index]);
+
 			vect = _mag_rotation[uorb_index] * vect;
 
 			_last_magnetometer[uorb_index].timestamp = mag_report.timestamp;
@@ -980,3 +987,18 @@ void VotedSensorsUpdate::calcMagInconsistency(sensor_preflight_s &preflt)
 	// will be zero if there is only one magnetometer and hence nothing to compare
 	preflt.mag_inconsistency_angle = mag_angle_diff_max;
 }
+
+void VotedSensorsUpdate::update_mag_comp_armed(bool armed)
+{
+	_mag_compensator.update_armed_flag(armed);
+}
+
+void VotedSensorsUpdate::update_mag_comp_throttle(float throttle)
+{
+	_mag_compensator.update_throttle(throttle);
+}
+
+void VotedSensorsUpdate::update_mag_comp_current(float current)
+{
+	_mag_compensator.update_current(current);
+}

src/modules/sensors/voted_sensors_update.h

@@ -51,6 +51,7 @@
 
 #include <lib/ecl/validation/data_validator.h>
 #include <lib/ecl/validation/data_validator_group.h>
+#include <lib/mag_compensation/MagCompensation.hpp>
 
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationQueued.hpp>
@@ -74,7 +75,7 @@ namespace sensors
  *
  * Handling of sensor updates with voting
  */
-class VotedSensorsUpdate
+class VotedSensorsUpdate : public ModuleParams
 {
 public:
 	/**
@@ -140,6 +141,21 @@ public:
 	 */
 	void calcMagInconsistency(sensor_preflight_s &preflt);
 
+	/**
+	 * Update armed flag for mag compensation.
+	 */
+	void update_mag_comp_armed(bool armed);
+
+	/**
+	 * Update throttle for mag compensation.
+	 */
+	void update_mag_comp_throttle(float throttle);
+
+	/**
+	 * Update current for mag compensation.
+	 */
+	void update_mag_comp_current(float current);
+
 private:
 
 	struct SensorData {
@@ -164,6 +180,7 @@ private:
 		int subscription_count;
 		DataValidatorGroup voter;
 		unsigned int last_failover_count;
+		matrix::Vector3f power_compensation[SENSOR_COUNT_MAX];
 	};
 
 	void initSensorClass(const orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max);
@@ -225,6 +242,9 @@ private:
 	float _gyro_diff[3][2] {};			/**< filtered gyro differences between IMU uinits (rad/s) */
 	float _mag_angle_diff[2] {};			/**< filtered mag angle differences between sensor instances (Ga) */
 
+	/* Magnetometer interference compensation */
+	MagCompensator _mag_compensator;
+
 	uint32_t _accel_device_id[SENSOR_COUNT_MAX] {};	/**< accel driver device id for each uorb instance */
 	uint32_t _gyro_device_id[SENSOR_COUNT_MAX] {};	/**< gyro driver device id for each uorb instance */
 	uint32_t _mag_device_id[SENSOR_COUNT_MAX] {};	/**< mag driver device id for each uorb instance */