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.