accel and gyro calibration refactor and cleanup

- remove all remaining IOCTLs for accel and gyro and handle all calibration entirely in sensors module with parameters
 - sensor_accel and sensor_gyro are now always raw sensor data
 - calibration procedures no longer need to first clear existing values before starting
 - temperature calibration (TC) remove all scale (SCL) parameters
    - gyro and baro scale are completely unused
    - regular accel calibration scale can be used (CAL_ACC*_xSCALE) instead of TC scale

src/modules/sensors/CMakeLists.txt

@@ -31,7 +31,7 @@
 #
 ############################################################################
 
-add_subdirectory(sensor_corrections) # used by vehicle_{acceleration, angular_velocity, imu}
+add_subdirectory(sensor_calibration) # used by vehicle_{acceleration, angular_velocity, imu}
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)

src/modules/sensors/sensor_calibration/CMakeLists.txt

@@ -31,8 +31,9 @@
 #
 ############################################################################
 
-px4_add_library(sensor_corrections
-	SensorCorrections.cpp
-	SensorCorrections.hpp
+px4_add_library(sensor_calibration
+	SensorCalibration.cpp
+	SensorCalibration.hpp
 )
-target_compile_options(sensor_corrections PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(sensor_calibration PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(sensor_calibration PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..)

src/modules/sensors/sensor_calibration/SensorCalibration.cpp

@@ -0,0 +1,252 @@
+/****************************************************************************
+ *
+ *   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 "SensorCalibration.hpp"
+
+#include <lib/parameters/param.h>
+
+using namespace matrix;
+using namespace time_literals;
+using math::radians;
+
+namespace sensors
+{
+
+void SensorCalibration::set_device_id(uint32_t device_id)
+{
+	if (_device_id != device_id) {
+		_device_id = device_id;
+		SensorCorrectionsUpdate(true);
+		ParametersUpdate();
+	}
+}
+
+matrix::Vector3f SensorCalibration::Correct(const matrix::Vector3f &data)
+{
+	SensorCorrectionsUpdate();
+	return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
+}
+
+const char *SensorCalibration::SensorString() const
+{
+	switch (_type) {
+	case SensorType::Accelerometer:
+		return "ACC";
+
+	case SensorType::Gyroscope:
+		return "GYRO";
+	}
+
+	return nullptr;
+}
+
+int SensorCalibration::FindCalibrationIndex(uint32_t device_id) const
+{
+	if (device_id == 0) {
+		return -1;
+	}
+
+	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
+		char str[16] {};
+		sprintf(str, "CAL_%s%u_ID", SensorString(), i);
+
+		int32_t device_id_val = 0;
+
+		if (param_get(param_find(str), &device_id_val) != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id_val == device_id) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+bool SensorCalibration::CalibrationEnabled(uint8_t calibration_index) const
+{
+	char str[20] {};
+	sprintf(str, "CAL_%s%u_EN", SensorString(), calibration_index);
+	int32_t enabled_val = 1;
+	param_get(param_find(str), &enabled_val);
+
+	return (enabled_val != 0);
+}
+
+Vector3f SensorCalibration::CalibrationOffset(uint8_t calibration_index) const
+{
+	// offsets (x, y, z)
+	Vector3f offset{0.f, 0.f, 0.f};
+	char str[20] {};
+
+	sprintf(str, "CAL_%s%u_XOFF", SensorString(), calibration_index);
+	param_get(param_find(str), &offset(0));
+
+	sprintf(str, "CAL_%s%u_YOFF", SensorString(), calibration_index);
+	param_get(param_find(str), &offset(1));
+
+	sprintf(str, "CAL_%s%u_ZOFF", SensorString(), calibration_index);
+	param_get(param_find(str), &offset(2));
+
+	return offset;
+}
+
+Vector3f SensorCalibration::CalibrationScale(uint8_t calibration_index) const
+{
+	// scale factors (x, y, z)
+	Vector3f scale{1.f, 1.f, 1.f};
+
+	// gyroscope doesn't have a scale factor calibration
+	if (_type != SensorType::Gyroscope) {
+		char str[20] {};
+
+		sprintf(str, "CAL_%s%u_XSCALE", SensorString(), calibration_index);
+		param_get(param_find(str), &scale(0));
+
+		sprintf(str, "CAL_%s%u_YSCALE", SensorString(), calibration_index);
+		param_get(param_find(str), &scale(1));
+
+		sprintf(str, "CAL_%s%u_ZSCALE", SensorString(), calibration_index);
+		param_get(param_find(str), &scale(2));
+	}
+
+	return scale;
+}
+
+void SensorCalibration::SensorCorrectionsUpdate(bool force)
+{
+	// check if the selected sensor has updated
+	if (_sensor_correction_sub.updated() || force) {
+
+		// valid device id required
+		if (_device_id == 0) {
+			return;
+		}
+
+		sensor_correction_s corrections;
+
+		if (_sensor_correction_sub.copy(&corrections)) {
+			// find sensor_corrections index
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if ((_type == SensorType::Accelerometer) && (corrections.accel_device_ids[i] == _device_id)) {
+					switch (i) {
+					case 0:
+						_thermal_offset = Vector3f{corrections.accel_offset_0};
+						return;
+					case 1:
+						_thermal_offset = Vector3f{corrections.accel_offset_1};
+						return;
+					case 2:
+						_thermal_offset = Vector3f{corrections.accel_offset_2};
+						return;
+					}
+
+				} else if ((_type == SensorType::Gyroscope) && (corrections.gyro_device_ids[i] == _device_id)) {
+					switch (i) {
+					case 0:
+						_thermal_offset = Vector3f{corrections.gyro_offset_0};
+						return;
+					case 1:
+						_thermal_offset = Vector3f{corrections.gyro_offset_1};
+						return;
+					case 2:
+						_thermal_offset = Vector3f{corrections.gyro_offset_2};
+						return;
+					}
+				}
+			}
+		}
+
+		// zero thermal offset if not found
+		_thermal_offset.zero();
+	}
+}
+
+void SensorCalibration::ParametersUpdate()
+{
+	if (!_external) {
+		// fine tune the rotation
+		float x_offset = 0.f;
+		float y_offset = 0.f;
+		float z_offset = 0.f;
+		param_get(param_find("SENS_BOARD_X_OFF"), &x_offset);
+		param_get(param_find("SENS_BOARD_Y_OFF"), &y_offset);
+		param_get(param_find("SENS_BOARD_Z_OFF"), &z_offset);
+
+		const Dcmf board_rotation_offset(Eulerf(radians(x_offset), radians(y_offset), radians(z_offset)));
+
+		// get transformation matrix from sensor/board to body frame
+		int32_t board_rot = 0;
+		param_get(param_find("SENS_BOARD_ROT"), &board_rot);
+		_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rot);
+
+	} else {
+		// TODO: per sensor external rotation
+		_rotation.setIdentity();
+	}
+
+
+	int calibration_index = FindCalibrationIndex(_device_id);
+
+	if (calibration_index >= 0) {
+		_enabled = CalibrationEnabled(calibration_index);
+		_offset = CalibrationOffset(calibration_index);
+		_scale = CalibrationScale(calibration_index);
+
+	} else {
+		_enabled = true;
+		_offset.zero();
+		_scale = Vector3f{1.f, 1.f, 1.f};
+	}
+}
+
+void SensorCalibration::PrintStatus()
+{
+	if (_type != SensorType::Gyroscope) {
+		PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f] scale: [%.4f %.4f %.4f]", SensorString(), _device_id, _enabled,
+			 (double)_offset(0), (double)_offset(1), (double)_offset(2), (double)_scale(0), (double)_scale(1), (double)_scale(2));
+
+	} else {
+		PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f]", SensorString(), _device_id, _enabled,
+			 (double)_offset(0), (double)_offset(1), (double)_offset(2));
+	}
+
+	if (_thermal_offset.norm() > 0.f) {
+		PX4_INFO("%s %d temperature offset: [%.4f %.4f %.4f]", SensorString(), _device_id,
+			 (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2));
+	}
+}
+
+} // namespace sensors

src/modules/sensors/sensor_calibration/SensorCalibration.hpp

@@ -37,14 +37,13 @@
 #include <lib/matrix/matrix/math.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/log.h>
-#include <px4_platform_common/module_params.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_correction.h>
 
 namespace sensors
 {
 
-class SensorCorrections : public ModuleParams
+class SensorCalibration
 {
 public:
 
@@ -53,49 +52,55 @@ public:
 		Gyroscope,
 	};
 
-	SensorCorrections() = delete;
-	SensorCorrections(ModuleParams *parent, SensorType type);
-	~SensorCorrections() override = default;
+	SensorCalibration() = delete;
+	explicit SensorCalibration(SensorType type) : _type(type) {}
+	~SensorCalibration() = default;
 
 	void PrintStatus();
 
 	void set_device_id(uint32_t device_id);
-	uint32_t get_device_id() const { return _device_id; }
+	void set_external(bool external = true) { _external = external; }
+
+	uint32_t device_id() const { return _device_id; }
+	bool enabled() const { return _enabled; }
+	bool external() const { return _external; }
 
 	// apply offsets and scale
 	// rotate corrected measurements from sensor to body frame
-	matrix::Vector3f Correct(const matrix::Vector3f &data) const { return _board_rotation * matrix::Vector3f{(data - _offset).emult(_scale)}; }
+	matrix::Vector3f Correct(const matrix::Vector3f &data);
 
 	void ParametersUpdate();
 	void SensorCorrectionsUpdate(bool force = false);
 
-	const matrix::Dcmf &getBoardRotation() const { return _board_rotation; }
+	const matrix::Dcmf &getBoardRotation() const { return _rotation; }
 
 private:
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
+	int FindCalibrationIndex(uint32_t device_id) const;
+
+	bool CalibrationEnabled(uint8_t calibration_index) const;
+	matrix::Vector3f CalibrationOffset(uint8_t calibration_index) const;
+	matrix::Vector3f CalibrationScale(uint8_t calibration_index) const;
+
 	const char *SensorString() const;
 
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
-	matrix::Dcmf _board_rotation;
+	matrix::Dcmf _rotation;
 
 	matrix::Vector3f _offset{0.f, 0.f, 0.f};
 	matrix::Vector3f _scale{1.f, 1.f, 1.f};
 
+	matrix::Vector3f _thermal_offset{0.f, 0.f, 0.f};
+
 	uint32_t _device_id{0};
-	int8_t _corrections_selected_instance{-1};
 
 	const SensorType _type;
 
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
-
-		(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
-		(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
-		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
-	)
+	bool _enabled{true};
+	bool _external{false};
 };
 
 } // namespace sensors

src/modules/sensors/sensor_corrections/SensorCorrections.cpp

@@ -1,170 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SensorCorrections.hpp"
-
-using namespace matrix;
-using namespace time_literals;
-using math::radians;
-
-namespace sensors
-{
-
-SensorCorrections::SensorCorrections(ModuleParams *parent, SensorType type) :
-	ModuleParams(parent),
-	_type(type)
-{
-}
-
-void SensorCorrections::set_device_id(uint32_t device_id)
-{
-	if (_device_id != device_id) {
-		_device_id = device_id;
-		SensorCorrectionsUpdate(true);
-	}
-}
-
-const char *SensorCorrections::SensorString() const
-{
-	switch (_type) {
-	case SensorType::Accelerometer:
-		return "ACC";
-
-	case SensorType::Gyroscope:
-		return "GYRO";
-	}
-
-	return nullptr;
-}
-
-void SensorCorrections::SensorCorrectionsUpdate(bool force)
-{
-	// check if the selected sensor has updated
-	if (_sensor_correction_sub.updated() || force) {
-
-		sensor_correction_s corrections;
-
-		if (_sensor_correction_sub.copy(&corrections)) {
-
-			// selected sensor has changed, find updated index
-			if ((_corrections_selected_instance < 0) || force) {
-				_corrections_selected_instance = -1;
-
-				// find sensor_corrections index
-				for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-
-					switch (_type) {
-					case SensorType::Accelerometer:
-						if (corrections.accel_device_ids[i] == _device_id) {
-							_corrections_selected_instance = i;
-						}
-
-						break;
-
-					case SensorType::Gyroscope:
-						if (corrections.gyro_device_ids[i] == _device_id) {
-							_corrections_selected_instance = i;
-						}
-
-						break;
-					}
-				}
-			}
-
-			switch (_type) {
-			case SensorType::Accelerometer:
-				switch (_corrections_selected_instance) {
-				case 0:
-					_offset = Vector3f{corrections.accel_offset_0};
-					_scale = Vector3f{corrections.accel_scale_0};
-					return;
-				case 1:
-					_offset = Vector3f{corrections.accel_offset_1};
-					_scale = Vector3f{corrections.accel_scale_1};
-					return;
-				case 2:
-					_offset = Vector3f{corrections.accel_offset_2};
-					_scale = Vector3f{corrections.accel_scale_2};
-					return;
-				}
-
-				break;
-
-			case SensorType::Gyroscope:
-				switch (_corrections_selected_instance) {
-				case 0:
-					_offset = Vector3f{corrections.gyro_offset_0};
-					_scale = Vector3f{corrections.gyro_scale_0};
-					return;
-				case 1:
-					_offset = Vector3f{corrections.gyro_offset_1};
-					_scale = Vector3f{corrections.gyro_scale_1};
-					return;
-				case 2:
-					_offset = Vector3f{corrections.gyro_offset_2};
-					_scale = Vector3f{corrections.gyro_scale_2};
-					return;
-				}
-
-				break;
-			}
-		}
-	}
-}
-
-void SensorCorrections::ParametersUpdate()
-{
-	// fine tune the rotation
-	const Dcmf board_rotation_offset(Eulerf(
-			radians(_param_sens_board_x_off.get()),
-			radians(_param_sens_board_y_off.get()),
-			radians(_param_sens_board_z_off.get())));
-
-	// get transformation matrix from sensor/board to body frame
-	_board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
-}
-
-void SensorCorrections::PrintStatus()
-{
-	if (_offset.norm() > 0.f) {
-		PX4_INFO("%s %d offset: [%.3f %.3f %.3f]", SensorString(), _device_id, (double)_offset(0), (double)_offset(1),
-			 (double)_offset(2));
-	}
-
-	if (fabsf(_scale.norm_squared() - 3.f) > FLT_EPSILON) {
-		PX4_INFO("%s %d scale: [%.3f %.3f %.3f]", SensorString(), _device_id, (double)_scale(0), (double)_scale(1),
-			 (double)_scale(2));
-	}
-}
-
-} // namespace sensors

src/modules/sensors/sensors.cpp

@@ -239,6 +239,8 @@ Sensors::~Sensors()
 			i->Stop();
 		}
 	}
+
+	perf_free(_loop_perf);
 }
 
 bool Sensors::init()

src/modules/sensors/vehicle_acceleration/CMakeLists.txt

@@ -41,6 +41,6 @@ target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(vehicle_acceleration
 	PRIVATE
 		mathlib
-		sensor_corrections
+		sensor_calibration
 		px4_work_queue
 )

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -43,8 +43,7 @@ namespace sensors
 
 VehicleAcceleration::VehicleAcceleration() :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers),
-	_corrections(this, SensorCorrections::SensorType::Accelerometer)
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers)
 {
 	_lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get());
 }
@@ -182,7 +181,7 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
 						// clear bias and corrections
 						_bias.zero();
 
-						_corrections.set_device_id(report.device_id);
+						_calibration.set_device_id(report.device_id);
 
 						// reset sample interval accumulator on sensor change
 						_timestamp_sample_last = 0;
@@ -212,7 +211,7 @@ void VehicleAcceleration::ParametersUpdate(bool force)
 
 		updateParams();
 
-		_corrections.ParametersUpdate();
+		_calibration.ParametersUpdate();
 	}
 }
 
@@ -224,7 +223,7 @@ void VehicleAcceleration::Run()
 	// update corrections first to set _selected_sensor
 	bool selection_updated = SensorSelectionUpdate();
 
-	_corrections.SensorCorrectionsUpdate(selection_updated);
+	_calibration.SensorCorrectionsUpdate(selection_updated);
 	SensorBiasUpdate(selection_updated);
 	ParametersUpdate();
 
@@ -264,7 +263,7 @@ void VehicleAcceleration::Run()
 
 			if (!sensor_updated) {
 				// correct for in-run bias errors
-				const Vector3f accel = _corrections.Correct(accel_filtered) - _bias;
+				const Vector3f accel = _calibration.Correct(accel_filtered) - _bias;
 
 				// Publish vehicle_acceleration
 				vehicle_acceleration_s v_acceleration;
@@ -283,8 +282,9 @@ void VehicleAcceleration::PrintStatus()
 {
 	PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
 		 _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
-	PX4_INFO("estimated bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
-	_corrections.PrintStatus();
+	PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
+
+	_calibration.PrintStatus();
 }
 
 } // namespace sensors

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include <sensor_corrections/SensorCorrections.hpp>
+#include <sensor_calibration/SensorCalibration.hpp>
 
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
@@ -88,7 +88,7 @@ private:
 		{this, ORB_ID(sensor_accel), 2}
 	};
 
-	SensorCorrections _corrections;
+	SensorCalibration _calibration{SensorCalibration::SensorType::Accelerometer};
 
 	matrix::Vector3f _bias{0.f, 0.f, 0.f};
 

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -72,44 +72,36 @@ void VehicleAirData::Stop()
 	}
 }
 
-void VehicleAirData::SensorCorrectionsUpdate()
+void VehicleAirData::SensorCorrectionsUpdate(bool force)
 {
-	// check if the selected sensor has updated
-	sensor_correction_s corrections;
+	if (_sensor_correction_sub.updated() || force) {
+		sensor_correction_s corrections;
 
-	if (_sensor_correction_sub.update(&corrections)) {
-		for (int baro_index = 0; baro_index < MAX_SENSOR_COUNT; baro_index++) {
-			const sensor_baro_s &baro = _last_data[baro_index];
+		if (_sensor_correction_sub.copy(&corrections)) {
+			for (int baro_index = 0; baro_index < MAX_SENSOR_COUNT; baro_index++) {
+				// find sensor (by device id) in sensor_correction
+				const uint32_t device_id = _last_data[baro_index].device_id;
 
-			// find sensor (by device id) in sensor_correction
-			if (_sensor_correction_index[baro_index] < 0) {
-				for (int correction_index = 0; correction_index < MAX_SENSOR_COUNT; correction_index++) {
-					if ((baro.device_id > 0) && (corrections.baro_device_ids[correction_index] == baro.device_id)) {
-						_sensor_correction_index[baro_index] = correction_index;
+				if (device_id != 0) {
+					for (int correction_index = 0; correction_index < MAX_SENSOR_COUNT; correction_index++) {
+						if (corrections.baro_device_ids[correction_index] == device_id) {
+							switch (correction_index) {
+							case 0:
+								_thermal_offset[baro_index] = corrections.baro_offset_0;
+								break;
+
+							case 1:
+								_thermal_offset[baro_index] = corrections.baro_offset_1;
+								break;
+
+							case 2:
+								_thermal_offset[baro_index] = corrections.baro_offset_2;
+								break;
+							}
+						}
 					}
 				}
 			}
-
-			switch (_sensor_correction_index[baro_index]) {
-			case 0:
-				_offset[baro_index] = corrections.baro_offset_0;
-				_scale[baro_index] = corrections.baro_scale_0;
-				break;
-
-			case 1:
-				_offset[baro_index] = corrections.baro_offset_1;
-				_scale[baro_index] = corrections.baro_scale_1;
-				break;
-
-			case 2:
-				_offset[baro_index] = corrections.baro_offset_2;
-				_scale[baro_index] = corrections.baro_scale_2;
-				break;
-
-			default:
-				_offset[baro_index] = 0.f;
-				_scale[baro_index] = 1.f;
-			}
 		}
 	}
 }
@@ -130,6 +122,8 @@ void VehicleAirData::Run()
 {
 	perf_begin(_cycle_perf);
 
+	SensorCorrectionsUpdate();
+
 	bool updated[MAX_SENSOR_COUNT] {};
 
 	for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
@@ -147,6 +141,9 @@ void VehicleAirData::Run()
 
 					_advertised[uorb_index] = true;
 
+					// force temperature correction update
+					SensorCorrectionsUpdate(true);
+
 				} else {
 					_last_data[uorb_index].timestamp = hrt_absolute_time();
 				}
@@ -164,8 +161,8 @@ void VehicleAirData::Run()
 				// millibar to Pa
 				const float raw_pressure_pascals = _last_data[uorb_index].pressure * 100.f;
 
-				// pressure corrected with offset and scale (if available)
-				const float pressure_corrected = (raw_pressure_pascals - _offset[uorb_index]) * _scale[uorb_index];
+				// pressure corrected with offset (if available)
+				const float pressure_corrected = (raw_pressure_pascals - _thermal_offset[uorb_index]);
 
 				float vect[3] {pressure_corrected, _last_data[uorb_index].temperature, 0.f};
 				_voter.put(uorb_index, _last_data[uorb_index].timestamp, vect, _last_data[uorb_index].error_count,
@@ -192,7 +189,6 @@ void VehicleAirData::Run()
 
 	if ((_selected_sensor_sub_index >= 0) && updated[_selected_sensor_sub_index]) {
 		ParametersUpdate();
-		SensorCorrectionsUpdate();
 
 		const sensor_baro_s &baro = _last_data[_selected_sensor_sub_index];
 
@@ -203,8 +199,7 @@ void VehicleAirData::Run()
 		out.baro_temp_celcius = baro.temperature;
 
 		// Convert from millibar to Pa and apply temperature compensation
-		out.baro_pressure_pa = (100.0f * baro.pressure - _offset[_selected_sensor_sub_index]) *
-				       _scale[_selected_sensor_sub_index];
+		out.baro_pressure_pa = 100.0f * baro.pressure - _thermal_offset[_selected_sensor_sub_index];
 
 		// calculate altitude using the hypsometric equation
 		static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
@@ -281,6 +276,11 @@ void VehicleAirData::PrintStatus()
 {
 	if (_selected_sensor_sub_index >= 0) {
 		PX4_INFO("selected barometer: %d (%d)", _last_data[_selected_sensor_sub_index].device_id, _selected_sensor_sub_index);
+
+		if (fabsf(_thermal_offset[_selected_sensor_sub_index]) > 0.f) {
+			PX4_INFO("%d temperature offset: %.4f", _last_data[_selected_sensor_sub_index].device_id,
+				 (double)_thermal_offset[_selected_sensor_sub_index]);
+		}
 	}
 
 	_voter.print();

src/modules/sensors/vehicle_air_data/VehicleAirData.hpp

@@ -66,7 +66,7 @@ private:
 	void Run() override;
 
 	void ParametersUpdate();
-	void SensorCorrectionsUpdate();
+	void SensorCorrectionsUpdate(bool force = false);
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
@@ -96,10 +96,8 @@ private:
 	sensor_baro_s _last_data[MAX_SENSOR_COUNT] {};
 	bool _advertised[MAX_SENSOR_COUNT] {};
 
-	float _offset[MAX_SENSOR_COUNT] {0.f, 0.f, 0.f};
-	float _scale[MAX_SENSOR_COUNT] {1.f, 1.f, 1.f};
+	float _thermal_offset[MAX_SENSOR_COUNT] {0.f, 0.f, 0.f};
 
-	int8_t _sensor_correction_index[MAX_SENSOR_COUNT] {-1, -1, -1};
 	uint8_t _priority[MAX_SENSOR_COUNT] {};
 
 	int8_t _selected_sensor_sub_index{-1};

src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt

@@ -41,6 +41,6 @@ target_compile_options(vehicle_angular_velocity PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(vehicle_angular_velocity
 	PRIVATE
 		mathlib
-		sensor_corrections
+		sensor_calibration
 		px4_work_queue
 )

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -43,8 +43,7 @@ namespace sensors
 
 VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
-	_corrections(this, SensorCorrections::SensorType::Gyroscope)
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 	_lp_filter_velocity.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get());
 	_notch_filter_velocity.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
@@ -202,7 +201,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 						// clear bias and corrections
 						_bias.zero();
 
-						_corrections.set_device_id(report.device_id);
+						_calibration.set_device_id(report.device_id);
 
 						// reset sample interval accumulator on sensor change
 						_timestamp_sample_last = 0;
@@ -232,7 +231,7 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 
 		updateParams();
 
-		_corrections.ParametersUpdate();
+		_calibration.ParametersUpdate();
 	}
 }
 
@@ -244,7 +243,7 @@ void VehicleAngularVelocity::Run()
 	// update corrections first to set _selected_sensor
 	bool selection_updated = SensorSelectionUpdate();
 
-	_corrections.SensorCorrectionsUpdate(selection_updated);
+	_calibration.SensorCorrectionsUpdate(selection_updated);
 	SensorBiasUpdate(selection_updated);
 	ParametersUpdate();
 
@@ -280,7 +279,7 @@ void VehicleAngularVelocity::Run()
 			const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
 
 			// correct for in-run bias errors
-			const Vector3f angular_velocity_raw = _corrections.Correct(val) - _bias;
+			const Vector3f angular_velocity_raw = _calibration.Correct(val) - _bias;
 
 			// Gyro filtering:
 			// - Apply general notch filter (IMU_GYRO_NF_FREQ)
@@ -339,8 +338,8 @@ void VehicleAngularVelocity::PrintStatus()
 {
 	PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
 		 _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
-	PX4_INFO("estimated bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
-	_corrections.PrintStatus();
+	PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
+	_calibration.PrintStatus();
 }
 
 } // namespace sensors

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include <sensor_corrections/SensorCorrections.hpp>
+#include <sensor_calibration/SensorCalibration.hpp>
 
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
@@ -91,7 +91,7 @@ private:
 		{this, ORB_ID(sensor_gyro), 2}
 	};
 
-	SensorCorrections _corrections;
+	SensorCalibration _calibration{SensorCalibration::SensorType::Gyroscope};
 
 	matrix::Vector3f _bias{0.f, 0.f, 0.f};
 

src/modules/sensors/vehicle_imu/CMakeLists.txt

@@ -38,4 +38,4 @@ px4_add_library(vehicle_imu
 	VehicleIMU.hpp
 )
 target_compile_options(vehicle_imu PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_link_libraries(vehicle_imu PRIVATE sensor_corrections px4_work_queue)
+target_link_libraries(vehicle_imu PRIVATE sensor_calibration px4_work_queue)

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -49,9 +49,7 @@ VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers),
 	_sensor_accel_sub(this, ORB_ID(sensor_accel), accel_index),
-	_sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index),
-	_accel_corrections(this, SensorCorrections::SensorType::Accelerometer),
-	_gyro_corrections(this, SensorCorrections::SensorType::Gyroscope)
+	_sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index)
 {
 	const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
 
@@ -107,8 +105,8 @@ void VehicleIMU::ParametersUpdate(bool force)
 
 		updateParams();
 
-		_accel_corrections.ParametersUpdate();
-		_gyro_corrections.ParametersUpdate();
+		_accel_calibration.ParametersUpdate();
+		_gyro_calibration.ParametersUpdate();
 
 		// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
 		int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), 50, 1000);
@@ -191,7 +189,7 @@ void VehicleIMU::Run()
 
 		_gyro_last_generation = _sensor_gyro_sub.get_last_generation();
 
-		_gyro_corrections.set_device_id(gyro.device_id);
+		_gyro_calibration.set_device_id(gyro.device_id);
 
 		if (gyro.error_count != _status.gyro_error_count) {
 			publish_status = true;
@@ -229,7 +227,7 @@ void VehicleIMU::Run()
 
 		_accel_last_generation = _sensor_accel_sub.get_last_generation();
 
-		_accel_corrections.set_device_id(accel.device_id);
+		_accel_calibration.set_device_id(accel.device_id);
 
 		if (accel.error_count != _status.accel_error_count) {
 			publish_status = true;
@@ -249,7 +247,7 @@ void VehicleIMU::Run()
 		if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
 
 			// rotate sensor clip counts into vehicle body frame
-			const Vector3f clipping{_accel_corrections.getBoardRotation() *
+			const Vector3f clipping{_accel_calibration.getBoardRotation() *
 				Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}};
 
 			// round to get reasonble clip counts per axis (after board rotation)
@@ -301,14 +299,14 @@ void VehicleIMU::Run()
 		    && _gyro_integrator.reset(delta_angle, gyro_integral_dt)) {
 
 			// delta angle: apply offsets, scale, and board rotation
-			_gyro_corrections.SensorCorrectionsUpdate();
-			const float gyro_dt = 1.e-6f * gyro_integral_dt;
-			const Vector3f delta_angle_corrected{_gyro_corrections.Correct(delta_angle * gyro_dt) / gyro_dt};
+			_gyro_calibration.SensorCorrectionsUpdate();
+			const float gyro_dt_inv = 1.e6f / gyro_integral_dt;
+			const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv};
 
 			// delta velocity: apply offsets, scale, and board rotation
-			_accel_corrections.SensorCorrectionsUpdate();
-			const float accel_dt = 1.e-6f * accel_integral_dt;
-			Vector3f delta_velocity_corrected{_accel_corrections.Correct(delta_velocity * accel_dt) / accel_dt};
+			_accel_calibration.SensorCorrectionsUpdate();
+			const float accel_dt_inv = 1.e6f / accel_integral_dt;
+			Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv};
 
 			UpdateAccelVibrationMetrics(delta_velocity_corrected);
 			UpdateGyroVibrationMetrics(delta_angle_corrected);
@@ -316,8 +314,8 @@ void VehicleIMU::Run()
 			// vehicle_imu_status
 			//  publish before vehicle_imu so that error counts are available synchronously if needed
 			if (publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) {
-				_status.accel_device_id = _accel_corrections.get_device_id();
-				_status.gyro_device_id = _gyro_corrections.get_device_id();
+				_status.accel_device_id = _accel_calibration.device_id();
+				_status.gyro_device_id = _gyro_calibration.device_id();
 				_status.timestamp = hrt_absolute_time();
 				_vehicle_imu_status_pub.publish(_status);
 			}
@@ -326,8 +324,8 @@ void VehicleIMU::Run()
 			// publish vehicle_imu
 			vehicle_imu_s imu;
 			imu.timestamp_sample = _last_timestamp_sample_gyro;
-			imu.accel_device_id = _accel_corrections.get_device_id();
-			imu.gyro_device_id = _gyro_corrections.get_device_id();
+			imu.accel_device_id = _accel_calibration.device_id();
+			imu.gyro_device_id = _gyro_calibration.device_id();
 			delta_angle_corrected.copyTo(imu.delta_angle);
 			delta_velocity_corrected.copyTo(imu.delta_velocity);
 			imu.delta_angle_dt = gyro_integral_dt;
@@ -376,7 +374,7 @@ void VehicleIMU::UpdateIntegratorConfiguration()
 		_intervals_update = false; // stop monitoring topic publication rate
 
 		PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d, accel interval: %.1f, gyro interval: %.1f",
-			  _accel_corrections.get_device_id(), _gyro_corrections.get_device_id(), accel_integral_samples, gyro_integral_samples,
+			  _accel_calibration.device_id(), _gyro_calibration.device_id(), accel_integral_samples, gyro_integral_samples,
 			  (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval);
 	}
 }
@@ -406,16 +404,16 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle)
 void VehicleIMU::PrintStatus()
 {
 	PX4_INFO("Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us",
-		 _accel_corrections.get_device_id(), (double)_accel_interval.update_interval,
-		 _gyro_corrections.get_device_id(), (double)_gyro_interval.update_interval);
+		 _accel_calibration.device_id(), (double)_accel_interval.update_interval,
+		 _gyro_calibration.device_id(), (double)_gyro_interval.update_interval);
 
 	perf_print_counter(_accel_generation_gap_perf);
 	perf_print_counter(_gyro_generation_gap_perf);
 	perf_print_counter(_accel_update_perf);
 	perf_print_counter(_gyro_update_perf);
 
-	_accel_corrections.PrintStatus();
-	_gyro_corrections.PrintStatus();
+	_accel_calibration.PrintStatus();
+	_gyro_calibration.PrintStatus();
 }
 
 } // namespace sensors

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -35,7 +35,7 @@
 
 #include "Integrator.hpp"
 
-#include <sensor_corrections/SensorCorrections.hpp>
+#include <sensor_calibration/SensorCalibration.hpp>
 
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
@@ -91,8 +91,8 @@ private:
 	uORB::SubscriptionCallbackWorkItem _sensor_accel_sub;
 	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub;
 
-	SensorCorrections _accel_corrections;
-	SensorCorrections _gyro_corrections;
+	SensorCalibration _accel_calibration{SensorCalibration::SensorType::Accelerometer};
+	SensorCalibration _gyro_calibration{SensorCalibration::SensorType::Gyroscope};
 
 	Integrator _accel_integrator{}; // 200 Hz default
 	Integrator _gyro_integrator{true};   // 200 Hz default, coning compensation enabled

src/modules/sensors/voted_sensors_update.cpp

@@ -39,11 +39,12 @@
 
 #include "voted_sensors_update.h"
 
-#include <systemlib/mavlink_log.h>
-
+#include <lib/conversion/rotation.h>
+#include <lib/ecl/geo/geo.h>
+#include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
-#include <conversion/rotation.h>
-#include <ecl/geo/geo.h>
+#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"
@@ -91,6 +92,8 @@ void VotedSensorsUpdate::initializeSensors()
 
 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])}};
 
@@ -101,211 +104,44 @@ void VotedSensorsUpdate::parametersUpdate()
 		_mag_rotation[topic_instance] = _board_rotation;
 	}
 
-	updateParams();
-
 	/* set offset parameters to new values */
 	bool failed = false;
 
-	/* run through all gyro sensors */
-	unsigned gyro_count = 0;
-	unsigned gyro_cal_found_count = 0;
+	// 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};
+		imu.update();
 
-	for (unsigned driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) {
-		_gyro.enabled[driver_index] = true;
+		if (imu.get().timestamp > 0 && imu.get().accel_device_id > 0 && imu.get().gyro_device_id > 0) {
 
-		char str[30] {};
-		(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, driver_index);
+			if (_accel.priority[uorb_index] == ORB_PRIO_UNINITIALIZED) {
+				// find corresponding sensor_accel publication
+				for (uint8_t i = 0; i < ACCEL_COUNT_MAX; i++) {
+					uORB::SubscriptionData<sensor_accel_s> sensor_accel{ORB_ID(sensor_accel), i};
+					sensor_accel.update();
 
-		int fd = px4_open(str, O_RDWR);
-
-		if (fd < 0) {
-			continue;
-		}
-
-		uint32_t driver_device_id = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-		bool config_ok = false;
-
-		/* run through all stored calibrations that are applied at the driver level*/
-		for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) {
-			/* initially status is ok per config */
-			failed = false;
-
-			(void)sprintf(str, "CAL_GYRO%u_ID", i);
-			int32_t device_id = 0;
-			failed = failed || (PX4_OK != param_get(param_find(str), &device_id));
-
-			(void)sprintf(str, "CAL_GYRO%u_EN", i);
-			int32_t device_enabled = 1;
-			failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled));
-
-			if (failed) {
-				continue;
-			}
-
-			if (driver_index == 0 && device_id > 0) {
-				gyro_cal_found_count++;
-			}
-
-			/* if the calibration is for this device, apply it */
-			if ((uint32_t)device_id == driver_device_id) {
-				_gyro.enabled[driver_index] = (device_enabled == 1);
-
-				if (!_gyro.enabled[driver_index]) {
-					_gyro.priority[driver_index] = ORB_PRIO_UNINITIALIZED;
-				}
-
-				gyro_calibration_s gscale{};
-
-				(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &gscale.x_offset));
-
-				(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &gscale.y_offset));
-
-				(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &gscale.z_offset));
-
-				if (failed) {
-					PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
-
-				} else {
-					/* apply new scaling and offsets */
-					config_ok = (px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale) == 0);
-
-					if (!config_ok) {
-						PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i);
+					if (imu.get().accel_device_id == sensor_accel.get().device_id) {
+						_accel.priority[uorb_index] = sensor_accel.get_priority();
+						break;
 					}
 				}
-
-				break;
-			}
-		}
-
-		if (config_ok) {
-			gyro_count++;
-		}
-
-		px4_close(fd);
-	}
-
-	// There are fewer gyros than calibrations
-	// reset the board calibration and fail the initial load
-	if (gyro_count < gyro_cal_found_count) {
-		PX4_ERR("fewer accels than calibrations, resetting all CAL_GYROx_ID");
-
-		// run through all stored calibrations and reset them
-		for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) {
-			char str[30] {};
-			(void)sprintf(str, "CAL_GYRO%u_ID", i);
-			int32_t device_id = 0;
-			(void)param_set(param_find(str), &device_id);
-		}
-	}
-
-	/* run through all accel sensors */
-	unsigned accel_count = 0;
-	unsigned accel_cal_found_count = 0;
-
-	for (unsigned driver_index = 0; driver_index < ACCEL_COUNT_MAX; driver_index++) {
-		_accel.enabled[driver_index] = true;
-
-		char str[30] {};
-		(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, driver_index);
-
-		int fd = px4_open(str, O_RDWR);
-
-		if (fd < 0) {
-			continue;
-		}
-
-		uint32_t driver_device_id = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-		bool config_ok = false;
-
-		/* run through all stored calibrations */
-		for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) {
-			/* initially status is ok per config */
-			failed = false;
-
-			(void)sprintf(str, "CAL_ACC%u_ID", i);
-			int32_t device_id = 0;
-			failed = failed || (PX4_OK != param_get(param_find(str), &device_id));
-
-			(void)sprintf(str, "CAL_ACC%u_EN", i);
-			int32_t device_enabled = 1;
-			failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled));
-
-			if (failed) {
-				continue;
 			}
 
-			if (driver_index == 0 && device_id > 0) {
-				accel_cal_found_count++;
-			}
+			if (_gyro.priority[uorb_index] == ORB_PRIO_UNINITIALIZED) {
+				// find corresponding sensor_gyro publication
+				for (uint8_t i = 0; i < GYRO_COUNT_MAX; i++) {
+					uORB::SubscriptionData<sensor_accel_s> sensor_gyro{ORB_ID(sensor_gyro), i};
+					sensor_gyro.update();
 
-			/* if the calibration is for this device, apply it */
-			if ((uint32_t)device_id == driver_device_id) {
-				_accel.enabled[driver_index] = (device_enabled == 1);
-
-				if (!_accel.enabled[driver_index]) {
-					_accel.priority[driver_index] = ORB_PRIO_UNINITIALIZED;
-				}
-
-				accel_calibration_s ascale{};
-
-				(void)sprintf(str, "CAL_ACC%u_XOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.x_offset));
-
-				(void)sprintf(str, "CAL_ACC%u_YOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.y_offset));
-
-				(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.z_offset));
-
-				(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.x_scale));
-
-				(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.y_scale));
-
-				(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.z_scale));
-
-				if (failed) {
-					PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
-
-				} else {
-					/* apply new scaling and offsets */
-					config_ok = (px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale) == 0);
-
-					if (!config_ok) {
-						PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel ", i);
+					if (imu.get().gyro_device_id == sensor_gyro.get().device_id) {
+						_gyro.priority[uorb_index] = sensor_gyro.get_priority();
+						break;
 					}
 				}
-
-				break;
 			}
 		}
-
-		if (config_ok) {
-			accel_count++;
-		}
-
-		px4_close(fd);
 	}
 
-	// There are fewer accels than calibrations
-	// reset the board calibration and fail the initial load
-	if (accel_count < accel_cal_found_count) {
-		PX4_ERR("fewer accels than calibrations, resetting all CAL_ACCx_ID");
-
-		// run through all stored calibrations and reset them
-		for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) {
-			char str[30] {};
-			(void)sprintf(str, "CAL_ACC%u_ID", i);
-			int32_t device_id = 0;
-			(void)param_set(param_find(str), &device_id);
-		}
-	}
 
 	/* 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

src/modules/sensors/voted_sensors_update.h

@@ -41,8 +41,6 @@
 
 #include "parameters.h"
 
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>