sensors: move mag aggregation to new VehicleMagnetometer WorkItem

- purge all reminaing magnetometer IOCTL usage
 - mag calibration add off diagonal (soft iron) scale factors

.ci/Jenkinsfile-hardware

@@ -866,6 +866,7 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_imu"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_imu_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_local_position"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_magnetometer"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
@@ -932,6 +933,7 @@ void statusSEGGER() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_imu"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_imu_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_local_position"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_magnetometer"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /dev"'

Tools/ecl_ekf/plotting/pdf_report.py

@@ -46,10 +46,10 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
         raise PreconditionError('could not find innovation data')
 
     try:
-        sensor_preflight = ulog.get_dataset('sensor_preflight').data
+        sensor_preflight_imu = ulog.get_dataset('sensor_preflight_imu').data
         print('found sensor_preflight data')
     except:
-        raise PreconditionError('could not find sensor_preflight data')
+        raise PreconditionError('could not find sensor_preflight_imu data')
 
     control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status)
 
@@ -61,10 +61,10 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
     with PdfPages(output_plot_filename) as pdf_pages:
 
         # plot IMU consistency data
-        if ('accel_inconsistency_m_s_s' in sensor_preflight.keys()) and (
+        if ('accel_inconsistency_m_s_s' in sensor_preflight_imu.keys()) and (
-                'gyro_inconsistency_rad_s' in sensor_preflight.keys()):
+                'gyro_inconsistency_rad_s' in sensor_preflight_imu.keys()):
             data_plot = TimeSeriesPlot(
-                sensor_preflight, [['accel_inconsistency_m_s_s'], ['gyro_inconsistency_rad_s']],
+                sensor_preflight_imu, [['accel_inconsistency_m_s_s'], ['gyro_inconsistency_rad_s']],
                 x_labels=['data index', 'data index'],
                 y_labels=['acceleration (m/s/s)', 'angular rate (rad/s)'],
                 plot_title='IMU Consistency Check Levels', pdf_handle=pdf_pages)

msg/CMakeLists.txt

@@ -119,7 +119,8 @@ set(msg_files
 	sensor_gyro.msg
 	sensor_gyro_fifo.msg
 	sensor_mag.msg
-	sensor_preflight.msg
+	sensor_preflight_imu.msg
+	sensor_preflight_mag.msg
 	sensor_selection.msg
 	subsystem_info.msg
 	system_power.msg

msg/sensor_mag.msg

@@ -11,4 +11,4 @@ float32 temperature       # temperature in degrees celsius
 
 uint32 error_count
 
-bool  is_external	# if true the mag is external (i.e. not built into the board)
+bool is_external	# if true the mag is external (i.e. not built into the board)

msg/sensor_preflight_imu.msg

@@ -5,4 +5,3 @@
 uint64 timestamp # time since system start (microseconds)
 float32 accel_inconsistency_m_s_s # magnitude of maximum acceleration difference between IMU instances in (m/s/s).
 float32 gyro_inconsistency_rad_s # magnitude of maximum angular rate difference between IMU instances in (rad/s).
-float32 mag_inconsistency_angle # maximum angle between magnetometer instance field vectors in radians.

msg/sensor_preflight_mag.msg

@@ -0,0 +1,7 @@
+#
+# Pre-flight sensor check metrics.
+# The topic will not be updated when the vehicle is armed
+#
+uint64 timestamp # time since system start (microseconds)
+
+float32 mag_inconsistency_angle # maximum angle between magnetometer instance field vectors in radians.

msg/sensor_selection.msg

@@ -5,4 +5,3 @@
 uint64 timestamp		# time since system start (microseconds)
 uint32 accel_device_id		# unique device ID for the selected accelerometers
 uint32 gyro_device_id		# unique device ID for the selected rate gyros
-uint32 mag_device_id		# unique device ID for the selected magnetometer

msg/tools/uorb_rtps_message_ids.yaml

@@ -158,7 +158,7 @@ rtps:
     id: 67
   - msg: sensor_mag
     id: 68
-  - msg: sensor_preflight
+  - msg: sensor_preflight_imu
     id: 69
   - msg: sensor_selection
     id: 70
@@ -297,6 +297,8 @@ rtps:
     id: 132
   - msg: telemetry_heartbeat
     id: 133
+  - msg: sensor_preflight_mag
+    id: 134
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

msg/vehicle_magnetometer.msg

@@ -1,3 +1,8 @@
-uint64 timestamp			# time since system start (microseconds)
 
-float32[3] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
+uint64 timestamp            # time since system start (microseconds)
+
+uint64 timestamp_sample     # the timestamp of the raw data (microseconds)
+
+uint32 device_id            # unique device ID for the selected magnetometer
+
+float32[3] magnetometer_ga  # Magnetic field in NED body frame, in Gauss

src/drivers/drv_mag.h

@@ -51,30 +51,4 @@
 
 #include <uORB/topics/sensor_mag.h>
 
-/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
-struct mag_calibration_s {
-	float	x_offset;
-	float	x_scale;
-	float	y_offset;
-	float	y_scale;
-	float	z_offset;
-	float	z_scale;
-};
-
-/*
- * ioctl() definitions
- */
-
-#define _MAGIOCBASE		(0x2400)
-#define _MAGIOC(_n)		(_PX4_IOC(_MAGIOCBASE, _n))
-
-/** set the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCSSCALE		_MAGIOC(4)
-
-/** copy the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCGSCALE		_MAGIOC(5)
-
-/** determine if mag is external or onboard */
-#define MAGIOCGEXTERNAL		_MAGIOC(11)
-
 #endif /* _DRV_MAG_H */

src/drivers/imu/adis16448/ADIS16448.cpp

@@ -307,8 +307,6 @@ ADIS16448::print_status()
 	perf_print_counter(_perf_transfer);
 	perf_print_counter(_perf_bad_transfer);
 	perf_print_counter(_perf_crc_bad);
-
-	_px4_mag.print_status();
 }
 
 void

src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp

@@ -75,8 +75,6 @@ void ICM20948_AK09916::PrintInfo()
 {
 	perf_print_counter(_bad_transfer_perf);
 	perf_print_counter(_magnetic_sensor_overflow_perf);
-
-	_px4_mag.print_status();
 }
 
 void ICM20948_AK09916::Run()

src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp

@@ -75,8 +75,6 @@ void MPU9250_AK8963::PrintInfo()
 {
 	perf_print_counter(_bad_transfer_perf);
 	perf_print_counter(_magnetic_sensor_overflow_perf);
-
-	_px4_mag.print_status();
 }
 
 void MPU9250_AK8963::Run()

src/drivers/imu/mpu9250/MPU9250_mag.h

@@ -124,8 +124,6 @@ public:
 	bool ak8963_check_id(uint8_t &id);
 	bool ak8963_read_adjustments();
 
-	void print_status() { _px4_mag.print_status(); }
-
 protected:
 	device::Device			*_interface;
 

src/drivers/imu/mpu9250/mpu9250.cpp

@@ -669,6 +669,4 @@ MPU9250::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_bad_registers);
 	perf_print_counter(_duplicates);
-
-	_mag.print_status();
 }

src/drivers/magnetometer/akm/ak09916/AK09916.cpp

@@ -84,8 +84,6 @@ void AK09916::print_status()
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
 	perf_print_counter(_magnetic_sensor_overflow_perf);
-
-	_px4_mag.print_status();
 }
 
 int AK09916::probe()

src/drivers/magnetometer/akm/ak8963/AK8963.cpp

@@ -84,8 +84,6 @@ void AK8963::print_status()
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
 	perf_print_counter(_magnetic_sensor_overflow_perf);
-
-	_px4_mag.print_status();
 }
 
 int AK8963::probe()

src/drivers/magnetometer/bmm150/bmm150.cpp

@@ -549,7 +549,6 @@ void BMM150::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_bad_transfers);
 	perf_print_counter(_good_transfers);
-	_px4_mag.print_status();
 }
 
 int BMM150::self_test()

src/drivers/magnetometer/hmc5883/HMC5883.cpp

@@ -482,5 +482,4 @@ void HMC5883::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	printf("interval:  %u us\n", _measure_interval);
-	_px4_mag.print_status();
 }

src/drivers/magnetometer/hmc5883/HMC5883.hpp

@@ -39,7 +39,6 @@
 #include <drivers/device/i2c.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <lib/perf/perf_counter.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_device.h>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>

src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp

@@ -39,7 +39,6 @@
 
 #include <px4_platform_common/px4_config.h>
 #include <drivers/device/i2c.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_device.h>
 
 #include "hmc5883.h"

src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp

@@ -39,7 +39,6 @@
 
 #include <px4_platform_common/px4_config.h>
 #include <drivers/device/spi.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_device.h>
 
 #include "hmc5883.h"

src/drivers/magnetometer/isentek/ist8308/IST8308.cpp

@@ -81,7 +81,6 @@ void IST8308::print_status()
 	perf_print_counter(_transfer_perf);
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
-	_px4_mag.print_status();
 }
 
 int IST8308::probe()

src/drivers/magnetometer/ist8310/ist8310.cpp

@@ -49,7 +49,6 @@
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <lib/perf/perf_counter.h>
 #include <drivers/device/i2c.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_device.h>
 
@@ -581,7 +580,6 @@ void IST8310::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	printf("poll interval:  %u interval\n", _measure_interval);
-	_px4_mag.print_status();
 }
 
 I2CSPIDriverBase *

src/drivers/magnetometer/lis2mdl/lis2mdl.cpp

@@ -163,7 +163,6 @@ LIS2MDL::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	PX4_INFO("poll interval:  %u", _measure_interval);
-	_px4_mag.print_status();
 }
 
 void

src/drivers/magnetometer/lis2mdl/lis2mdl.h

@@ -41,7 +41,6 @@
 
 #include <drivers/device/i2c.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_mag.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>

src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp

@@ -49,7 +49,6 @@
 #include <unistd.h>
 
 #include <drivers/device/i2c.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_device.h>
 
 #include "board_config.h"

src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp

@@ -49,7 +49,6 @@
 #include <unistd.h>
 
 #include <drivers/device/spi.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_device.h>
 
 #include "board_config.h"

src/drivers/magnetometer/lis3mdl/lis3mdl.cpp

@@ -195,7 +195,6 @@ void LIS3MDL::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	PX4_INFO("poll interval:  %u", _measure_interval);
-	_px4_mag.print_status();
 }
 
 int LIS3MDL::reset()

src/drivers/magnetometer/lis3mdl/lis3mdl.h

@@ -41,7 +41,6 @@
 
 #include <drivers/device/i2c.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_mag.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>

src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp

@@ -49,7 +49,6 @@
 #include <unistd.h>
 
 #include <drivers/device/i2c.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_device.h>
 
 #include "board_config.h"

src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp

@@ -49,7 +49,6 @@
 #include <unistd.h>
 
 #include <drivers/device/spi.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_device.h>
 
 #include "board_config.h"

src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp

@@ -334,5 +334,4 @@ void LSM303AGR::print_status()
 	perf_print_counter(_mag_sample_perf);
 	perf_print_counter(_bad_registers);
 	perf_print_counter(_bad_values);
-	_px4_mag.print_status();
 }

src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp

@@ -35,7 +35,6 @@
 
 #include <drivers/drv_hrt.h>
 #include <drivers/device/spi.h>
-#include <drivers/drv_mag.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>

src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp

@@ -200,5 +200,4 @@ void LSM9DS1_MAG::print_status()
 	perf_print_counter(_interval_perf);
 	perf_print_counter(_transfer_perf);
 	perf_print_counter(_data_overrun_perf);
-	_px4_mag.print_status();
 }

src/drivers/magnetometer/qmc5883/QMC5883.cpp

@@ -308,5 +308,4 @@ void QMC5883::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	printf("interval:  %u us\n", _measure_interval);
-	_px4_mag.print_status();
 }

src/drivers/magnetometer/qmc5883/QMC5883.hpp

@@ -45,7 +45,6 @@
 #include <drivers/device/i2c.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <lib/perf/perf_counter.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_device.h>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>

src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp

@@ -39,7 +39,6 @@
 
 #include <px4_platform_common/px4_config.h>
 #include <drivers/device/i2c.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_device.h>
 
 #include "qmc5883.h"

src/drivers/magnetometer/rm3100/rm3100.cpp

@@ -267,7 +267,6 @@ void RM3100::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	PX4_INFO("poll interval:  %u", _measure_interval);
-	_px4_mag.print_status();
 }
 
 int RM3100::reset()

src/drivers/magnetometer/rm3100/rm3100.h

@@ -41,7 +41,6 @@
 
 #include <drivers/device/i2c.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_mag.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/i2c_spi_buses.h>

src/drivers/magnetometer/rm3100/rm3100_i2c.cpp

@@ -49,7 +49,6 @@
 #include <unistd.h>
 
 #include <drivers/device/i2c.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_device.h>
 
 #include "board_config.h"

src/drivers/magnetometer/rm3100/rm3100_spi.cpp

@@ -49,7 +49,6 @@
 #include <unistd.h>
 
 #include <drivers/device/spi.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_device.h>
 
 #include "board_config.h"

src/lib/CMakeLists.txt

@@ -51,7 +51,6 @@ add_subdirectory(hysteresis)
 add_subdirectory(l1)
 add_subdirectory(landing_slope)
 add_subdirectory(led)
-add_subdirectory(mag_compensation)
 add_subdirectory(mathlib)
 add_subdirectory(mixer)
 add_subdirectory(mixer_module)

src/lib/drivers/magnetometer/PX4Magnetometer.cpp

@@ -38,13 +38,14 @@
 
 PX4Magnetometer::PX4Magnetometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
 	CDev(nullptr),
-	_sensor_mag_pub{ORB_ID(sensor_mag), priority},
+	_sensor_pub{ORB_ID(sensor_mag), priority},
-	_rotation{rotation},
+	_device_id{device_id},
-	_device_id{device_id}
+	_rotation{rotation}
 {
-	_class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
+	// advertise immediately to keep instance numbering in sync
+	_sensor_pub.advertise();
 
-	_sensor_mag_pub.advertise();
+	_class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
 }
 
 PX4Magnetometer::~PX4Magnetometer()
@@ -53,46 +54,7 @@ PX4Magnetometer::~PX4Magnetometer()
 		unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_device_instance);
 	}
 
-	_sensor_mag_pub.unadvertise();
+	_sensor_pub.unadvertise();
-}
-
-int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-	case MAGIOCSSCALE: {
-			// Copy offsets and scale factors in
-			mag_calibration_s cal{};
-			memcpy(&cal, (mag_calibration_s *) arg, sizeof(cal));
-
-			_calibration_offset = matrix::Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
-			_calibration_scale = matrix::Vector3f{cal.x_scale, cal.y_scale, cal.z_scale};
-		}
-
-		return PX4_OK;
-
-	case MAGIOCGSCALE: {
-			// copy out scale factors
-			mag_calibration_s cal{};
-			cal.x_offset = _calibration_offset(0);
-			cal.y_offset = _calibration_offset(1);
-			cal.z_offset = _calibration_offset(2);
-			cal.x_scale = _calibration_scale(0);
-			cal.y_scale = _calibration_scale(1);
-			cal.z_scale = _calibration_scale(2);
-			memcpy((mag_calibration_s *)arg, &cal, sizeof(cal));
-		}
-
-		return 0;
-
-	case MAGIOCGEXTERNAL:
-		return _external;
-
-	case DEVIOCGDEVICEID:
-		return _device_id;
-
-	default:
-		return -ENOTTY;
-	}
 }
 
 void PX4Magnetometer::set_device_type(uint8_t devtype)
@@ -104,11 +66,11 @@ void PX4Magnetometer::set_device_type(uint8_t devtype)
 	// update to new device type
 	device_id.devid_s.devtype = devtype;
 
-	// copy back to report
+	// copy back
 	_device_id = device_id.devid;
 }
 
-void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
+void PX4Magnetometer::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
 {
 	sensor_mag_s report;
 	report.timestamp_sample = timestamp_sample;
@@ -119,27 +81,12 @@ void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, flo
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x, y, z);
 
-	const matrix::Vector3f raw_f{x, y, z};
+	report.x = x * _scale;
-
+	report.y = y * _scale;
-	// Apply range scale and the calibrating offset/scale
+	report.z = z * _scale;
-	const matrix::Vector3f val_calibrated{(((raw_f * _scale) - _calibration_offset).emult(_calibration_scale))};
-
-	report.x = val_calibrated(0);
-	report.y = val_calibrated(1);
-	report.z = val_calibrated(2);
 
 	report.is_external = _external;
 
 	report.timestamp = hrt_absolute_time();
-	_sensor_mag_pub.publish(report);
+	_sensor_pub.publish(report);
-}
-
-void PX4Magnetometer::print_status()
-{
-	PX4_INFO(MAG_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
-
-	PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1),
-		 (double)_calibration_scale(2));
-	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
-		 (double)_calibration_offset(2));
 }

src/lib/drivers/magnetometer/PX4Magnetometer.hpp

@@ -37,7 +37,6 @@
 #include <drivers/drv_hrt.h>
 #include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
-#include <uORB/uORB.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_mag.h>
 
@@ -47,10 +46,9 @@ public:
 	PX4Magnetometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Magnetometer() override;
 
-	int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
-
 	bool external() { return _external; }
 
+	void set_device_id(uint32_t device_id) { _device_id = device_id; }
 	void set_device_type(uint8_t devtype);
 	void set_error_count(uint32_t error_count) { _error_count = error_count; }
 	void increase_error_count() { _error_count++; }
@@ -58,24 +56,19 @@ public:
 	void set_temperature(float temperature) { _temperature = temperature; }
 	void set_external(bool external) { _external = external; }
 
-	void update(hrt_abstime timestamp_sample, float x, float y, float z);
+	void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
 
 	int get_class_instance() { return _class_device_instance; };
 
-	void print_status();
-
 private:
-	uORB::PublicationMulti<sensor_mag_s> _sensor_mag_pub;
+	uORB::PublicationMulti<sensor_mag_s> _sensor_pub;
 
-	matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f};
+	uint32_t		_device_id{0};
-	matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f};
+	const enum Rotation	_rotation;
 
-	const enum Rotation _rotation;
+	float			_scale{1.f};
-	uint32_t _device_id{0};
+	float			_temperature{NAN};
-	float _temperature{NAN};
+	uint32_t		_error_count{0};
-	float _scale{1.f};
-
-	uint32_t _error_count{0};
 
 	bool _external{false};
 

src/lib/mag_compensation/MagCompensation.cpp

@@ -1,53 +0,0 @@
-/****************************************************************************
- *
- *   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) {
-		mag = mag + param_vect * _power;
-	}
-}

src/lib/mag_compensation/MagCompensation.hpp

@@ -1,63 +0,0 @@
-/****************************************************************************
- *
- *   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_power(float power) { _power = power; }
-
-	void calculate_mag_corrected(matrix::Vector3f &mag, const matrix::Vector3f &param_vect);
-
-private:
-	float _power{0};
-	bool _armed{false};
-};

src/lib/mag_compensation/mag_compensation_params.c

@@ -1,44 +0,0 @@
-/****************************************************************************
- *
- *   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 (battery_status instance 0)
- * @value 3 Current-based compensation (battery_status instance 1)
- *
- * @category system
- * @group Sensor Calibration
- */
-PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0);

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -87,11 +87,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 		if (sys_has_mag == 1) {
 
-			bool prime_found = false;
-
-			int32_t prime_id = -1;
-			param_get(param_find("CAL_MAG_PRIME"), &prime_id);
-
 			/* check all sensors individually, but fail only for mandatory ones */
 			for (unsigned i = 0; i < max_optional_mag_count; i++) {
 				const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
@@ -99,29 +94,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 				int32_t device_id = -1;
 
-				if (magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
+				if (!magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
-
-					if ((prime_id > 0) && (device_id == prime_id)) {
-						prime_found = true;
-					}
-
-				} else {
 					if (required) {
 						failed = true;
 					}
 				}
 			}
 
-
+			// TODO: highest priority mag
-			/* check if the primary device is present */
-			if (!prime_found && prime_id != 0) {
-				if (reportFailures) {
-					mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
-				}
-
-				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, status);
-				failed = true;
-			}
 
 			/* mag consistency checks (need to be performed after the individual checks) */
 			if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures))) {
@@ -132,10 +112,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 	/* ---- ACCEL ---- */
 	if (checkSensors) {
-		bool prime_found = false;
-		int32_t prime_id = -1;
-		param_get(param_find("CAL_ACC_PRIME"), &prime_id);
-
 		/* check all sensors individually, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_accel_count; i++) {
 			const bool required = (i < max_mandatory_accel_count);
@@ -143,63 +119,31 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 			int32_t device_id = -1;
 
-			if (accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
+			if (!accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
-
-				if ((prime_id > 0) && (device_id == prime_id)) {
-					prime_found = true;
-				}
-
-			} else {
 				if (required) {
 					failed = true;
 				}
 			}
 		}
 
-		/* check if the primary device is present */
+		// TODO: highest priority (from params)
-		if (!prime_found && prime_id != 0) {
-			if (reportFailures) {
-				mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
-			}
-
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, status);
-			failed = true;
-		}
 	}
 
 	/* ---- GYRO ---- */
 	if (checkSensors) {
-		bool prime_found = false;
-		int32_t prime_id = -1;
-		param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
-
 		/* check all sensors individually, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
 			const bool required = (i < max_mandatory_gyro_count);
 			int32_t device_id = -1;
 
-			if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) {
+			if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) {
-
-				if ((prime_id > 0) && (device_id == prime_id)) {
-					prime_found = true;
-				}
-
-			} else {
 				if (required) {
 					failed = true;
 				}
 			}
 		}
 
-		/* check if the primary device is present */
+		// TODO: highest priority (from params)
-		if (!prime_found && prime_id != 0) {
-			if (reportFailures) {
-				mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
-			}
-
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, status);
-			failed = true;
-		}
 	}
 
 	/* ---- BARO ---- */

src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp

@@ -38,7 +38,7 @@
 #include <lib/parameters/param.h>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/sensor_preflight.h>
+#include <uORB/topics/sensor_preflight_imu.h>
 #include <uORB/topics/subsystem_info.h>
 
 bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
@@ -47,9 +47,9 @@ bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_
 	float test_limit = 1.0f; // pass limit re-used for each test
 
 	// Get sensor_preflight data if available and exit with a fail recorded if not
-	uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
+	uORB::SubscriptionData<sensor_preflight_imu_s> sensors_sub{ORB_ID(sensor_preflight_imu)};
 	sensors_sub.update();
-	const sensor_preflight_s &sensors = sensors_sub.get();
+	const sensor_preflight_imu_s &sensors = sensors_sub.get();
 
 	// Use the difference between IMU's to detect a bad calibration.
 	// If a single IMU is fitted, the value being checked will be zero so this check will always pass.

src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp

@@ -40,7 +40,7 @@
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/subsystem_info.h>
-#include <uORB/topics/sensor_preflight.h>
+#include <uORB/topics/sensor_preflight_mag.h>
 
 // return false if the magnetomer measurements are inconsistent
 bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
@@ -49,9 +49,9 @@ bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_
 	bool pass = false; // flag for result of checks
 
 	// get the sensor preflight data
-	uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
+	uORB::SubscriptionData<sensor_preflight_mag_s> sensors_sub{ORB_ID(sensor_preflight_mag)};
 	sensors_sub.update();
-	const sensor_preflight_s &sensors = sensors_sub.get();
+	const sensor_preflight_mag_s &sensors = sensors_sub.get();
 
 	if (sensors.timestamp == 0) {
 		// can happen if not advertised (yet)

src/modules/ekf2/ekf2_main.cpp

@@ -886,8 +886,8 @@ void Ekf2::Run()
 				// Do not reset parmameters when armed to prevent potential time slips casued by parameter set
 				// and notification events
 				// Check if there has been a persistant change in magnetometer ID
-				if (_sensor_selection.mag_device_id != 0
+				if (magnetometer.device_id != 0
-				    && (_sensor_selection.mag_device_id != (uint32_t)_param_ekf2_magbias_id.get())) {
+				    && (magnetometer.device_id != (uint32_t)_param_ekf2_magbias_id.get())) {
 
 					if (_invalid_mag_id_count < 200) {
 						_invalid_mag_id_count++;
@@ -908,7 +908,7 @@ void Ekf2::Run()
 					_param_ekf2_magbias_y.commit_no_notification();
 					_param_ekf2_magbias_z.set(0.f);
 					_param_ekf2_magbias_z.commit_no_notification();
-					_param_ekf2_magbias_id.set(_sensor_selection.mag_device_id);
+					_param_ekf2_magbias_id.set(magnetometer.device_id);
 					_param_ekf2_magbias_id.commit();
 
 					_invalid_mag_id_count = 0;
@@ -1510,7 +1510,7 @@ void Ekf2::Run()
 					bias.accel_device_id = _sensor_selection.accel_device_id;
 				}
 
-				bias.mag_device_id = _sensor_selection.mag_device_id;
+				bias.mag_device_id = _param_ekf2_magbias_id.get();
 
 				// In-run bias estimates
 				_ekf.getGyroBias().copyTo(bias.gyro_bias);
@@ -1632,8 +1632,7 @@ void Ekf2::Run()
 
 				// Check and save the last valid calibration when we are disarmed
 				if ((_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
-				    && (status.filter_fault_flags == 0)
+				    && (status.filter_fault_flags == 0)) {
-				    && (_sensor_selection.mag_device_id == (uint32_t)_param_ekf2_magbias_id.get())) {
 
 					update_mag_bias(_param_ekf2_magbias_x, 0);
 					update_mag_bias(_param_ekf2_magbias_y, 1);
@@ -1642,7 +1641,6 @@ void Ekf2::Run()
 					// reset to prevent data being saved too frequently
 					_total_cal_time_us = 0;
 				}
-
 			}
 
 			publish_wind_estimate(now);

src/modules/logger/logged_topics.cpp

@@ -79,7 +79,8 @@ void LoggedTopics::add_default_topics()
 	add_topic("safety");
 	add_topic("sensor_combined");
 	add_topic("sensor_correction");
-	add_topic("sensor_preflight", 200);
+	add_topic("sensor_preflight_imu", 200);
+	add_topic("sensor_preflight_mag", 500);
 	add_topic("sensor_selection");
 	add_topic("system_power", 500);
 	add_topic("tecs_status", 200);

src/modules/sensors/CMakeLists.txt

@@ -38,6 +38,7 @@ add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
 add_subdirectory(vehicle_air_data)
 add_subdirectory(vehicle_imu)
+add_subdirectory(vehicle_magnetometer)
 
 px4_add_module(
 	MODULE modules__sensors
@@ -45,7 +46,6 @@ px4_add_module(
 	SRCS
 		voted_sensors_update.cpp
 		sensors.cpp
-		parameters.cpp
 	DEPENDS
 		airspeed
 		conversion
@@ -58,5 +58,5 @@ px4_add_module(
 		vehicle_angular_velocity
 		vehicle_air_data
 		vehicle_imu
-		mag_compensation
+		vehicle_magnetometer
 	)

src/modules/sensors/common.h

@@ -1,52 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2017 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
-
-/**
- * @file common.h
- * common definitions used in sensors module
- *
- * @author Beat Kueng <beat-kueng@gmx.net>
- */
-
-namespace sensors
-{
-
-constexpr uint8_t MAG_COUNT_MAX = 4;
-constexpr uint8_t GYRO_COUNT_MAX = 3;
-constexpr uint8_t ACCEL_COUNT_MAX = 3;
-
-constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX, math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX));
-
-} /* namespace sensors */

src/modules/sensors/data_validator/tests/tests_common.cpp

@@ -100,4 +100,4 @@ void fill_validator_with_samples(DataValidator *validator,
 	*timestamp_io = timestamp;
 	*value_io = val;
 
-}
+}

src/modules/sensors/parameters.cpp

@@ -1,147 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2016 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 parameters.cpp
- *
- * @author Beat Kueng <beat-kueng@gmx.net>
- */
-
-#include "parameters.h"
-
-namespace sensors
-{
-
-void initialize_parameter_handles(ParameterHandles &parameter_handles)
-{
-	/* Differential pressure offset */
-	parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
-#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
-	parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
-#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
-
-	/* rotations */
-	parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
-
-	/* rotation offsets */
-	parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
-	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_type = param_find("CAL_MAG_COMP_TYP");
-
-	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");
-
-	(void)param_find("BAT_V_DIV");
-	(void)param_find("BAT_A_PER_V");
-
-	(void)param_find("CAL_ACC0_ID");
-	(void)param_find("CAL_GYRO0_ID");
-
-	(void)param_find("CAL_MAG0_ID");
-	(void)param_find("CAL_MAG1_ID");
-	(void)param_find("CAL_MAG2_ID");
-	(void)param_find("CAL_MAG3_ID");
-	(void)param_find("CAL_MAG0_ROT");
-	(void)param_find("CAL_MAG1_ROT");
-	(void)param_find("CAL_MAG2_ROT");
-	(void)param_find("CAL_MAG3_ROT");
-	(void)param_find("CAL_MAG_SIDES");
-
-	(void)param_find("SYS_PARAM_VER");
-	(void)param_find("SYS_AUTOSTART");
-	(void)param_find("SYS_AUTOCONFIG");
-	(void)param_find("TRIG_MODE");
-	(void)param_find("UAVCAN_ENABLE");
-
-	// Parameters controlling the on-board sensor thermal calibrator
-	(void)param_find("SYS_CAL_TDEL");
-	(void)param_find("SYS_CAL_TMAX");
-	(void)param_find("SYS_CAL_TMIN");
-}
-
-void update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters)
-{
-	/* Airspeed offset */
-	param_get(parameter_handles.diff_pres_offset_pa, &(parameters.diff_pres_offset_pa));
-#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
-	param_get(parameter_handles.diff_pres_analog_scale, &(parameters.diff_pres_analog_scale));
-#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
-
-	param_get(parameter_handles.board_rotation, &(parameters.board_rotation));
-
-	param_get(parameter_handles.board_offset[0], &(parameters.board_offset[0]));
-	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_type, &(parameters.mag_comp_type));
-
-	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);
-}
-
-} /* namespace sensors */

src/modules/sensors/parameters.h

@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2016 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
-
-/**
- * @file parameters.h
- *
- * defines the list of parameters that are used within the sensors module
- *
- * @author Beat Kueng <beat-kueng@gmx.net>
- */
-
-#include <lib/parameters/param.h>
-
-namespace sensors
-{
-
-struct Parameters {
-	float diff_pres_offset_pa;
-#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
-	float diff_pres_analog_scale;
-#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
-
-	int32_t board_rotation;
-
-	float board_offset[3];
-
-	//parameters for current/throttle-based mag compensation
-	int32_t mag_comp_type;
-	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;
-};
-
-struct ParameterHandles {
-	param_t diff_pres_offset_pa;
-#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
-	param_t diff_pres_analog_scale;
-#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
-
-	param_t board_rotation;
-
-	param_t board_offset[3];
-
-	param_t mag_comp_type;
-	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;
-
-};
-
-/**
- * initialize ParameterHandles struct
- */
-void initialize_parameter_handles(ParameterHandles &parameter_handles);
-
-
-/**
- * Read out the parameters using the handles into the parameters struct.
- * @return 0 on success, <0 on error
- */
-void update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters);
-
-} /* namespace sensors */

src/modules/sensors/sensor_params_mag.c

@@ -62,3 +62,32 @@ PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
  * @group Sensors
  */
 PARAM_DEFINE_INT32(CAL_MAG_SIDES, 63);
+
+/**
+ * Type of magnetometer compensation
+ *
+ * @value 0 Disabled
+ * @value 1 Throttle-based compensation
+ * @value 2 Current-based compensation (battery_status instance 0)
+ * @value 3 Current-based compensation (battery_status instance 1)
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0);
+
+/**
+ * Magnetometer max rate.
+ *
+ * Magnetometer data maximum publication rate. This is an upper bound,
+ * actual magnetometer data rate is still dependant on the sensor.
+ *
+ * @min 1
+ * @max 200
+ * @group Sensors
+ * @unit Hz
+ *
+ * @reboot_required true
+ *
+ */
+PARAM_DEFINE_FLOAT(SENS_MAG_RATE, 50.0f);

src/modules/sensors/sensor_params_mag0.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(CAL_MAG0_ZSCALE, 1.0f);
 
+/**
+ * Magnetometer X-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG0_XODIAG, 0.0f);
+
+/**
+ * Magnetometer Y-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG0_YODIAG, 0.0f);
+
+/**
+ * Magnetometer Z-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG0_ZODIAG, 0.0f);
+
 /**
 * Coefficient describing linear relationship between
 * X component of magnetometer in body frame axis

src/modules/sensors/sensor_params_mag1.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(CAL_MAG1_ZSCALE, 1.0f);
 
+/**
+ * Magnetometer X-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG1_XODIAG, 0.0f);
+
+/**
+ * Magnetometer Y-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG1_YODIAG, 0.0f);
+
+/**
+ * Magnetometer Z-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG1_ZODIAG, 0.0f);
+
 /**
 * Coefficient describing linear relationship between
 * X component of magnetometer in body frame axis

src/modules/sensors/sensor_params_mag2.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(CAL_MAG2_ZSCALE, 1.0f);
 
+/**
+ * Magnetometer X-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_XODIAG, 0.0f);
+
+/**
+ * Magnetometer Y-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_YODIAG, 0.0f);
+
+/**
+ * Magnetometer Z-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_ZODIAG, 0.0f);
+
 /**
 * Coefficient describing linear relationship between
 * X component of magnetometer in body frame axis

src/modules/sensors/sensor_params_mag3.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -159,6 +159,30 @@ PARAM_DEFINE_FLOAT(CAL_MAG3_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(CAL_MAG3_ZSCALE, 1.0f);
 
+/**
+ * Magnetometer X-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG3_XODIAG, 0.0f);
+
+/**
+ * Magnetometer Y-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG3_YODIAG, 0.0f);
+
+/**
+ * Magnetometer Z-axis off diagonal factor
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG3_ZODIAG, 0.0f);
+
 /**
 * Coefficient describing linear relationship between
 * X component of magnetometer in body frame axis

src/modules/sensors/sensors.cpp

@@ -45,7 +45,6 @@
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
 #include <lib/airspeed/airspeed.h>
-#include <lib/conversion/rotation.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
@@ -65,19 +64,17 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_preflight.h>
+#include <uORB/topics/sensor_preflight_imu.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_imu.h>
-#include <uORB/topics/vehicle_magnetometer.h>
-#include <uORB/topics/battery_status.h>
 
-#include "parameters.h"
 #include "voted_sensors_update.h"
 #include "vehicle_acceleration/VehicleAcceleration.hpp"
 #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
 #include "vehicle_air_data/VehicleAirData.hpp"
 #include "vehicle_imu/VehicleIMU.hpp"
+#include "vehicle_magnetometer/VehicleMagnetometer.hpp"
 
 using namespace sensors;
 using namespace time_literals;
@@ -116,10 +113,9 @@ private:
 
 	hrt_abstime     _last_config_update{0};
 	hrt_abstime     _sensor_combined_prev_timestamp{0};
-	hrt_abstime     _magnetometer_prev_timestamp{0};
 
 	sensor_combined_s _sensor_combined{};
-	sensor_preflight_s _sensor_preflight{};
+	sensor_preflight_imu_s _sensor_preflight_imu{};
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[3] {
 		{this, ORB_ID(vehicle_imu), 0},
@@ -127,26 +123,14 @@ private:
 		{this, ORB_ID(vehicle_imu), 2}
 	};
 
-	uORB::Subscription	_actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)};		/**< attitude controls sub */
+	uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
-	uORB::Subscription	_diff_pres_sub{ORB_ID(differential_pressure)};			/**< raw differential pressure subscription */
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};				/**< notification of parameter updates */
+	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
-	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	_vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
-	uORB::Subscription	_battery_status_sub{ORB_ID(battery_status), 0};		/**< battery_status instance 0 subscription */
 
-	uORB::Publication<airspeed_s>			_airspeed_pub{ORB_ID(airspeed)};			/**< airspeed */
+	uORB::Publication<airspeed_s>             _airspeed_pub{ORB_ID(airspeed)};
-	uORB::Publication<sensor_combined_s>		_sensor_pub{ORB_ID(sensor_combined)};			/**< combined sensor data topic */
+	uORB::Publication<sensor_combined_s>      _sensor_pub{ORB_ID(sensor_combined)};
-	uORB::Publication<sensor_preflight_s>		_sensor_preflight_pub{ORB_ID(sensor_preflight)};		/**< sensor preflight topic */
+	uORB::Publication<sensor_preflight_imu_s> _sensor_preflight_imu_pub{ORB_ID(sensor_preflight_imu)};
-	uORB::Publication<vehicle_magnetometer_s>	_magnetometer_pub{ORB_ID(vehicle_magnetometer)};	/**< combined sensor data topic */
-
-	enum class MagCompensationType {
-		Disabled = 0,
-		Throttle,
-		Current_inst0,
-		Current_inst1
-	};
-
-	MagCompensationType _mag_comp_type{MagCompensationType::Disabled};
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -161,14 +145,35 @@ private:
 	uORB::PublicationMulti<differential_pressure_s>	_diff_pres_pub{ORB_ID(differential_pressure)};		/**< differential_pressure */
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 
-	Parameters		_parameters{};			/**< local copies of interesting parameters */
+
-	ParameterHandles	_parameter_handles{};		/**< handles for interesting parameters */
+	struct Parameters {
+		float diff_pres_offset_pa;
+#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
+		float diff_pres_analog_scale;
+#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
+
+		int32_t air_cmodel;
+		float air_tube_length;
+		float air_tube_diameter_mm;
+	} _parameters{}; /**< local copies of interesting parameters */
+
+	struct ParameterHandles {
+		param_t diff_pres_offset_pa;
+#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
+		param_t diff_pres_analog_scale;
+#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
+
+		param_t air_cmodel;
+		param_t air_tube_length;
+		param_t air_tube_diameter_mm;
+	} _parameter_handles{};		/**< handles for interesting parameters */
 
 	VotedSensorsUpdate _voted_sensors_update;
 
 	VehicleAcceleration	_vehicle_acceleration;
 	VehicleAngularVelocity	_vehicle_angular_velocity;
 	VehicleAirData          *_vehicle_air_data{nullptr};
+	VehicleMagnetometer     *_vehicle_magnetometer{nullptr};
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
@@ -202,9 +207,11 @@ private:
 
 	void		InitializeVehicleAirData();
 	void		InitializeVehicleIMU();
+	void		InitializeVehicleMagnetometer();
 
 	DEFINE_PARAMETERS(
-		(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro
+		(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
+		(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
 	)
 };
 
@@ -213,9 +220,39 @@ Sensors::Sensors(bool hil_enabled) :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
 	_hil_enabled(hil_enabled),
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
-	_voted_sensors_update(_parameters, hil_enabled, _vehicle_imu_sub)
+	_voted_sensors_update(hil_enabled, _vehicle_imu_sub)
 {
-	initialize_parameter_handles(_parameter_handles);
+	/* Differential pressure offset */
+	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
+#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
+	_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
+#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
+
+	_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");
+
+	param_find("BAT_V_DIV");
+	param_find("BAT_A_PER_V");
+
+	param_find("CAL_ACC0_ID");
+	param_find("CAL_GYRO0_ID");
+
+	param_find("SENS_BOARD_ROT");
+	param_find("SENS_BOARD_X_OFF");
+	param_find("SENS_BOARD_Y_OFF");
+	param_find("SENS_BOARD_Z_OFF");
+
+	param_find("SYS_PARAM_VER");
+	param_find("SYS_AUTOSTART");
+	param_find("SYS_AUTOCONFIG");
+	param_find("TRIG_MODE");
+	param_find("UAVCAN_ENABLE");
+
+	// Parameters controlling the on-board sensor thermal calibrator
+	param_find("SYS_CAL_TDEL");
+	param_find("SYS_CAL_TMAX");
+	param_find("SYS_CAL_TMIN");
 
 	_airspeed_validator.set_timeout(300000);
 	_airspeed_validator.set_equal_value_threshold(100);
@@ -239,6 +276,11 @@ Sensors::~Sensors()
 		delete _vehicle_air_data;
 	}
 
+	if (_vehicle_magnetometer) {
+		_vehicle_magnetometer->Stop();
+		delete _vehicle_magnetometer;
+	}
+
 	for (auto &vehicle_imu : _vehicle_imu_list) {
 		if (vehicle_imu) {
 			vehicle_imu->Stop();
@@ -265,8 +307,15 @@ int Sensors::parameters_update()
 		return 0;
 	}
 
-	/* read the parameter values into _parameters */
+	/* Airspeed offset */
-	update_parameters(_parameter_handles, _parameters);
+	param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
+#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
+	param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
+#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
+
+	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);
 
 	_voted_sensors_update.parametersUpdate();
 
@@ -474,6 +523,21 @@ void Sensors::InitializeVehicleIMU()
 	}
 }
 
+void Sensors::InitializeVehicleMagnetometer()
+{
+	if (_param_sys_has_mag.get()) {
+		if (_vehicle_magnetometer == nullptr) {
+			if (orb_exists(ORB_ID(sensor_mag), 0) == PX4_OK) {
+				_vehicle_magnetometer = new VehicleMagnetometer();
+
+				if (_vehicle_magnetometer) {
+					_vehicle_magnetometer->Start();
+				}
+			}
+		}
+	}
+}
+
 void Sensors::Run()
 {
 	if (should_exit()) {
@@ -490,6 +554,7 @@ void Sensors::Run()
 	if (_last_config_update == 0) {
 		InitializeVehicleAirData();
 		InitializeVehicleIMU();
+		InitializeVehicleMagnetometer();
 		_voted_sensors_update.init(_sensor_combined);
 		parameter_update_poll(true);
 	}
@@ -505,57 +570,16 @@ void Sensors::Run()
 
 		if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
 			_armed = vcontrol_mode.flag_armed;
-			_voted_sensors_update.update_mag_comp_armed(_armed);
-		}
-
-		//check mag power compensation type (change battery current subscription instance if necessary)
-		if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst0
-		    && _mag_comp_type != MagCompensationType::Current_inst0) {
-			_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 0};
-		}
-
-		if ((MagCompensationType)_parameters.mag_comp_type == MagCompensationType::Current_inst1
-		    && _mag_comp_type != MagCompensationType::Current_inst1) {
-			_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 1};
-		}
-
-		_mag_comp_type = (MagCompensationType)_parameters.mag_comp_type;
-
-		//update power signal for mag compensation
-		if (_mag_comp_type == MagCompensationType::Throttle) {
-			actuator_controls_s controls {};
-
-			if (_actuator_ctrl_0_sub.update(&controls)) {
-				_voted_sensors_update.update_mag_comp_power(controls.control[actuator_controls_s::INDEX_THROTTLE]);
-			}
-
-		} else if (_mag_comp_type == MagCompensationType::Current_inst0
-			   || _mag_comp_type == MagCompensationType::Current_inst1) {
-			battery_status_s bat_stat {};
-
-			if (_battery_status_sub.update(&bat_stat)) {
-				_voted_sensors_update.update_mag_comp_power(bat_stat.current_a * 0.001f); //current in [kA]
-			}
 		}
 	}
 
-	vehicle_magnetometer_s magnetometer{};
+	_voted_sensors_update.sensorsPoll(_sensor_combined);
-	_voted_sensors_update.sensorsPoll(_sensor_combined, magnetometer);
 
 	// check analog airspeed
 	adc_poll();
 
 	diff_pres_poll();
 
-	if ((magnetometer.timestamp != 0) && (magnetometer.timestamp != _magnetometer_prev_timestamp)) {
-		_magnetometer_pub.publish(magnetometer);
-		_magnetometer_prev_timestamp = magnetometer.timestamp;
-
-		if (!_armed) {
-			_voted_sensors_update.calcMagInconsistency(_sensor_preflight);
-		}
-	}
-
 	if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
 
 		_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
@@ -565,11 +589,11 @@ void Sensors::Run()
 		// If the the vehicle is disarmed calculate the length of the maximum difference between
 		// IMU units as a consistency metric and publish to the sensor preflight topic
 		if (!_armed) {
-			_voted_sensors_update.calcAccelInconsistency(_sensor_preflight);
+			_voted_sensors_update.calcAccelInconsistency(_sensor_preflight_imu);
-			_voted_sensors_update.calcGyroInconsistency(_sensor_preflight);
+			_voted_sensors_update.calcGyroInconsistency(_sensor_preflight_imu);
 
-			_sensor_preflight.timestamp = hrt_absolute_time();
+			_sensor_preflight_imu.timestamp = hrt_absolute_time();
-			_sensor_preflight_pub.publish(_sensor_preflight);
+			_sensor_preflight_imu_pub.publish(_sensor_preflight_imu);
 		}
 	}
 
@@ -579,6 +603,7 @@ void Sensors::Run()
 		_voted_sensors_update.initializeSensors();
 		InitializeVehicleAirData();
 		InitializeVehicleIMU();
+		InitializeVehicleMagnetometer();
 		_last_config_update = hrt_absolute_time();
 
 	} else {
@@ -644,6 +669,11 @@ int Sensors::print_status()
 {
 	_voted_sensors_update.printStatus();
 
+	if (_vehicle_magnetometer) {
+		PX4_INFO_RAW("\n");
+		_vehicle_magnetometer->PrintStatus();
+	}
+
 	if (_vehicle_air_data) {
 		PX4_INFO_RAW("\n");
 		_vehicle_air_data->PrintStatus();
@@ -696,7 +726,7 @@ The provided functionality includes:
 - Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or
   on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the
   sensor drivers must already be running when `sensors` is started.
-- Do preflight sensor consistency checks and publish the `sensor_preflight` topic.
+- Do preflight sensor consistency checks and publish the `sensor_preflight_imu` topic.
 
 ### Implementation
 It runs in its own thread and polls on the currently selected gyro topic.

src/modules/sensors/vehicle_magnetometer/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#   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
@@ -31,4 +31,13 @@
 #
 ############################################################################
 
-px4_add_library(mag_compensation MagCompensation.cpp)
+px4_add_library(vehicle_magnetometer
+	VehicleMagnetometer.cpp
+	VehicleMagnetometer.hpp
+)
+
+target_link_libraries(vehicle_magnetometer
+	PRIVATE
+		px4_work_queue
+		sensor_calibration
+)

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -0,0 +1,383 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "VehicleMagnetometer.hpp"
+
+#include <px4_platform_common/log.h>
+#include <lib/ecl/geo/geo.h>
+
+namespace sensors
+{
+
+using namespace matrix;
+using namespace time_literals;
+
+static constexpr int32_t MAG_ROT_VAL_INTERNAL{-1};
+
+static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
+
+VehicleMagnetometer::VehicleMagnetometer() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
+{
+	char str[20] {};
+
+	for (int mag_index = 0; mag_index < 4; mag_index++) {
+		// CAL_MAGx_ID
+		sprintf(str, "CAL_%s%u_ID", "MAG", mag_index);
+		param_find(str);
+
+		// CAL_MAGx_ROT
+		sprintf(str, "CAL_%s%u_ROT", "MAG", mag_index);
+		param_find(str);
+	}
+
+	param_find("CAL_MAG_SIDES");
+	param_find("CAL_MAG_ROT_AUTO");
+
+	_voter.set_timeout(SENSOR_TIMEOUT);
+	_voter.set_equal_value_threshold(1000);
+
+	ParametersUpdate(true);
+}
+
+VehicleMagnetometer::~VehicleMagnetometer()
+{
+	Stop();
+	perf_free(_cycle_perf);
+}
+
+bool VehicleMagnetometer::Start()
+{
+	ScheduleNow();
+	return true;
+}
+
+void VehicleMagnetometer::Stop()
+{
+	Deinit();
+
+	// clear all registered callbacks
+	for (auto &sub : _sensor_sub) {
+		sub.unregisterCallback();
+	}
+}
+
+void VehicleMagnetometer::ParametersUpdate(bool force)
+{
+	// Check if parameters have changed
+	if (_params_sub.updated() || force) {
+		// clear update
+		parameter_update_s param_update;
+		_params_sub.copy(&param_update);
+
+		updateParams();
+
+		// Mag compensation type
+		MagCompensationType mag_comp_typ = static_cast<MagCompensationType>(_param_mag_comp_typ.get());
+
+		if (mag_comp_typ != _mag_comp_type) {
+			// check mag power compensation type (change battery current subscription instance if necessary)
+			if (mag_comp_typ == MagCompensationType::Current_inst0 && _mag_comp_type != MagCompensationType::Current_inst0) {
+				_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 0};
+			}
+
+			if (mag_comp_typ == MagCompensationType::Current_inst1 && _mag_comp_type != MagCompensationType::Current_inst1) {
+				_battery_status_sub = uORB::Subscription{ORB_ID(battery_status), 1};
+			}
+
+			if (mag_comp_typ == MagCompensationType::Throttle) {
+				_actuator_controls_0_sub = uORB::Subscription{ORB_ID(actuator_controls_0)};
+			}
+		}
+
+		_mag_comp_type = mag_comp_typ;
+	}
+}
+
+void VehicleMagnetometer::Run()
+{
+	perf_begin(_cycle_perf);
+
+	ParametersUpdate();
+
+	// check vehicle status for changes to armed state
+	if (_vcontrol_mode_sub.updated()) {
+		vehicle_control_mode_s vcontrol_mode;
+
+		if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
+			_armed = vcontrol_mode.flag_armed;
+		}
+	}
+
+	if (_mag_comp_type != MagCompensationType::Disabled) {
+		// update power signal for mag compensation
+		if (_armed) {
+			if (_mag_comp_type == MagCompensationType::Throttle) {
+				actuator_controls_s controls;
+
+				if (_actuator_controls_0_sub.update(&controls)) {
+					for (auto &cal : _calibration) {
+						cal.UpdatePower(controls.control[actuator_controls_s::INDEX_THROTTLE]);
+					}
+				}
+
+			} else if (_mag_comp_type == MagCompensationType::Current_inst0
+				   || _mag_comp_type == MagCompensationType::Current_inst1) {
+
+				battery_status_s bat_stat;
+
+				if (_battery_status_sub.update(&bat_stat)) {
+					float power = bat_stat.current_a * 0.001f; //current in [kA]
+
+					for (auto &cal : _calibration) {
+						cal.UpdatePower(power);
+					}
+				}
+			}
+
+		} else {
+			for (auto &cal : _calibration) {
+				cal.UpdatePower(0.f);
+			}
+		}
+	}
+
+	bool updated[MAX_SENSOR_COUNT] {};
+
+	for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
+
+		if (!_calibration[uorb_index].enabled()) {
+			continue;
+		}
+
+		if (!_advertised[uorb_index]) {
+			// use data's timestamp to throttle advertisement checks
+			if (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s) {
+				if (_sensor_sub[uorb_index].advertised()) {
+					if (uorb_index > 0) {
+						/* the first always exists, but for each further sensor, add a new validator */
+						if (!_voter.add_new_validator()) {
+							PX4_ERR("failed to add validator for %s %i", "MAG", uorb_index);
+						}
+					}
+
+					_advertised[uorb_index] = true;
+
+				} else {
+					_last_data[uorb_index].timestamp = hrt_absolute_time();
+				}
+			}
+
+		} else {
+			sensor_mag_s report;
+			updated[uorb_index] = _sensor_sub[uorb_index].update(&report);
+
+			if (updated[uorb_index]) {
+				if (_calibration[uorb_index].device_id() != report.device_id) {
+					_calibration[uorb_index].set_external(report.is_external);
+					_calibration[uorb_index].set_device_id(report.device_id);
+					_priority[uorb_index] = _sensor_sub[uorb_index].get_priority();
+				}
+
+				if (_calibration[uorb_index].enabled()) {
+					Vector3f vect = _calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z});
+
+					_last_data[uorb_index].timestamp_sample = report.timestamp_sample;
+					_last_data[uorb_index].device_id = report.device_id;
+					_last_data[uorb_index].x = vect(0);
+					_last_data[uorb_index].y = vect(1);
+					_last_data[uorb_index].z = vect(2);
+
+					float mag_array[3] {vect(0), vect(1), vect(2)};
+					_voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]);
+				}
+			}
+		}
+	}
+
+	// check for the current best sensor
+	int best_index = 0;
+	_voter.get_best(hrt_absolute_time(), &best_index);
+
+	if (best_index >= 0) {
+		if (_selected_sensor_sub_index != best_index) {
+			// clear all registered callbacks
+			for (auto &sub : _sensor_sub) {
+				sub.unregisterCallback();
+			}
+
+			if (_selected_sensor_sub_index >= 0) {
+				PX4_INFO("%s switch from #%u -> #%d", "MAG", _selected_sensor_sub_index, best_index);
+			}
+
+			_selected_sensor_sub_index = best_index;
+			_sensor_sub[_selected_sensor_sub_index].registerCallback();
+		}
+	}
+
+	if ((_selected_sensor_sub_index >= 0)
+	    && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR)
+	    && updated[_selected_sensor_sub_index]) {
+
+		const sensor_mag_s &mag = _last_data[_selected_sensor_sub_index];
+
+		_mag_timestamp_sum += mag.timestamp_sample;
+		_mag_sum += Vector3f{mag.x, mag.y, mag.z};
+		_mag_sum_count++;
+
+		if ((_param_sens_mag_rate.get() > 0)
+		    && hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_mag_rate.get())) {
+
+			const Vector3f magnetometer_data = _mag_sum / _mag_sum_count;
+			const hrt_abstime timestamp_sample = _mag_timestamp_sum / _mag_sum_count;
+
+			// reset
+			_mag_timestamp_sum = 0;
+			_mag_sum.zero();
+			_mag_sum_count = 0;
+
+			// populate vehicle_magnetometer with primary mag and publish
+			vehicle_magnetometer_s out{};
+			out.timestamp_sample = timestamp_sample;
+			out.device_id = mag.device_id;
+			magnetometer_data.copyTo(out.magnetometer_ga);
+
+			out.timestamp = hrt_absolute_time();
+			_vehicle_magnetometer_pub.publish(out);
+
+			_last_publication_timestamp = out.timestamp;
+		}
+	}
+
+	// check failover and report
+	if (_last_failover_count != _voter.failover_count()) {
+		uint32_t flags = _voter.failover_state();
+		int failover_index = _voter.failover_index();
+
+		if (flags != DataValidator::ERROR_FLAG_NO_ERROR) {
+			if (failover_index != -1) {
+				const hrt_abstime now = hrt_absolute_time();
+
+				if (now - _last_error_message > 3_s) {
+					mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!",
+							      "MAG",
+							      failover_index,
+							      ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
+							      ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
+							      ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""),
+							      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""),
+							      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : ""));
+					_last_error_message = now;
+				}
+
+				// reduce priority of failed sensor to the minimum
+				_priority[failover_index] = 1;
+			}
+		}
+
+		_last_failover_count = _voter.failover_count();
+	}
+
+	if (!_armed) {
+		calcMagInconsistency();
+	}
+
+	// reschedule timeout
+	ScheduleDelayed(100_ms);
+
+	perf_end(_cycle_perf);
+}
+
+void VehicleMagnetometer::calcMagInconsistency()
+{
+	sensor_preflight_mag_s preflt{};
+
+	const sensor_mag_s &primary_mag_report = _last_data[_selected_sensor_sub_index];
+	const Vector3f primary_mag(primary_mag_report.x, primary_mag_report.y,
+				   primary_mag_report.z); // primary mag field vector
+
+	float mag_angle_diff_max = 0.0f; // the maximum angle difference
+	unsigned check_index = 0; // the number of sensors the primary has been checked against
+
+	// Check each sensor against the primary
+	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+		// check that the sensor we are checking against is not the same as the primary
+		if (_advertised[i] && (_priority[i] > 0) && (i != _selected_sensor_sub_index)) {
+			// calculate angle to 3D magnetic field vector of the primary sensor
+			const sensor_mag_s &current_mag_report = _last_data[i];
+			Vector3f current_mag{current_mag_report.x, current_mag_report.y, current_mag_report.z};
+
+			float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle();
+
+			// complementary filter to not fail/pass on single outliers
+			_mag_angle_diff[check_index] *= 0.95f;
+			_mag_angle_diff[check_index] += 0.05f * angle_error;
+
+			mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]);
+
+			// increment the check index
+			check_index++;
+		}
+
+		// check to see if the maximum number of checks has been reached and break
+		if (check_index >= 2) {
+			break;
+		}
+	}
+
+	// get the vector length of the largest difference and write to the combined sensor struct
+	// will be zero if there is only one magnetometer and hence nothing to compare
+	preflt.mag_inconsistency_angle = mag_angle_diff_max;
+
+	preflt.timestamp = hrt_absolute_time();
+	_sensor_preflight_mag_pub.publish(preflt);
+}
+
+void VehicleMagnetometer::PrintStatus()
+{
+	if (_selected_sensor_sub_index >= 0) {
+		PX4_INFO("selected magnetometer: %d (%d)", _last_data[_selected_sensor_sub_index].device_id,
+			 _selected_sensor_sub_index);
+	}
+
+	_voter.print();
+
+	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+		if (_advertised[i] && (_priority[i] > 0)) {
+			_calibration[i].PrintStatus();
+		}
+	}
+}
+
+}; // namespace sensors

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "data_validator/DataValidatorGroup.hpp"
+
+#include <lib/sensor_calibration/Magnetometer.hpp>
+#include <lib/conversion/rotation.h>
+#include <lib/mathlib/math/Limits.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <lib/systemlib/mavlink_log.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_mag.h>
+#include <uORB/topics/sensor_preflight_mag.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_magnetometer.h>
+
+namespace sensors
+{
+class VehicleMagnetometer : public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+
+	VehicleMagnetometer();
+	~VehicleMagnetometer() override;
+
+	bool Start();
+	void Stop();
+
+	void PrintStatus();
+
+private:
+	void Run() override;
+
+	void ParametersUpdate(bool force = false);
+
+	/**
+	 * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
+	 */
+	void calcMagInconsistency();
+
+	static constexpr int MAX_SENSOR_COUNT = 4;
+
+	uORB::Publication<sensor_preflight_mag_s> _sensor_preflight_mag_pub{ORB_ID(sensor_preflight_mag)};
+	uORB::Publication<vehicle_magnetometer_s> _vehicle_magnetometer_pub{ORB_ID(vehicle_magnetometer)};
+
+	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0};
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
+
+	uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
+		{this, ORB_ID(sensor_mag), 0},
+		{this, ORB_ID(sensor_mag), 1},
+		{this, ORB_ID(sensor_mag), 2},
+		{this, ORB_ID(sensor_mag), 3}
+	};
+
+	calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
+
+	// Magnetometer interference compensation
+	enum class MagCompensationType {
+		Disabled = 0,
+		Throttle,
+		Current_inst0,
+		Current_inst1
+	};
+	MagCompensationType _mag_comp_type{MagCompensationType::Disabled};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	hrt_abstime _last_publication_timestamp{0};
+	hrt_abstime _last_error_message{0};
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	DataValidatorGroup _voter{1};
+	unsigned _last_failover_count{0};
+
+	uint64_t _mag_timestamp_sum{0};
+	matrix::Vector3f _mag_sum{};
+	int _mag_sum_count{0};
+
+	sensor_mag_s _last_data[MAX_SENSOR_COUNT] {};
+	bool _advertised[MAX_SENSOR_COUNT] {};
+
+	float _mag_angle_diff[2] {};			/**< filtered mag angle differences between sensor instances (Ga) */
+
+	uint8_t _priority[MAX_SENSOR_COUNT] {};
+
+	int8_t _selected_sensor_sub_index{-1};
+
+	bool _armed{false};				/**< arming status of the vehicle */
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
+		(ParamFloat<px4::params::SENS_MAG_RATE>) _param_sens_mag_rate
+	)
+};
+}; // namespace sensors

src/modules/sensors/voted_sensors_update.cpp

@@ -39,32 +39,20 @@
 
 #include "voted_sensors_update.h"
 
-#include <lib/conversion/rotation.h>
+#include <lib/sensor_calibration/Utilities.hpp>
 #include <lib/ecl/geo/geo.h>
 #include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/sensor_accel.h>
-#include <uORB/topics/sensor_gyro.h>
-
-#define MAG_ROT_VAL_INTERNAL		-1
-#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
 
 using namespace sensors;
 using namespace matrix;
 using namespace time_literals;
-using math::radians;
 
-VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled,
+VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]) :
-				       uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]) :
 	ModuleParams(nullptr),
 	_vehicle_imu_sub(vehicle_imu_sub),
-	_parameters(parameters),
+	_hil_enabled(hil_enabled)
-	_hil_enabled(hil_enabled),
-	_mag_compensator(this)
 {
-	_mag.voter.set_timeout(300000);
-	_mag.voter.set_equal_value_threshold(1000);
-
 	if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit
 		_gyro.voter.set_timeout(500000);
 		_accel.voter.set_timeout(500000);
@@ -87,26 +75,12 @@ void VotedSensorsUpdate::initializeSensors()
 {
 	initSensorClass(_gyro, GYRO_COUNT_MAX);
 	initSensorClass(_accel, ACCEL_COUNT_MAX);
-	initSensorClass(_mag, MAG_COUNT_MAX);
 }
 
 void VotedSensorsUpdate::parametersUpdate()
 {
 	updateParams();
 
-	/* fine tune board offset */
-	const Dcmf board_rotation_offset{Eulerf{radians(_parameters.board_offset[0]), radians(_parameters.board_offset[1]), radians(_parameters.board_offset[2])}};
-
-	_board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_parameters.board_rotation);
-
-	// initialze all mag rotations with the board rotation in case there is no calibration data available
-	for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX; ++topic_instance) {
-		_mag_rotation[topic_instance] = _board_rotation;
-	}
-
-	/* set offset parameters to new values */
-	bool failed = false;
-
 	// run through all IMUs
 	for (uint8_t uorb_index = 0; uorb_index < math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX); uorb_index++) {
 		uORB::SubscriptionData<vehicle_imu_s> imu{ORB_ID(vehicle_imu), uorb_index};
@@ -141,159 +115,6 @@ void VotedSensorsUpdate::parametersUpdate()
 			}
 		}
 	}
-
-
-	/* run through all mag sensors
-	 * Because we store the device id in _mag_device_id, we need to get the id via uorb topic since
-	 * the DevHandle method does not work on POSIX.
-	 */
-
-	/* first we have to reset all possible mags, since we are looping through the uORB instances instead of the drivers,
-	 * and not all uORB instances have to be published yet at the initial call of parametersUpdate()
-	 */
-	for (int i = 0; i < MAG_COUNT_MAX; i++) {
-		_mag.enabled[i] = true;
-	}
-
-	for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX
-	     && topic_instance < _mag.subscription_count; ++topic_instance) {
-
-		sensor_mag_s report{};
-
-		if (!_mag.subscription[topic_instance].copy(&report)) {
-			continue;
-		}
-
-		uint32_t topic_device_id = report.device_id;
-		bool is_external = report.is_external;
-		_mag_device_id[topic_instance] = topic_device_id;
-
-		// find the driver handle that matches the topic_device_id
-		int fd = -1;
-		char str[30] {};
-
-		for (unsigned driver_index = 0; driver_index < MAG_COUNT_MAX; ++driver_index) {
-
-			(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, driver_index);
-
-			fd = px4_open(str, O_RDWR);
-
-			if (fd < 0) {
-				/* the driver is not running, continue with the next */
-				continue;
-			}
-
-			uint32_t driver_device_id = (uint32_t)px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-
-			if (driver_device_id == topic_device_id) {
-				break; // we found the matching driver
-
-			} else {
-				px4_close(fd);
-			}
-		}
-
-		bool config_ok = false;
-
-		/* run through all stored calibrations */
-		for (unsigned i = 0; i < MAG_COUNT_MAX; i++) {
-			/* initially status is ok per config */
-			failed = false;
-
-			(void)sprintf(str, "CAL_MAG%u_ID", i);
-			int32_t device_id = 0;
-			failed = failed || (PX4_OK != param_get(param_find(str), &device_id));
-
-			(void)sprintf(str, "CAL_MAG%u_EN", i);
-			int32_t device_enabled = 1;
-			failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled));
-
-			if (failed) {
-				continue;
-			}
-
-			/* 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
-				if (!_mag.enabled[topic_instance]) {
-					_mag.priority[topic_instance] = ORB_PRIO_UNINITIALIZED;
-				}
-
-				mag_calibration_s mscale{};
-
-				(void)sprintf(str, "CAL_MAG%u_XOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &mscale.x_offset));
-
-				(void)sprintf(str, "CAL_MAG%u_YOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &mscale.y_offset));
-
-				(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &mscale.z_offset));
-
-				(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &mscale.x_scale));
-
-				(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &mscale.y_scale));
-
-				(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &mscale.z_scale));
-
-				(void)sprintf(str, "CAL_MAG%u_ROT", i);
-				int32_t mag_rot = 0;
-				param_get(param_find(str), &mag_rot);
-
-				if (is_external) {
-
-					/* check if this mag is still set as internal, otherwise leave untouched */
-					if (mag_rot < 0) {
-						/* it was marked as internal, change to external with no rotation */
-						mag_rot = 0;
-						param_set_no_notification(param_find(str), &mag_rot);
-					}
-
-				} else {
-					/* mag is internal - reset param to -1 to indicate internal mag */
-					if (mag_rot != MAG_ROT_VAL_INTERNAL) {
-						mag_rot = MAG_ROT_VAL_INTERNAL;
-						param_set_no_notification(param_find(str), &mag_rot);
-					}
-				}
-
-				/* now get the mag rotation */
-				if (mag_rot >= 0) {
-					// Set external magnetometers to use the parameter value
-					_mag_rotation[topic_instance] = get_rot_matrix((enum Rotation)mag_rot);
-
-				} else {
-					// Set internal magnetometers to use the board rotation
-					_mag_rotation[topic_instance] = _board_rotation;
-				}
-
-				if (failed) {
-					PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
-
-				} else {
-
-					/* apply new scaling and offsets */
-					config_ok = (px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale) == 0);
-
-					if (!config_ok) {
-						PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag ", i);
-					}
-				}
-
-				break;
-			}
-		}
-
-		px4_close(fd);
-	}
-
 }
 
 void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
@@ -398,51 +219,6 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 	}
 }
 
-void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)
-{
-	for (int uorb_index = 0; uorb_index < _mag.subscription_count; uorb_index++) {
-		sensor_mag_s mag_report;
-
-		if (_mag.enabled[uorb_index] && _mag.subscription[uorb_index].update(&mag_report)) {
-
-			// First publication with data
-			if (_mag.priority[uorb_index] == 0) {
-				_mag.priority[uorb_index] = _mag.subscription[uorb_index].get_priority();
-			}
-
-			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;
-			_last_magnetometer[uorb_index].magnetometer_ga[0] = vect(0);
-			_last_magnetometer[uorb_index].magnetometer_ga[1] = vect(1);
-			_last_magnetometer[uorb_index].magnetometer_ga[2] = vect(2);
-
-			_mag.voter.put(uorb_index, mag_report.timestamp, _last_magnetometer[uorb_index].magnetometer_ga, mag_report.error_count,
-				       _mag.priority[uorb_index]);
-		}
-	}
-
-	int best_index;
-	_mag.voter.get_best(hrt_absolute_time(), &best_index);
-
-	checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG);
-
-	if (best_index >= 0) {
-		magnetometer = _last_magnetometer[best_index];
-		_mag.last_best_vote = (uint8_t)best_index;
-
-		if (_selection.mag_device_id != _mag_device_id[best_index]) {
-			_selection_changed = true;
-			_selection.mag_device_id = _mag_device_id[best_index];
-		}
-	}
-}
-
 bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type)
 {
 	if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) {
@@ -494,8 +270,6 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na
 						if (type == subsystem_info_s::SUBSYSTEM_TYPE_GYRO) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; }
 
 						if (type == subsystem_info_s::SUBSYSTEM_TYPE_ACC) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; }
-
-						if (type == subsystem_info_s::SUBSYSTEM_TYPE_MAG) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; }
 					}
 
 					_info.timestamp = hrt_absolute_time();
@@ -558,16 +332,11 @@ void VotedSensorsUpdate::printStatus()
 	PX4_INFO_RAW("\n");
 	PX4_INFO("selected accel: %d (%d)", _selection.accel_device_id, _accel.last_best_vote);
 	_accel.voter.print();
-
-	PX4_INFO_RAW("\n");
-	PX4_INFO("selected mag: %d (%d)", _selection.mag_device_id, _mag.last_best_vote);
-	_mag.voter.print();
 }
 
-void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer)
+void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
 {
 	imuPoll(raw);
-	magPoll(magnetometer);
 
 	// publish sensor selection if changed
 	if (_selection_changed) {
@@ -588,8 +357,7 @@ void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
 	}
 }
 
-void
+void VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_imu_s &preflt)
-VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt)
 {
 	float accel_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
 	unsigned check_index = 0; // the number of sensors the primary has been checked against
@@ -638,7 +406,7 @@ VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt)
 	}
 }
 
-void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt)
+void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_imu_s &preflt)
 {
 	float gyro_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
 	unsigned check_index = 0; // the number of sensors the primary has been checked against
@@ -686,48 +454,3 @@ void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt)
 		preflt.gyro_inconsistency_rad_s = sqrtf(gyro_diff_sum_max_sq);
 	}
 }
-
-void VotedSensorsUpdate::calcMagInconsistency(sensor_preflight_s &preflt)
-{
-	Vector3f primary_mag(_last_magnetometer[_mag.last_best_vote].magnetometer_ga); // primary mag field vector
-	float mag_angle_diff_max = 0.0f; // the maximum angle difference
-	unsigned check_index = 0; // the number of sensors the primary has been checked against
-
-	// Check each sensor against the primary
-	for (int i = 0; i < _mag.subscription_count; i++) {
-		// check that the sensor we are checking against is not the same as the primary
-		if (_mag.enabled[i] && (_mag.priority[i] > 0) && (i != _mag.last_best_vote)) {
-			// calculate angle to 3D magnetic field vector of the primary sensor
-			Vector3f current_mag(_last_magnetometer[i].magnetometer_ga);
-			float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle();
-
-			// complementary filter to not fail/pass on single outliers
-			_mag_angle_diff[check_index] *= 0.95f;
-			_mag_angle_diff[check_index] += 0.05f * angle_error;
-
-			mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]);
-
-			// increment the check index
-			check_index++;
-		}
-
-		// check to see if the maximum number of checks has been reached and break
-		if (check_index >= 2) {
-			break;
-		}
-	}
-
-	// get the vector length of the largest difference and write to the combined sensor struct
-	// 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_power(float power)
-{
-	_mag_compensator.update_power(power);
-}

src/modules/sensors/voted_sensors_update.h

@@ -39,35 +39,33 @@
  * @author Beat Kueng <beat-kueng@gmx.net>
  */
 
-#include "parameters.h"
-
 #include "data_validator/DataValidator.hpp"
 #include "data_validator/DataValidatorGroup.hpp"
 
-#include <drivers/drv_mag.h>
+#include <px4_platform_common/module_params.h>
 #include <drivers/drv_hrt.h>
-
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
-
-#include <lib/mag_compensation/MagCompensation.hpp>
-
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/sensor_preflight.h>
+#include <uORB/topics/sensor_preflight_imu.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/vehicle_imu_status.h>
-#include <uORB/topics/vehicle_magnetometer.h>
 #include <uORB/topics/subsystem_info.h>
 
-#include "common.h"
-
 namespace sensors
 {
 
+static constexpr uint8_t GYRO_COUNT_MAX = 3;
+static constexpr uint8_t ACCEL_COUNT_MAX = 3;
+
+static constexpr uint8_t SENSOR_COUNT_MAX = math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX);
+
 /**
  ** class VotedSensorsUpdate
  *
@@ -80,8 +78,7 @@ public:
 	 * @param parameters parameter values. These do not have to be initialized when constructing this object.
 	 * Only when calling init(), they have to be initialized.
 	 */
-	VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled,
+	VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]);
-			   uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[3]);
 
 	/**
 	 * initialize subscriptions etc.
@@ -105,7 +102,7 @@ public:
 	/**
 	 * read new sensor data
 	 */
-	void sensorsPoll(sensor_combined_s &raw, vehicle_magnetometer_s &magnetometer);
+	void sensorsPoll(sensor_combined_s &raw);
 
 	/**
 	 * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll,
@@ -116,34 +113,22 @@ public:
 	/**
 	 * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor
 	 */
-	void calcAccelInconsistency(sensor_preflight_s &preflt);
+	void calcAccelInconsistency(sensor_preflight_imu_s &preflt);
 
 	/**
 	 * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor
 	 */
-	void calcGyroInconsistency(sensor_preflight_s &preflt);
+	void calcGyroInconsistency(sensor_preflight_imu_s &preflt);
-
-	/**
-	 * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
-	 */
-	void calcMagInconsistency(sensor_preflight_s &preflt);
-
-	/**
-	 * Update armed flag for mag compensation.
-	 */
-	void update_mag_comp_armed(bool armed);
-
-	/**
-	 * Update power signal for mag compensation.
-	 * power: either throttle value [0,1] or current measurement in [kA]
-	 */
-	void update_mag_comp_power(float power);
 
 private:
 
+	static constexpr uint8_t ACCEL_COUNT_MAX = 3;
+	static constexpr uint8_t GYRO_COUNT_MAX = 3;
+	static constexpr uint8_t SENSOR_COUNT_MAX = math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX);
+
 	struct SensorData {
 		SensorData() = delete;
-		explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {}
+		explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}} {}
 
 		uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
 		DataValidatorGroup voter{1};
@@ -151,9 +136,8 @@ private:
 		ORB_PRIO priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */
 		uint8_t last_best_vote{0}; /**< index of the latest best vote */
 		uint8_t subscription_count{0};
-		bool enabled[SENSOR_COUNT_MAX] {true, true, true, true};
+		bool enabled[SENSOR_COUNT_MAX] {true, true, true};
-		bool advertised[SENSOR_COUNT_MAX] {false, false, false, false};
+		bool advertised[SENSOR_COUNT_MAX] {false, false, false};
-		matrix::Vector3f power_compensation[SENSOR_COUNT_MAX];
 	};
 
 	void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max);
@@ -166,14 +150,6 @@ private:
 	 */
 	void imuPoll(sensor_combined_s &raw);
 
-	/**
-	 * Poll the magnetometer for updated data.
-	 *
-	 * @param raw	Combined sensor data structure into which
-	 *		data should be returned.
-	 */
-	void magPoll(vehicle_magnetometer_s &magnetometer);
-
 	/**
 	 * Check & handle failover of a sensor
 	 * @return true if a switch occured (could be for a non-critical reason)
@@ -182,7 +158,6 @@ private:
 
 	SensorData _accel{ORB_ID::sensor_accel};
 	SensorData _gyro{ORB_ID::sensor_gyro};
-	SensorData _mag{ORB_ID::sensor_mag};
 
 	hrt_abstime _last_error_message{0};
 	orb_advert_t _mavlink_log_pub{nullptr};
@@ -198,26 +173,16 @@ private:
 	};
 
 	sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {};	/**< latest sensor data from all sensors instances */
-	vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
 
-	matrix::Dcmf _board_rotation {};		/**< rotation matrix for the orientation that the board is mounted */
-	matrix::Dcmf _mag_rotation[MAG_COUNT_MAX] {};	/**< rotation matrix for the orientation that the external mag0 is mounted */
-
-	const Parameters &_parameters;
 	const bool _hil_enabled{false};			/**< is hardware-in-the-loop mode enabled? */
 
 	bool _selection_changed{true};			/**< true when a sensor selection has changed and not been published */
 
 	float _accel_diff[3][2] {};			/**< filtered accel differences between IMU units (m/s/s) */
 	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 */
 
 	uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {};	/**< latest full timestamp */
 

src/modules/simulator/simulator.cpp

@@ -64,12 +64,6 @@ void Simulator::parameters_update(bool force)
 	}
 }
 
-void Simulator::print_status()
-{
-	PX4_INFO("magnetometer");
-	_px4_mag.print_status();
-}
-
 int Simulator::start(int argc, char *argv[])
 {
 	_instance = new Simulator();
@@ -154,7 +148,7 @@ int simulator_main(int argc, char *argv[])
 			return 1;
 
 		} else {
-			Simulator::getInstance()->print_status();
+			PX4_INFO("running");
 		}
 
 	} else {

src/modules/simulator/simulator.h

@@ -127,8 +127,6 @@ public:
 	bool has_initialized() { return _has_initialized.load(); }
 #endif
 
-	void print_status();
-
 private:
 	Simulator() : ModuleParams(nullptr)
 	{