split sensor calibration and configuration

- introduce SENS_GNSS instance configuration and GPS blending updates
 - SENS_GNSSx_ID
 - SENS_GNSSx_{X,Y,Z}POS

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -143,9 +143,12 @@ then
 	param set CAL_GYRO2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
 
 	param set CAL_MAG0_ID 197388
-	param set CAL_MAG0_PRIO 50
+	param set SENS_MAG0_ID 197388
+	param set SENS_MAG0_PRIO 50
+
 	param set CAL_MAG1_ID 197644
-	param set CAL_MAG1_PRIO 50
+	param set SENS_MAG1_ID 197644
+	param set SENS_MAG1_PRIO 50
 
 	param set SENS_BOARD_X_OFF 0.000001
 	param set SENS_DPRES_OFF 0.001

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -31,7 +31,7 @@ param set-default BAT1_R_INTERNAL 0.0025
 param set-default CBRK_AIRSPD_CHK 162128
 param set-default CBRK_IO_SAFETY 22027
 
-param set-default EKF2_GPS_POS_X -0.12
+param set-default SENS_GNSS0_XPOS -0.12
 param set-default EKF2_IMU_POS_X -0.12
 
 param set-default FW_ARSP_MODE 1

ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision

@@ -18,8 +18,8 @@ param set-default COM_DISARM_LAND 0.5
 # EKF2 parameters
 param set-default EKF2_DRAG_CTRL 1
 param set-default EKF2_IMU_POS_X 0.02
-param set-default EKF2_GPS_POS_X 0.055
+param set-default SENS_GNSS0_XPOS 0.055
-param set-default EKF2_GPS_POS_Z -0.15
+param set-default SENS_GNSS0_ZPOS -0.15
 param set-default EKF2_MIN_RNG 0.03
 param set-default EKF2_OF_CTRL 1
 param set-default EKF2_OF_POS_X 0.055

ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu

@@ -46,7 +46,9 @@ param set-default EKF2_BCOEF_Y 25.5
 param set-default EKF2_DRAG_CTRL 1
 
 param set-default EKF2_GPS_DELAY 100
-param set-default EKF2_GPS_POS_X 0.06
+param set-default SENS_GNSS0_XPOS 0.06
+param set-default SENS_GNSS0_YPOS 0.0
+param set-default SENS_GNSS0_ZPOS 0.0
 param set-default EKF2_GPS_V_NOISE 0.5
 
 param set-default EKF2_IMU_POS_X 0.06

msg/SensorGps.msg

@@ -62,4 +62,6 @@ uint8 RTCM_MSG_USED_NOT_USED = 1
 uint8 RTCM_MSG_USED_USED = 2
 uint8 rtcm_msg_used		# Indicates if the RTCM message was used successfully by the receiver
 
+float32[3] position_offset      # position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity)
+
 # TOPICS sensor_gps vehicle_gps_position

src/lib/CMakeLists.txt

@@ -67,7 +67,7 @@ add_subdirectory(pid_design EXCLUDE_FROM_ALL)
 add_subdirectory(rate_control EXCLUDE_FROM_ALL)
 add_subdirectory(rc EXCLUDE_FROM_ALL)
 add_subdirectory(ringbuffer EXCLUDE_FROM_ALL)
-add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL)
+add_subdirectory(sensor EXCLUDE_FROM_ALL)
 add_subdirectory(slew_rate EXCLUDE_FROM_ALL)
 add_subdirectory(systemlib EXCLUDE_FROM_ALL)
 add_subdirectory(system_identification EXCLUDE_FROM_ALL)

src/lib/parameters/param_translation.cpp

@@ -44,5 +44,24 @@ bool param_modify_on_import(bson_node_t node)
 {
 
 
+	// TODO:
+	// ACC, BARO, GYRO, MAG
+	//  prio, rot
+
+	// 2023-11-30: translate sensor calibration to configuration
+	{
+		if (strcmp("CAL_ACC0_PRIO", node->name) == 0) {
+			strcpy(node->name, "SENS_ACC0_PRIO");
+			PX4_INFO("copying %s -> %s", "CAL_ACC0_PRIO", "SENS_ACC0_PRIO");
+
+			// TODO: set SENS_ACC0_ID=CAL_ACC0_ID
+
+
+			return true;
+		}
+	}
+
+
+
 	return false;
 }

src/lib/sensor/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(sensor_utilities
+	Utilities.cpp
+	Utilities.hpp
+)
+
+target_link_libraries(sensor_utilities PRIVATE conversion)
+
+add_subdirectory(configuration)
+add_subdirectory(calibration)

src/lib/sensor/Utilities.cpp

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2022 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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <lib/conversion/rotation.h>
+#include <lib/drivers/device/Device.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/parameters/param.h>
+
+#if defined(CONFIG_I2C)
+# include <px4_platform_common/i2c.h>
+#endif // CONFIG_I2C
+
+#if defined(CONFIG_SPI)
+# include <px4_platform_common/spi.h>
+#endif // CONFIG_SPI
+
+using math::radians;
+using matrix::Eulerf;
+using matrix::Dcmf;
+using matrix::Vector3f;
+
+namespace sensor
+{
+namespace utilities
+{
+
+Eulerf GetSensorLevelAdjustment()
+{
+	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);
+
+	return Eulerf{radians(x_offset), radians(y_offset), radians(z_offset)};
+}
+
+enum Rotation GetBoardRotation()
+{
+	// get transformation matrix from sensor/board to body frame
+	int32_t board_rot = -1;
+	param_get(param_find("SENS_BOARD_ROT"), &board_rot);
+
+	if (board_rot >= 0 && board_rot <= Rotation::ROTATION_MAX) {
+		return static_cast<enum Rotation>(board_rot);
+
+	} else {
+		PX4_ERR("invalid SENS_BOARD_ROT: %" PRId32, board_rot);
+	}
+
+	return Rotation::ROTATION_NONE;
+}
+
+Dcmf GetBoardRotationMatrix()
+{
+	return get_rot_matrix(GetBoardRotation());
+}
+
+bool DeviceExternal(uint32_t device_id)
+{
+	bool external = true;
+
+	// decode device id to determine if external
+	union device::Device::DeviceId id {};
+	id.devid = device_id;
+
+	const device::Device::DeviceBusType bus_type = id.devid_s.bus_type;
+
+	switch (bus_type) {
+	case device::Device::DeviceBusType_I2C:
+#if defined(CONFIG_I2C)
+		external = px4_i2c_bus_external(id.devid_s.bus);
+#endif // CONFIG_I2C
+		break;
+
+	case device::Device::DeviceBusType_SPI:
+#if defined(CONFIG_SPI)
+		external = px4_spi_bus_external(id.devid_s.bus);
+#endif // CONFIG_SPI
+		break;
+
+	case device::Device::DeviceBusType_UAVCAN:
+		external = true;
+		break;
+
+	case device::Device::DeviceBusType_SIMULATION:
+		external = false;
+		break;
+
+	case device::Device::DeviceBusType_SERIAL:
+		external = true;
+		break;
+
+	case device::Device::DeviceBusType_MAVLINK:
+		external = true;
+		break;
+
+	case device::Device::DeviceBusType_UNKNOWN:
+		external = true;
+		break;
+	}
+
+	return external;
+}
+
+} // namespace utilities
+} // namespace sensor

src/lib/sensor/Utilities.hpp

@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2022 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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <lib/conversion/rotation.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/parameters/param.h>
+#include <matrix/math.hpp>
+
+namespace sensor
+{
+namespace utilities
+{
+
+/**
+ * @brief Get the board sensor level adjustment (SENS_BOARD_X_OFF, SENS_BOARD_Y_OFF, SENS_BOARD_Z_OFF).
+ *
+ * @return matrix::Eulerf
+ */
+matrix::Eulerf GetSensorLevelAdjustment();
+
+/**
+ * @brief Get the board rotation.
+ *
+ * @return enum Rotation
+ */
+Rotation GetBoardRotation();
+
+/**
+ * @brief Get the board rotation Dcm.
+ *
+ * @return matrix::Dcmf
+ */
+matrix::Dcmf GetBoardRotationMatrix();
+
+/**
+ * @brief Determine if device is on an external bus
+ *
+ * @return true if device is on an external bus
+ */
+bool DeviceExternal(uint32_t device_id);
+
+} // namespace utilities
+} // namespace sensor

src/lib/sensor/calibration/Accelerometer.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -36,10 +36,14 @@
 #include "Utilities.hpp"
 
 #include <lib/parameters/param.h>
+#include <lib/sensor/Utilities.hpp>
 
 using namespace matrix;
 using namespace time_literals;
+using namespace sensor::utilities;
 
+namespace sensor
+{
 namespace calibration
 {
 
@@ -48,7 +52,8 @@ Accelerometer::Accelerometer()
 	Reset();
 }
 
-Accelerometer::Accelerometer(uint32_t device_id)
+Accelerometer::Accelerometer(uint32_t device_id) :
+	_configuration(device_id)
 {
 	set_device_id(device_id);
 }
@@ -57,10 +62,9 @@ void Accelerometer::set_device_id(uint32_t device_id)
 {
 	bool external = DeviceExternal(device_id);
 
-	if (_device_id != device_id || _external != external) {
+	if (_configuration.device_id() != device_id || _configuration.external() != external) {
 
-		_device_id = device_id;
+		_configuration.set_device_id(device_id);
-		_external = external;
 
 		Reset();
 
@@ -75,7 +79,7 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
 	if (_sensor_correction_sub.updated() || force) {
 
 		// valid device id required
-		if (_device_id == 0) {
+		if (device_id() == 0) {
 			return;
 		}
 
@@ -84,17 +88,20 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
 		if (_sensor_correction_sub.copy(&corrections)) {
 			// find sensor_corrections index
 			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-				if (corrections.accel_device_ids[i] == _device_id) {
+				if (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;
+
 					case 3:
 						_thermal_offset = Vector3f{corrections.accel_offset_3};
 						return;
@@ -110,12 +117,15 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
 
 bool Accelerometer::set_offset(const Vector3f &offset)
 {
-	if (Vector3f(_offset - offset).longerThan(0.01f)) {
+	static constexpr float MIN_OFFSET_CHANGE = 0.01f;
-		if (offset.isAllFinite()) {
+
-			_offset = offset;
+	if (offset.isAllFinite()
-			_calibration_count++;
+	    && (Vector3f(_offset - offset).longerThan(MIN_OFFSET_CHANGE) || (_calibration_count == 0))
-			return true;
+	   ) {
-		}
+		_offset = offset;
+
+		_calibration_count++;
+		return true;
 	}
 
 	return false;
@@ -123,25 +133,20 @@ bool Accelerometer::set_offset(const Vector3f &offset)
 
 bool Accelerometer::set_scale(const Vector3f &scale)
 {
-	if (Vector3f(_scale - scale).longerThan(0.01f)) {
+	static constexpr float MIN_SCALE_CHANGE = 0.01f;
-		if (scale.isAllFinite() && (scale(0) > 0.f) && (scale(1) > 0.f) && (scale(2) > 0.f)) {
+
-			_scale = scale;
+	if (scale.isAllFinite() && scale.longerThan(0.f)
-			_calibration_count++;
+	    && (Vector3f(_scale - scale).longerThan(MIN_SCALE_CHANGE) || (_calibration_count == 0))
-			return true;
+	   ) {
-		}
+		_scale = scale;
+
+		_calibration_count++;
+		return true;
 	}
 
 	return false;
 }
 
-void Accelerometer::set_rotation(Rotation rotation)
-{
-	_rotation_enum = rotation;
-
-	// always apply board level adjustments
-	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
-}
-
 bool Accelerometer::set_calibration_index(int calibration_index)
 {
 	if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
@@ -154,11 +159,11 @@ bool Accelerometer::set_calibration_index(int calibration_index)
 
 void Accelerometer::ParametersUpdate()
 {
-	if (_device_id == 0) {
+	if (device_id() == 0) {
 		return;
 	}
 
-	_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
+	_calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id());
 
 	if (_calibration_index == -1) {
 		// no saved calibration available
@@ -166,52 +171,32 @@ void Accelerometer::ParametersUpdate()
 
 	} else {
 		ParametersLoad();
+
+		if (calibrated() && !_configuration.configured()) {
+			_configuration.ParametersSave(calibration_index());
+		}
 	}
 }
 
 bool Accelerometer::ParametersLoad()
 {
 	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
-		// CAL_ACCx_ROT
-		int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index);
 
-		if (_external) {
+		// CAL_{}n_{X,Y,Z}OFF
-			if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
+		Vector3f offset{
-				// invalid rotation, resetting
+			GetCalibrationParamFloat(SensorString(), "XOFF", _calibration_index),
-				rotation_value = ROTATION_NONE;
+			GetCalibrationParamFloat(SensorString(), "YOFF", _calibration_index),
-			}
+			GetCalibrationParamFloat(SensorString(), "ZOFF", _calibration_index)
+		};
 
-			set_rotation(static_cast<Rotation>(rotation_value));
+		// CAL_{}n_{X,Y,Z}SCALE
+		Vector3f scale{
+			GetCalibrationParamFloat(SensorString(), "XSCALE", _calibration_index),
+			GetCalibrationParamFloat(SensorString(), "YSCALE", _calibration_index),
+			GetCalibrationParamFloat(SensorString(), "ZSCALE", _calibration_index)
+		};
 
-		} else {
+		return set_offset(offset) && set_scale(scale);
-			// internal sensors follow board rotation
-			set_rotation(GetBoardRotation());
-		}
-
-		// CAL_ACCx_PRIO
-		_priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index);
-
-		if ((_priority < 0) || (_priority > 100)) {
-			// reset to default, -1 is the uninitialized parameter value
-			static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1;
-
-			if (_priority != CAL_PRIO_UNINITIALIZED) {
-				PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
-					_calibration_index, _priority);
-
-				SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED);
-			}
-
-			_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
-		}
-
-		// CAL_ACCx_OFF{X,Y,Z}
-		set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
-
-		// CAL_ACCx_SCALE{X,Y,Z}
-		set_scale(GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index));
-
-		return true;
 	}
 
 	return false;
@@ -219,21 +204,11 @@ bool Accelerometer::ParametersLoad()
 
 void Accelerometer::Reset()
 {
-	if (_external) {
-		set_rotation(ROTATION_NONE);
-
-	} else {
-		// internal sensors follow board rotation
-		set_rotation(GetBoardRotation());
-	}
-
 	_offset.zero();
 	_scale = Vector3f{1.f, 1.f, 1.f};
 
 	_thermal_offset.zero();
 
-	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
-
 	_calibration_index = -1;
 
 	_calibration_count = 0;
@@ -241,6 +216,8 @@ void Accelerometer::Reset()
 
 bool Accelerometer::ParametersSave(int desired_calibration_index, bool force)
 {
+	_configuration.ParametersSave();
+
 	if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
 		_calibration_index = desired_calibration_index;
 
@@ -249,10 +226,10 @@ bool Accelerometer::ParametersSave(int desired_calibration_index, bool force)
 
 		// ensure we have a valid calibration slot (matching existing or first available slot)
 		int8_t calibration_index_prev = _calibration_index;
-		_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
+		_calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index);
 
 		if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
-			PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
+			PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(),
 				 calibration_index_prev, _calibration_index);
 		}
 	}
@@ -260,17 +237,18 @@ bool Accelerometer::ParametersSave(int desired_calibration_index, bool force)
 	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
 		// save calibration
 		bool success = true;
-		success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
-		success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
-		success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
-		success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale);
 
-		if (_external) {
+		success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id());
-			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
 
-		} else {
+		// CAL_{}n_{X,Y,Z}OFF
-			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
+		success &= SetCalibrationParam(SensorString(), "XOFF", _calibration_index, _offset(0));
-		}
+		success &= SetCalibrationParam(SensorString(), "YOFF", _calibration_index, _offset(1));
+		success &= SetCalibrationParam(SensorString(), "ZOFF", _calibration_index, _offset(2));
+
+		// CAL_{}n_{X,Y,Z}SCALE
+		success &= SetCalibrationParam(SensorString(), "XSCALE", _calibration_index, _scale(0));
+		success &= SetCalibrationParam(SensorString(), "YSCALE", _calibration_index, _scale(1));
+		success &= SetCalibrationParam(SensorString(), "ZSCALE", _calibration_index, _scale(2));
 
 		return success;
 	}
@@ -297,9 +275,10 @@ void Accelerometer::PrintStatus()
 	}
 
 	if (_thermal_offset.norm() > 0.f) {
-		PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), _device_id,
+		PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), device_id(),
 			     (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2));
 	}
 }
 
 } // namespace calibration
+} // namespace sensor

src/lib/sensor/calibration/Accelerometer.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -33,13 +33,16 @@
 
 #pragma once
 
-#include <lib/conversion/rotation.h>
-#include <lib/matrix/matrix/math.hpp>
 #include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/log.h>
+
+#include <lib/matrix/matrix/math.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_correction.h>
 
+#include <lib/sensor/configuration/Accelerometer.hpp>
+
+namespace sensor
+{
 namespace calibration
 {
 class Accelerometer
@@ -47,9 +50,6 @@ class Accelerometer
 public:
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
-	static constexpr uint8_t DEFAULT_PRIORITY = 50;
-	static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
-
 	static constexpr const char *SensorString() { return "ACC"; }
 
 	Accelerometer();
@@ -61,34 +61,39 @@ public:
 
 	bool set_calibration_index(int calibration_index);
 	void set_device_id(uint32_t device_id);
+
 	bool set_offset(const matrix::Vector3f &offset);
 	bool set_scale(const matrix::Vector3f &scale);
-	void set_rotation(Rotation rotation);
 
-	bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
+	void set_rotation(Rotation rotation) { _configuration.set_rotation(rotation); }
+
+	bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); }
 	uint8_t calibration_count() const { return _calibration_count; }
 	int8_t calibration_index() const { return _calibration_index; }
-	uint32_t device_id() const { return _device_id; }
+
-	bool enabled() const { return (_priority > 0); }
-	bool external() const { return _external; }
 	const matrix::Vector3f &offset() const { return _offset; }
-	const int32_t &priority() const { return _priority; }
-	const matrix::Dcmf &rotation() const { return _rotation; }
-	const Rotation &rotation_enum() const { return _rotation_enum; }
 	const matrix::Vector3f &scale() const { return _scale; }
 
+	uint32_t device_id() const { return _configuration.device_id(); }
+	bool enabled() const { return _configuration.enabled(); }
+	bool external() const { return _configuration.external(); }
+
+	const int32_t &priority() const { return _configuration.priority(); }
+	const matrix::Dcmf &rotation() const { return _configuration.rotation(); }
+	const Rotation &rotation_enum() const { return _configuration.rotation_enum(); }
+
 	// apply offsets and scale
 	// rotate corrected measurements from sensor to body frame
 	inline matrix::Vector3f Correct(const matrix::Vector3f &data) const
 	{
-		return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
+		return _configuration.rotation() * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
 	}
 
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
 		// updated calibration offset = existing offset + bias rotated to sensor frame and unscaled
-		return _offset + (_rotation.I() * bias).edivide(_scale);
+		return _offset + (_configuration.rotation().I() * bias).edivide(_scale);
 	}
 
 	bool ParametersLoad();
@@ -100,21 +105,17 @@ public:
 	void SensorCorrectionsUpdate(bool force = false);
 
 private:
+	sensor::configuration::Accelerometer _configuration{};
+
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
-	Rotation _rotation_enum{ROTATION_NONE};
-
-	matrix::Dcmf _rotation;
 	matrix::Vector3f _offset;
 	matrix::Vector3f _scale;
 	matrix::Vector3f _thermal_offset;
 
 	int8_t _calibration_index{-1};
-	uint32_t _device_id{0};
-	int32_t _priority{-1};
-
-	bool _external{false};
-
 	uint8_t _calibration_count{0};
 };
+
 } // namespace calibration
+} // namespace sensor

src/lib/sensor/calibration/Barometer.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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
@@ -36,10 +36,14 @@
 #include "Utilities.hpp"
 
 #include <lib/parameters/param.h>
+#include <lib/sensor/Utilities.hpp>
 
 using namespace matrix;
 using namespace time_literals;
+using namespace sensor::utilities;
 
+namespace sensor
+{
 namespace calibration
 {
 
@@ -48,7 +52,8 @@ Barometer::Barometer()
 	Reset();
 }
 
-Barometer::Barometer(uint32_t device_id)
+Barometer::Barometer(uint32_t device_id) :
+	_configuration(device_id)
 {
 	set_device_id(device_id);
 }
@@ -57,10 +62,9 @@ void Barometer::set_device_id(uint32_t device_id)
 {
 	bool external = DeviceExternal(device_id);
 
-	if (_device_id != device_id || _external != external) {
+	if (_configuration.device_id() != device_id || _configuration.external() != external) {
 
-		_device_id = device_id;
+		_configuration.set_device_id(device_id);
-		_external = external;
 
 		Reset();
 
@@ -75,7 +79,7 @@ void Barometer::SensorCorrectionsUpdate(bool force)
 	if (_sensor_correction_sub.updated() || force) {
 
 		// valid device id required
-		if (_device_id == 0) {
+		if (device_id() == 0) {
 			return;
 		}
 
@@ -84,7 +88,7 @@ void Barometer::SensorCorrectionsUpdate(bool force)
 		if (_sensor_correction_sub.copy(&corrections)) {
 			// find sensor_corrections index
 			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-				if (corrections.baro_device_ids[i] == _device_id) {
+				if (corrections.baro_device_ids[i] == device_id()) {
 					switch (i) {
 					case 0:
 						_thermal_offset = corrections.baro_offset_0;
@@ -113,12 +117,15 @@ void Barometer::SensorCorrectionsUpdate(bool force)
 
 bool Barometer::set_offset(const float &offset)
 {
-	if (fabsf(_offset - offset) > 0.01f) {
+	static constexpr float MIN_OFFSET_CHANGE = 0.01f;
-		if (PX4_ISFINITE(offset)) {
+
-			_offset = offset;
+	if (PX4_ISFINITE(offset)
-			_calibration_count++;
+	    && ((fabsf(_offset - offset) > MIN_OFFSET_CHANGE) || (_calibration_count == 0))
-			return true;
+	   ) {
-		}
+		_offset = offset;
+
+		_calibration_count++;
+		return true;
 	}
 
 	return false;
@@ -136,11 +143,11 @@ bool Barometer::set_calibration_index(int calibration_index)
 
 void Barometer::ParametersUpdate()
 {
-	if (_device_id == 0) {
+	if (device_id() == 0) {
 		return;
 	}
 
-	_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
+	_calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id());
 
 	if (_calibration_index == -1) {
 		// no saved calibration available
@@ -148,33 +155,21 @@ void Barometer::ParametersUpdate()
 
 	} else {
 		ParametersLoad();
+
+		if (calibrated() && !_configuration.configured()) {
+			_configuration.ParametersSave(calibration_index());
+		}
 	}
 }
 
 bool Barometer::ParametersLoad()
 {
 	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
-		// CAL_BAROx_PRIO
-		_priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index);
 
-		if ((_priority < 0) || (_priority > 100)) {
+		// CAL_{}n_OFF
-			// reset to default, -1 is the uninitialized parameter value
+		float offset = GetCalibrationParamFloat(SensorString(), "OFF", _calibration_index);
-			static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1;
 
-			if (_priority != CAL_PRIO_UNINITIALIZED) {
+		return set_offset(offset);
-				PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
-					_calibration_index, _priority);
-
-				SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED);
-			}
-
-			_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
-		}
-
-		// CAL_BAROx_OFF
-		set_offset(GetCalibrationParamFloat(SensorString(), "OFF", _calibration_index));
-
-		return true;
 	}
 
 	return false;
@@ -186,8 +181,6 @@ void Barometer::Reset()
 
 	_thermal_offset = 0;
 
-	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
-
 	_calibration_index = -1;
 
 	_calibration_count = 0;
@@ -195,6 +188,8 @@ void Barometer::Reset()
 
 bool Barometer::ParametersSave(int desired_calibration_index, bool force)
 {
+	_configuration.ParametersSave();
+
 	if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
 		_calibration_index = desired_calibration_index;
 
@@ -203,10 +198,10 @@ bool Barometer::ParametersSave(int desired_calibration_index, bool force)
 
 		// ensure we have a valid calibration slot (matching existing or first available slot)
 		int8_t calibration_index_prev = _calibration_index;
-		_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
+		_calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index);
 
 		if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
-			PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
+			PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(),
 				 calibration_index_prev, _calibration_index);
 		}
 	}
@@ -214,8 +209,10 @@ bool Barometer::ParametersSave(int desired_calibration_index, bool force)
 	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
 		// save calibration
 		bool success = true;
-		success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
+
-		success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
+		success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id());
+
+		// CAL_{}n_OFF
 		success &= SetCalibrationParam(SensorString(), "OFF", _calibration_index, _offset);
 
 		return success;
@@ -239,8 +236,10 @@ void Barometer::PrintStatus()
 	}
 
 	if (fabsf(_thermal_offset) > 0.f) {
-		PX4_INFO_RAW("%s %" PRIu32 " temperature offset: %.4f\n", SensorString(), _device_id, (double)_thermal_offset);
+		PX4_INFO_RAW("%s %" PRIu32 " temperature offset: %.4f\n", SensorString(), device_id(),
+			     (double)_thermal_offset);
 	}
 }
 
 } // namespace calibration
+} // namespace sensor

src/lib/sensor/calibration/Barometer.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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
@@ -34,10 +34,14 @@
 #pragma once
 
 #include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/log.h>
+
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_correction.h>
 
+#include <lib/sensor/configuration/Barometer.hpp>
+
+namespace sensor
+{
 namespace calibration
 {
 class Barometer
@@ -45,9 +49,6 @@ class Barometer
 public:
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
-	static constexpr uint8_t DEFAULT_PRIORITY = 50;
-	static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
-
 	static constexpr const char *SensorString() { return "BARO"; }
 
 	Barometer();
@@ -59,18 +60,21 @@ public:
 
 	bool set_calibration_index(int calibration_index);
 	void set_device_id(uint32_t device_id);
+
 	bool set_offset(const float &offset);
 
-	bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
+	bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); }
 	uint8_t calibration_count() const { return _calibration_count; }
 	int8_t calibration_index() const { return _calibration_index; }
-	uint32_t device_id() const { return _device_id; }
+
-	bool enabled() const { return (_priority > 0); }
-	bool external() const { return _external; }
 	const float &offset() const { return _offset; }
-	const int32_t &priority() const { return _priority; }
 	const float &thermal_offset() const { return _thermal_offset; }
 
+	uint32_t device_id() const { return _configuration.device_id(); }
+	bool enabled() const { return _configuration.enabled(); }
+	bool external() const { return _configuration.external(); }
+	const int32_t &priority() const { return _configuration.priority(); }
+
 	// apply offsets
 	inline float Correct(const float &data) const
 	{
@@ -85,7 +89,8 @@ public:
 	// Compute sensor offset from bias (board frame)
 	float BiasCorrectedSensorOffset(const float &bias) const
 	{
-		return bias + _thermal_offset + _offset;
+		// updated calibration offset = existing offset + bias
+		return _offset + bias;
 	}
 
 	bool ParametersLoad();
@@ -97,17 +102,16 @@ public:
 	void SensorCorrectionsUpdate(bool force = false);
 
 private:
+	sensor::configuration::Barometer _configuration{};
+
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
 	float _offset{0};
 	float _thermal_offset{0};
 
 	int8_t _calibration_index{-1};
-	uint32_t _device_id{0};
-	int32_t _priority{-1};
-
-	bool _external{false};
-
 	uint8_t _calibration_count{0};
 };
+
 } // namespace calibration
+} // namespace sensor

src/lib/sensor/calibration/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2020-2022 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
@@ -44,4 +44,9 @@ px4_add_library(sensor_calibration
 	Utilities.hpp
 )
 
-target_link_libraries(sensor_calibration PRIVATE conversion)
+target_link_libraries(sensor_calibration
+	PRIVATE
+		conversion
+		sensor_configuration
+		sensor_utilities
+)

src/lib/sensor/calibration/Gyroscope.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -36,10 +36,14 @@
 #include "Utilities.hpp"
 
 #include <lib/parameters/param.h>
+#include <lib/sensor/Utilities.hpp>
 
 using namespace matrix;
 using namespace time_literals;
+using namespace sensor::utilities;
 
+namespace sensor
+{
 namespace calibration
 {
 
@@ -48,7 +52,8 @@ Gyroscope::Gyroscope()
 	Reset();
 }
 
-Gyroscope::Gyroscope(uint32_t device_id)
+Gyroscope::Gyroscope(uint32_t device_id) :
+	_configuration(device_id)
 {
 	set_device_id(device_id);
 }
@@ -57,10 +62,9 @@ void Gyroscope::set_device_id(uint32_t device_id)
 {
 	bool external = DeviceExternal(device_id);
 
-	if (_device_id != device_id || _external != external) {
+	if (_configuration.device_id() != device_id || _configuration.external() != external) {
 
-		_device_id = device_id;
+		_configuration.set_device_id(device_id);
-		_external = external;
 
 		Reset();
 
@@ -75,7 +79,7 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
 	if (_sensor_correction_sub.updated() || force) {
 
 		// valid device id required
-		if (_device_id == 0) {
+		if (device_id() == 0) {
 			return;
 		}
 
@@ -84,17 +88,20 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
 		if (_sensor_correction_sub.copy(&corrections)) {
 			// find sensor_corrections index
 			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-				if (corrections.gyro_device_ids[i] == _device_id) {
+				if (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;
+
 					case 3:
 						_thermal_offset = Vector3f{corrections.gyro_offset_3};
 						return;
@@ -110,25 +117,20 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
 
 bool Gyroscope::set_offset(const Vector3f &offset)
 {
-	if (Vector3f(_offset - offset).longerThan(0.01f) || (_calibration_count == 0)) {
+	static constexpr float MIN_OFFSET_CHANGE = 0.01f;
-		if (offset.isAllFinite()) {
+
-			_offset = offset;
+	if (offset.isAllFinite()
-			_calibration_count++;
+	    && (Vector3f(_offset - offset).longerThan(MIN_OFFSET_CHANGE) || (_calibration_count == 0))
-			return true;
+	   ) {
-		}
+		_offset = offset;
+
+		_calibration_count++;
+		return true;
 	}
 
 	return false;
 }
 
-void Gyroscope::set_rotation(Rotation rotation)
-{
-	_rotation_enum = rotation;
-
-	// always apply board level adjustments
-	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
-}
-
 bool Gyroscope::set_calibration_index(int calibration_index)
 {
 	if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
@@ -141,11 +143,11 @@ bool Gyroscope::set_calibration_index(int calibration_index)
 
 void Gyroscope::ParametersUpdate()
 {
-	if (_device_id == 0) {
+	if (device_id() == 0) {
 		return;
 	}
 
-	_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
+	_calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id());
 
 	if (_calibration_index == -1) {
 		// no saved calibration available
@@ -153,49 +155,25 @@ void Gyroscope::ParametersUpdate()
 
 	} else {
 		ParametersLoad();
+
+		if (calibrated() && !_configuration.configured()) {
+			_configuration.ParametersSave(calibration_index());
+		}
 	}
 }
 
 bool Gyroscope::ParametersLoad()
 {
 	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
-		// CAL_GYROx_ROT
-		int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index);
 
-		if (_external) {
+		// CAL_{}n_{X,Y,Z}OFF
-			if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
+		Vector3f offset{
-				// invalid rotation, resetting
+			GetCalibrationParamFloat(SensorString(), "XOFF", _calibration_index),
-				rotation_value = ROTATION_NONE;
+			GetCalibrationParamFloat(SensorString(), "YOFF", _calibration_index),
-			}
+			GetCalibrationParamFloat(SensorString(), "ZOFF", _calibration_index)
+		};
 
-			set_rotation(static_cast<Rotation>(rotation_value));
+		return set_offset(offset);
-
-		} else {
-			// internal sensors follow board rotation
-			set_rotation(GetBoardRotation());
-		}
-
-		// CAL_GYROx_PRIO
-		_priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index);
-
-		if ((_priority < 0) || (_priority > 100)) {
-			// reset to default, -1 is the uninitialized parameter value
-			static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1;
-
-			if (_priority != CAL_PRIO_UNINITIALIZED) {
-				PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
-					_calibration_index, _priority);
-
-				SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED);
-			}
-
-			_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
-		}
-
-		// CAL_GYROx_OFF{X,Y,Z}
-		set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
-
-		return true;
 	}
 
 	return false;
@@ -203,20 +181,10 @@ bool Gyroscope::ParametersLoad()
 
 void Gyroscope::Reset()
 {
-	if (_external) {
-		set_rotation(ROTATION_NONE);
-
-	} else {
-		// internal sensors follow board rotation
-		set_rotation(GetBoardRotation());
-	}
-
 	_offset.zero();
 
 	_thermal_offset.zero();
 
-	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
-
 	_calibration_index = -1;
 
 	_calibration_count = 0;
@@ -224,6 +192,8 @@ void Gyroscope::Reset()
 
 bool Gyroscope::ParametersSave(int desired_calibration_index, bool force)
 {
+	_configuration.ParametersSave();
+
 	if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
 		_calibration_index = desired_calibration_index;
 
@@ -232,10 +202,10 @@ bool Gyroscope::ParametersSave(int desired_calibration_index, bool force)
 
 		// ensure we have a valid calibration slot (matching existing or first available slot)
 		int8_t calibration_index_prev = _calibration_index;
-		_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
+		_calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index);
 
 		if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
-			PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
+			PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(),
 				 calibration_index_prev, _calibration_index);
 		}
 	}
@@ -243,16 +213,13 @@ bool Gyroscope::ParametersSave(int desired_calibration_index, bool force)
 	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
 		// save calibration
 		bool success = true;
-		success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
-		success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
-		success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
 
-		if (_external) {
+		success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id());
-			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
 
-		} else {
+		// CAL_{}n_{X,Y,Z}OFF
-			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
+		success &= SetCalibrationParam(SensorString(), "XOFF", _calibration_index, _offset(0));
-		}
+		success &= SetCalibrationParam(SensorString(), "YOFF", _calibration_index, _offset(1));
+		success &= SetCalibrationParam(SensorString(), "ZOFF", _calibration_index, _offset(2));
 
 		return success;
 	}
@@ -270,15 +237,17 @@ void Gyroscope::PrintStatus()
 			     rotation_enum());
 
 	} else {
-		PX4_INFO_RAW("%s %" PRIu32 " EN: %d, offset: [%05.3f %05.3f %05.3f], Internal\n",
+		PX4_INFO_RAW("%s %" PRIu32
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], Internal\n",
 			     SensorString(), device_id(), enabled(),
 			     (double)_offset(0), (double)_offset(1), (double)_offset(2));
 	}
 
 	if (_thermal_offset.norm() > 0.f) {
-		PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), _device_id,
+		PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), device_id(),
 			     (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2));
 	}
 }
 
 } // namespace calibration
+} // namespace sensor

src/lib/sensor/calibration/Gyroscope.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -33,13 +33,16 @@
 
 #pragma once
 
-#include <lib/conversion/rotation.h>
-#include <lib/matrix/matrix/math.hpp>
 #include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/log.h>
+
+#include <lib/matrix/matrix/math.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_correction.h>
 
+#include <lib/sensor/configuration/Gyroscope.hpp>
+
+namespace sensor
+{
 namespace calibration
 {
 class Gyroscope
@@ -47,9 +50,6 @@ class Gyroscope
 public:
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
-	static constexpr uint8_t DEFAULT_PRIORITY = 50;
-	static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
-
 	static constexpr const char *SensorString() { return "GYRO"; }
 
 	Gyroscope();
@@ -61,38 +61,43 @@ public:
 
 	bool set_calibration_index(int calibration_index);
 	void set_device_id(uint32_t device_id);
-	bool set_offset(const matrix::Vector3f &offset);
-	void set_rotation(Rotation rotation);
 
-	bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
+	bool set_offset(const matrix::Vector3f &offset);
+
+	void set_rotation(Rotation rotation) { _configuration.set_rotation(rotation); }
+
+	bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); }
 	uint8_t calibration_count() const { return _calibration_count; }
 	int8_t calibration_index() const { return _calibration_index; }
-	uint32_t device_id() const { return _device_id; }
+
-	bool enabled() const { return (_priority > 0); }
-	bool external() const { return _external; }
 	const matrix::Vector3f &offset() const { return _offset; }
-	const int32_t &priority() const { return _priority; }
-	const matrix::Dcmf &rotation() const { return _rotation; }
-	const Rotation &rotation_enum() const { return _rotation_enum; }
 	const matrix::Vector3f &thermal_offset() const { return _thermal_offset; }
 
+	uint32_t device_id() const { return _configuration.device_id(); }
+	bool enabled() const { return _configuration.enabled(); }
+	bool external() const { return _configuration.external(); }
+
+	const int32_t &priority() const { return _configuration.priority(); }
+	const matrix::Dcmf &rotation() const { return _configuration.rotation(); }
+	const Rotation &rotation_enum() const { return _configuration.rotation_enum(); }
+
 	// apply offsets and scale
 	// rotate corrected measurements from sensor to body frame
 	inline matrix::Vector3f Correct(const matrix::Vector3f &data) const
 	{
-		return _rotation * matrix::Vector3f{data - _thermal_offset - _offset};
+		return _configuration.rotation() * matrix::Vector3f{data - _thermal_offset - _offset};
 	}
 
 	inline matrix::Vector3f Uncorrect(const matrix::Vector3f &corrected_data) const
 	{
-		return (_rotation.I() * corrected_data) + _thermal_offset + _offset;
+		return (_configuration.rotation().I() * corrected_data) + _thermal_offset + _offset;
 	}
 
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
 		// updated calibration offset = existing offset + bias rotated to sensor frame
-		return _offset + (_rotation.I() * bias);
+		return _offset + (_configuration.rotation().I() * bias);
 	}
 
 	bool ParametersLoad();
@@ -104,20 +109,16 @@ public:
 	void SensorCorrectionsUpdate(bool force = false);
 
 private:
+	sensor::configuration::Gyroscope _configuration{};
+
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
-	Rotation _rotation_enum{ROTATION_NONE};
-
-	matrix::Dcmf _rotation;
 	matrix::Vector3f _offset;
 	matrix::Vector3f _thermal_offset;
 
 	int8_t _calibration_index{-1};
-	uint32_t _device_id{0};
-	int32_t _priority{-1};
-
-	bool _external{false};
-
 	uint8_t _calibration_count{0};
 };
+
 } // namespace calibration
+} // namespace sensor

src/lib/sensor/calibration/Magnetometer.cpp

@@ -0,0 +1,337 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2023 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 "Magnetometer.hpp"
+
+#include "Utilities.hpp"
+
+#include <lib/parameters/param.h>
+#include <lib/sensor/Utilities.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+using namespace sensor::utilities;
+
+namespace sensor
+{
+namespace calibration
+{
+
+Magnetometer::Magnetometer()
+{
+	Reset();
+}
+
+Magnetometer::Magnetometer(uint32_t device_id) :
+	_configuration(device_id)
+{
+	set_device_id(device_id);
+}
+
+void Magnetometer::set_device_id(uint32_t device_id)
+{
+	bool external = DeviceExternal(device_id);
+
+	if (_configuration.device_id() != device_id || _configuration.external() != external) {
+
+		_configuration.set_device_id(device_id);
+
+		Reset();
+
+		ParametersUpdate();
+		SensorCorrectionsUpdate(true);
+	}
+}
+
+void Magnetometer::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 (corrections.mag_device_ids[i] == device_id()) {
+					switch (i) {
+					case 0:
+						_thermal_offset = Vector3f{corrections.mag_offset_0};
+						return;
+
+					case 1:
+						_thermal_offset = Vector3f{corrections.mag_offset_1};
+						return;
+
+					case 2:
+						_thermal_offset = Vector3f{corrections.mag_offset_2};
+						return;
+
+					case 3:
+						_thermal_offset = Vector3f{corrections.mag_offset_3};
+						return;
+					}
+				}
+			}
+		}
+
+		// zero thermal offset if not found
+		_thermal_offset.zero();
+	}
+}
+
+bool Magnetometer::set_offset(const Vector3f &offset)
+{
+	static constexpr float MIN_OFFSET_CHANGE = 0.005f;
+
+	if (offset.isAllFinite()
+	    && (Vector3f(_offset - offset).longerThan(MIN_OFFSET_CHANGE) || (_calibration_count == 0))
+	   ) {
+		_offset = offset;
+
+		_calibration_count++;
+		return true;
+	}
+
+	return false;
+}
+
+bool Magnetometer::set_scale(const Vector3f &scale)
+{
+	static constexpr float MIN_SCALE_CHANGE = 0.01f;
+
+	if (scale.isAllFinite() && scale.longerThan(0.f)
+	    && (Vector3f(_scale.diag() - scale).longerThan(MIN_SCALE_CHANGE) || (_calibration_count == 0))
+	   ) {
+		_scale(0, 0) = scale(0);
+		_scale(1, 1) = scale(1);
+		_scale(2, 2) = scale(2);
+
+		_calibration_count++;
+		return true;
+	}
+
+	return false;
+}
+
+bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal)
+{
+	if (offdiagonal.isAllFinite()
+	    && (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiagonal).longerThan(0.01f)
+		|| (_calibration_count == 0))
+	   ) {
+
+		_scale(0, 1) = offdiagonal(0);
+		_scale(1, 0) = offdiagonal(0);
+
+		_scale(0, 2) = offdiagonal(1);
+		_scale(2, 0) = offdiagonal(1);
+
+		_scale(1, 2) = offdiagonal(2);
+		_scale(2, 1) = offdiagonal(2);
+
+		_calibration_count++;
+		return true;
+	}
+
+	return false;
+}
+
+bool Magnetometer::set_calibration_index(int calibration_index)
+{
+	if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
+		_calibration_index = calibration_index;
+		return true;
+	}
+
+	return false;
+}
+
+void Magnetometer::ParametersUpdate()
+{
+	if (device_id() == 0) {
+		return;
+	}
+
+	_calibration_index = FindCurrentCalibrationIndex(SensorString(), device_id());
+
+	if (_calibration_index == -1) {
+		// no saved calibration available
+		Reset();
+
+	} else {
+		ParametersLoad();
+
+		if (calibrated() && !_configuration.configured()) {
+			_configuration.ParametersSave(calibration_index());
+		}
+	}
+}
+
+bool Magnetometer::ParametersLoad()
+{
+	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
+
+		// CAL_{}n_{X,Y,Z}OFF
+		Vector3f offset{
+			GetCalibrationParamFloat(SensorString(), "XOFF", _calibration_index),
+			GetCalibrationParamFloat(SensorString(), "YOFF", _calibration_index),
+			GetCalibrationParamFloat(SensorString(), "ZOFF", _calibration_index)
+		};
+
+		// CAL_{}n_{X,Y,Z}SCALE
+		Vector3f scale{
+			GetCalibrationParamFloat(SensorString(), "XSCALE", _calibration_index),
+			GetCalibrationParamFloat(SensorString(), "YSCALE", _calibration_index),
+			GetCalibrationParamFloat(SensorString(), "ZSCALE", _calibration_index)
+		};
+
+		// CAL_{}n_{X,Y,Z}ODIAG
+		Vector3f odiag{
+			GetCalibrationParamFloat(SensorString(), "XODIAG", _calibration_index),
+			GetCalibrationParamFloat(SensorString(), "YODIAG", _calibration_index),
+			GetCalibrationParamFloat(SensorString(), "ZODIAG", _calibration_index)
+		};
+
+		// CAL_{}n_{X,Y,Z}COMP
+		Vector3f comp{
+			GetCalibrationParamFloat(SensorString(), "XCOMP", _calibration_index),
+			GetCalibrationParamFloat(SensorString(), "YCOMP", _calibration_index),
+			GetCalibrationParamFloat(SensorString(), "ZCOMP", _calibration_index)
+		};
+		_power_compensation = comp;
+
+		return set_offset(offset) && set_scale(scale) && set_offdiagonal(odiag);
+	}
+
+	return false;
+}
+
+void Magnetometer::Reset()
+{
+	_offset.zero();
+	_scale.setIdentity();
+
+	_power_compensation.zero();
+	_power = 0.f;
+
+	_thermal_offset.zero();
+
+	_calibration_index = -1;
+
+	_calibration_count = 0;
+}
+
+bool Magnetometer::ParametersSave(int desired_calibration_index, bool force)
+{
+	_configuration.ParametersSave();
+
+	if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
+		_calibration_index = desired_calibration_index;
+
+	} else if (!force || (_calibration_index < 0)
+		   || (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) {
+
+		// ensure we have a valid calibration slot (matching existing or first available slot)
+		int8_t calibration_index_prev = _calibration_index;
+		_calibration_index = FindAvailableCalibrationIndex(SensorString(), device_id(), desired_calibration_index);
+
+		if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
+			PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), device_id(),
+				 calibration_index_prev, _calibration_index);
+		}
+	}
+
+	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
+		// save calibration
+		bool success = true;
+
+		success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, device_id());
+
+		// CAL_{}n_{X,Y,Z}OFF
+		success &= SetCalibrationParam(SensorString(), "XOFF", _calibration_index, _offset(0));
+		success &= SetCalibrationParam(SensorString(), "YOFF", _calibration_index, _offset(1));
+		success &= SetCalibrationParam(SensorString(), "ZOFF", _calibration_index, _offset(2));
+
+		// CAL_{}n_{X,Y,Z}SCALE
+		success &= SetCalibrationParam(SensorString(), "XSCALE", _calibration_index, _scale(0, 0));
+		success &= SetCalibrationParam(SensorString(), "YSCALE", _calibration_index, _scale(1, 1));
+		success &= SetCalibrationParam(SensorString(), "ZSCALE", _calibration_index, _scale(2, 2));
+
+		// CAL_{}n_{X,Y,Z}ODIAG
+		success &= SetCalibrationParam(SensorString(), "XODIAG", _calibration_index, _scale(0, 1));
+		success &= SetCalibrationParam(SensorString(), "YODIAG", _calibration_index, _scale(0, 2));
+		success &= SetCalibrationParam(SensorString(), "ZODIAG", _calibration_index, _scale(1, 2));
+
+		// CAL_{}n_{X,Y,Z}COMP
+		success &= SetCalibrationParam(SensorString(), "XCOMP", _calibration_index, _power_compensation(0));
+		success &= SetCalibrationParam(SensorString(), "YCOMP", _calibration_index, _power_compensation(1));
+		success &= SetCalibrationParam(SensorString(), "ZCOMP", _calibration_index, _power_compensation(2));
+
+		return success;
+	}
+
+	return false;
+}
+
+void Magnetometer::PrintStatus()
+{
+	if (external()) {
+		PX4_INFO_RAW("%s %" PRIu32
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Ext ROT: %d\n",
+			     SensorString(), device_id(), enabled(),
+			     (double)_offset(0), (double)_offset(1), (double)_offset(2),
+			     (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2),
+			     rotation_enum());
+
+	} else {
+		PX4_INFO_RAW("%s %" PRIu32
+			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Internal\n",
+			     SensorString(), device_id(), enabled(),
+			     (double)_offset(0), (double)_offset(1), (double)_offset(2),
+			     (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2));
+	}
+
+	if (_thermal_offset.norm() > 0.f) {
+		PX4_INFO_RAW("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]\n", SensorString(), device_id(),
+			     (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2));
+	}
+}
+
+} // namespace calibration
+} // namespace sensor

src/lib/sensor/calibration/Magnetometer.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -33,14 +33,16 @@
 
 #pragma once
 
-#include <lib/conversion/rotation.h>
-#include <lib/matrix/matrix/math.hpp>
 #include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/log.h>
+
+#include <lib/matrix/matrix/math.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_correction.h>
 
+#include <lib/sensor/configuration/Magnetometer.hpp>
+
+namespace sensor
+{
 namespace calibration
 {
 class Magnetometer
@@ -48,9 +50,6 @@ class Magnetometer
 public:
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
-	static constexpr uint8_t DEFAULT_PRIORITY = 50;
-	static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
-
 	static constexpr const char *SensorString() { return "MAG"; }
 
 	Magnetometer();
@@ -62,48 +61,40 @@ public:
 
 	bool set_calibration_index(int calibration_index);
 	void set_device_id(uint32_t device_id);
+
 	bool set_offset(const matrix::Vector3f &offset);
 	bool set_scale(const matrix::Vector3f &scale);
 	bool set_offdiagonal(const matrix::Vector3f &offdiagonal);
 
-	/**
+	void set_rotation(Rotation rotation) { _configuration.set_rotation(rotation); }
-	 * @brief Set the rotation enum & corresponding rotation matrix for Magnetometer
-	 *
-	 * @param rotation Rotation enum
-	 */
-	void set_rotation(Rotation rotation);
 
-	/**
+	bool calibrated() const { return (_configuration.device_id() != 0) && (_calibration_index >= 0); }
-	 * @brief Set the custom rotation & rotation enum to ROTATION_CUSTOM for Magnetometer
-	 *
-	 * @param rotation Rotation euler angles
-	 */
-	void set_custom_rotation(const matrix::Eulerf &rotation);
-
-	bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
 	uint8_t calibration_count() const { return _calibration_count; }
 	int8_t calibration_index() const { return _calibration_index; }
-	uint32_t device_id() const { return _device_id; }
+
-	bool enabled() const { return (_priority > 0); }
-	bool external() const { return _external; }
 	const matrix::Vector3f &offset() const { return _offset; }
-	const int32_t &priority() const { return _priority; }
-	const matrix::Dcmf &rotation() const { return _rotation; }
-	const Rotation &rotation_enum() const { return _rotation_enum; }
 	const matrix::Matrix3f &scale() const { return _scale; }
 
+	uint32_t device_id() const { return _configuration.device_id(); }
+	bool enabled() const { return _configuration.enabled(); }
+	bool external() const { return _configuration.external(); }
+
+	const int32_t &priority() const { return _configuration.priority(); }
+	const matrix::Dcmf &rotation() const { return _configuration.rotation(); }
+	const Rotation &rotation_enum() const { return _configuration.rotation_enum(); }
+
 	// apply offsets and scale
 	// rotate corrected measurements from sensor to body frame
 	inline matrix::Vector3f Correct(const matrix::Vector3f &data) const
 	{
-		return _rotation * (_scale * ((data + _power * _power_compensation) - _offset));
+		return _configuration.rotation() * (_scale * ((data + _power * _power_compensation) - _offset));
 	}
 
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{
 		// updated calibration offset = existing offset + bias rotated to sensor frame and unscaled
-		return _offset + (_scale.I() * _rotation.I() * bias);
+		return _offset + (_scale.I() * _configuration.rotation().I() * bias);
 	}
 
 	bool ParametersLoad();
@@ -117,17 +108,10 @@ public:
 	void UpdatePower(float power) { _power = power; }
 
 private:
+	sensor::configuration::Magnetometer _configuration{};
+
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
-	Rotation _rotation_enum{ROTATION_NONE};
-
-	/**
-	 * @brief 3 x 3 Rotation matrix that translates from sensor frame (XYZ) to vehicle body frame (FRD)
-	 */
-	matrix::Dcmf _rotation;
-
-	matrix::Eulerf _rotation_custom_euler{0.f, 0.f, 0.f}; // custom rotation euler angles (optional)
-
 	matrix::Vector3f _offset;
 	matrix::Matrix3f _scale;
 	matrix::Vector3f _thermal_offset;
@@ -136,11 +120,8 @@ private:
 	float _power{0.f};
 
 	int8_t _calibration_index{-1};
-	uint32_t _device_id{0};
-	int32_t _priority{-1};
-
-	bool _external{false};
-
 	uint8_t _calibration_count{0};
 };
+
 } // namespace calibration
+} // namespace sensor

src/lib/sensor/calibration/Utilities.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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,26 +31,20 @@
  *
  ****************************************************************************/
 
+#include "Utilities.hpp"
+
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/log.h>
 #include <lib/conversion/rotation.h>
-#include <lib/drivers/device/Device.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
 
-#if defined(CONFIG_I2C)
-# include <px4_platform_common/i2c.h>
-#endif // CONFIG_I2C
-
-#if defined(CONFIG_SPI)
-# include <px4_platform_common/spi.h>
-#endif // CONFIG_SPI
-
-using math::radians;
 using matrix::Eulerf;
 using matrix::Dcmf;
 using matrix::Vector3f;
 
+namespace sensor
+{
 namespace calibration
 {
 
@@ -63,7 +57,7 @@ int8_t FindCurrentCalibrationIndex(const char *sensor_type, uint32_t device_id)
 	}
 
 	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
-		char str[20] {};
+		char str[16 + 1] {};
 		snprintf(str, sizeof(str), "CAL_%s%u_ID", sensor_type, i);
 
 		int32_t device_id_val = 0;
@@ -102,7 +96,7 @@ int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id
 	uint32_t cal_device_ids[MAX_SENSOR_COUNT] {};
 
 	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
-		char str[20] {};
+		char str[16 + 1] {};
 		snprintf(str, sizeof(str), "CAL_%s%u_ID", sensor_type, i);
 		int32_t device_id_val = 0;
 
@@ -136,8 +130,8 @@ int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id
 
 int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type, uint8_t instance)
 {
-	// eg CAL_MAGn_ID/CAL_MAGn_ROT
+	// eg CAL_{}n_ID
-	char str[20] {};
+	char str[16 + 1] {};
 	snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
 
 	int32_t value = 0;
@@ -151,8 +145,8 @@ int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type,
 
 float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance)
 {
-	// eg CAL_BAROn_OFF
+	// eg CAL_{}n_OFF
-	char str[20] {};
+	char str[16 + 1] {};
 	snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
 
 	float value = NAN;
@@ -164,124 +158,5 @@ float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, ui
 	return value;
 }
 
-Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance)
-{
-	Vector3f values{0.f, 0.f, 0.f};
-
-	char str[20] {};
-
-	for (int axis = 0; axis < 3; axis++) {
-		char axis_char = 'X' + axis;
-
-		// eg CAL_MAGn_{X,Y,Z}OFF
-		snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
-
-		if (param_get(param_find(str), &values(axis)) != 0) {
-			PX4_ERR("failed to get %s", str);
-		}
-	}
-
-	return values;
-}
-
-bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance, Vector3f values)
-{
-	int ret = PX4_OK;
-	char str[20] {};
-
-	for (int axis = 0; axis < 3; axis++) {
-		char axis_char = 'X' + axis;
-
-		// eg CAL_MAGn_{X,Y,Z}OFF
-		snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
-
-		if (param_set_no_notification(param_find(str), &values(axis)) != 0) {
-			PX4_ERR("failed to set %s = %.4f", str, (double)values(axis));
-			ret = PX4_ERROR;
-		}
-	}
-
-	return ret == PX4_OK;
-}
-
-Eulerf GetSensorLevelAdjustment()
-{
-	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);
-
-	return Eulerf{radians(x_offset), radians(y_offset), radians(z_offset)};
-}
-
-enum Rotation GetBoardRotation()
-{
-	// get transformation matrix from sensor/board to body frame
-	int32_t board_rot = -1;
-	param_get(param_find("SENS_BOARD_ROT"), &board_rot);
-
-	if (board_rot >= 0 && board_rot < Rotation::ROTATION_MAX) {
-		return static_cast<enum Rotation>(board_rot);
-
-	} else {
-		PX4_ERR("invalid SENS_BOARD_ROT: %" PRId32, board_rot);
-	}
-
-	return Rotation::ROTATION_NONE;
-}
-
-Dcmf GetBoardRotationMatrix()
-{
-	return get_rot_matrix(GetBoardRotation());
-}
-
-bool DeviceExternal(uint32_t device_id)
-{
-	bool external = true;
-
-	// decode device id to determine if external
-	union device::Device::DeviceId id {};
-	id.devid = device_id;
-
-	const device::Device::DeviceBusType bus_type = id.devid_s.bus_type;
-
-	switch (bus_type) {
-	case device::Device::DeviceBusType_I2C:
-#if defined(CONFIG_I2C)
-		external = px4_i2c_device_external(device_id);
-#endif // CONFIG_I2C
-		break;
-
-	case device::Device::DeviceBusType_SPI:
-#if defined(CONFIG_SPI)
-		external = px4_spi_bus_external(id.devid_s.bus);
-#endif // CONFIG_SPI
-		break;
-
-	case device::Device::DeviceBusType_UAVCAN:
-		external = true;
-		break;
-
-	case device::Device::DeviceBusType_SIMULATION:
-		external = false;
-		break;
-
-	case device::Device::DeviceBusType_SERIAL:
-		external = true;
-		break;
-
-	case device::Device::DeviceBusType_MAVLINK:
-		external = true;
-		break;
-
-	case device::Device::DeviceBusType_UNKNOWN:
-		external = true;
-		break;
-	}
-
-	return external;
-}
-
 } // namespace calibration
+} // namespace sensor

src/lib/sensor/calibration/Utilities.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 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
@@ -40,6 +40,8 @@
 #include <lib/parameters/param.h>
 #include <matrix/math.hpp>
 
+namespace sensor
+{
 namespace calibration
 {
 
@@ -85,9 +87,9 @@ float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, ui
 template<typename T>
 bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, T value)
 {
-	char str[20] {};
+	char str[16 + 1] {};
 
-	// eg CAL_MAGn_ID/CAL_MAGn_ROT
+	// eg CAL_{}n_ID
 	snprintf(str, sizeof(str), "CAL_%s%u_%s", sensor_type, instance, cal_type);
 
 	int ret = param_set_no_notification(param_find(str), &value);
@@ -99,54 +101,5 @@ bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t
 	return ret == PX4_OK;
 }
 
-/**
+} // namespace utilities
- * @brief Get the Calibration Params Vector 3f object
+} // namespace sensor
- *
- * @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG")
- * @param cal_type Calibration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO")
- * @param instance Calibration index (0 - 3)
- * @return matrix::Vector3f Vector of calibration values.
- */
-matrix::Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance);
-
-/**
- * @brief Set the Calibration Params Vector 3f object
- *
- * @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG")
- * @param cal_type Calibration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO")
- * @param instance Calibration index (0 - 3)
- * @param values Vector of calibration values x, y, z.
- * @return true if the parameter name was valid and all values saved successfully, false otherwise.
- */
-bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance,
-				  matrix::Vector3f values);
-
-/**
- * @brief Get the board sensor level adjustment (SENS_BOARD_X_OFF, SENS_BOARD_Y_OFF, SENS_BOARD_Z_OFF).
- *
- * @return matrix::Eulerf
- */
-matrix::Eulerf GetSensorLevelAdjustment();
-
-/**
- * @brief Get the board rotation.
- *
- * @return enum Rotation
- */
-Rotation GetBoardRotation();
-
-/**
- * @brief Get the board rotation Dcm.
- *
- * @return matrix::Dcmf
- */
-matrix::Dcmf GetBoardRotationMatrix();
-
-/**
- * @brief Determine if device is on an external bus
- *
- * @return true if device is on an external bus
- */
-bool DeviceExternal(uint32_t device_id);
-
-} // namespace calibration

src/lib/sensor/configuration/Accelerometer.cpp

@@ -0,0 +1,223 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2023 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 "Accelerometer.hpp"
+
+#include "Utilities.hpp"
+
+#include <lib/parameters/param.h>
+#include <lib/sensor/Utilities.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+using namespace sensor::utilities;
+
+namespace sensor
+{
+namespace configuration
+{
+
+Accelerometer::Accelerometer()
+{
+	Reset();
+}
+
+Accelerometer::Accelerometer(uint32_t device_id)
+{
+	set_device_id(device_id);
+}
+
+void Accelerometer::set_device_id(uint32_t device_id)
+{
+	bool external = DeviceExternal(device_id);
+
+	if (_device_id != device_id || _external != external) {
+
+		_device_id = device_id;
+		_external = external;
+
+		Reset();
+
+		ParametersUpdate();
+	}
+}
+
+bool Accelerometer::set_configuration_index(int configuration_index)
+{
+	if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) {
+		_configuration_index = configuration_index;
+		return true;
+	}
+
+	return false;
+}
+
+void Accelerometer::set_rotation(Rotation rotation)
+{
+	_rotation_enum = rotation;
+
+	// always apply board level adjustments
+	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
+}
+
+void Accelerometer::ParametersUpdate()
+{
+	if (_device_id == 0) {
+		return;
+	}
+
+	_configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id);
+
+	if (_configuration_index == -1) {
+		// no saved configuration available
+		Reset();
+
+	} else {
+		ParametersLoad();
+	}
+}
+
+bool Accelerometer::ParametersLoad()
+{
+	if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
+		// SENS_{}n_ROT
+		int32_t rotation_value = GetConfigurationParamInt32(SensorString(), "ROT", _configuration_index);
+
+		if (_external) {
+			if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
+				// invalid rotation, resetting
+				rotation_value = ROTATION_NONE;
+			}
+
+			set_rotation(static_cast<Rotation>(rotation_value));
+
+		} else {
+			// internal sensors follow board rotation
+			set_rotation(GetBoardRotation());
+		}
+
+		// SENS_{}n_PRIO
+		_priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index);
+
+		if ((_priority < 0) || (_priority > 100)) {
+			// reset to default, -1 is the uninitialized parameter value
+			static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1;
+
+			if (_priority != SENS_PRIO_UNINITIALIZED) {
+				PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
+					_configuration_index, _priority);
+
+				SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED);
+			}
+
+			_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+		}
+
+		return true;
+	}
+
+	return false;
+}
+
+void Accelerometer::Reset()
+{
+	if (_external) {
+		set_rotation(ROTATION_NONE);
+
+	} else {
+		// internal sensors follow board rotation
+		set_rotation(GetBoardRotation());
+	}
+
+	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+
+	_configuration_index = -1;
+
+	_configuration_count = 0;
+}
+
+bool Accelerometer::ParametersSave(int desired_configuration_index, bool force)
+{
+	if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) {
+		_configuration_index = desired_configuration_index;
+
+	} else if (!force || (_configuration_index < 0)
+		   || (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) {
+
+		// ensure we have a valid configuration slot (matching existing or first available slot)
+		int8_t configuration_index_prev = _configuration_index;
+		_configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index);
+
+		if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) {
+			PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
+				 configuration_index_prev, _configuration_index);
+		}
+	}
+
+	if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
+		// save configuration
+		bool success = true;
+
+		success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id);
+		success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority);
+
+		if (_external) {
+			success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, (int32_t)_rotation_enum);
+
+		} else {
+			success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, -1); // internal
+		}
+
+		return success;
+	}
+
+	return false;
+}
+
+void Accelerometer::PrintStatus()
+{
+	if (external()) {
+		PX4_INFO_RAW("%s %" PRIu32
+			     " EN: %d, Ext ROT: %d\n",
+			     SensorString(), device_id(), enabled(),
+			     rotation_enum());
+
+	} else {
+		PX4_INFO_RAW("%s %" PRIu32
+			     " EN: %d, Internal\n",
+			     SensorString(), device_id(), enabled());
+	}
+}
+
+} // namespace configuration
+} // namespace sensor

src/lib/sensor/configuration/Accelerometer.hpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2023 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 <px4_platform_common/px4_config.h>
+
+#include <lib/conversion/rotation.h>
+#include <lib/matrix/matrix/math.hpp>
+
+namespace sensor
+{
+namespace configuration
+{
+
+class Accelerometer
+{
+public:
+	static constexpr int MAX_SENSOR_COUNT = 4;
+
+	static constexpr uint8_t DEFAULT_PRIORITY = 50;
+	static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
+
+	static constexpr const char *SensorString() { return "ACC"; }
+
+	Accelerometer();
+	explicit Accelerometer(uint32_t device_id);
+
+	~Accelerometer() = default;
+
+	void PrintStatus();
+
+	bool set_configuration_index(int configuration_index);
+	void set_device_id(uint32_t device_id);
+
+	void set_rotation(Rotation rotation);
+
+	bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); }
+	uint8_t configuration_count() const { return _configuration_count; }
+	int8_t configuration_index() const { return _configuration_index; }
+	uint32_t device_id() const { return _device_id; }
+	bool enabled() const { return (_priority > 0); }
+	bool external() const { return _external; }
+
+	const int32_t &priority() const { return _priority; }
+	const matrix::Dcmf &rotation() const { return _rotation; }
+	const Rotation &rotation_enum() const { return _rotation_enum; }
+
+	bool ParametersLoad();
+	bool ParametersSave(int desired_configuration_index = -1, bool force = false);
+	void ParametersUpdate();
+
+	void Reset();
+
+private:
+	Rotation _rotation_enum{ROTATION_NONE};
+
+	matrix::Dcmf _rotation;
+
+	uint32_t _device_id{0};
+	int32_t _priority{-1};
+
+	bool _external{false};
+
+	int8_t _configuration_index{-1};
+	uint8_t _configuration_count{0};
+};
+
+} // namespace configuration
+} // namespace sensor

src/lib/sensor/configuration/Barometer.cpp

@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 "Barometer.hpp"
+
+#include "Utilities.hpp"
+
+#include <lib/parameters/param.h>
+#include <lib/sensor/Utilities.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+using namespace sensor::utilities;
+
+namespace sensor
+{
+namespace configuration
+{
+
+Barometer::Barometer()
+{
+	Reset();
+}
+
+Barometer::Barometer(uint32_t device_id)
+{
+	set_device_id(device_id);
+}
+
+void Barometer::set_device_id(uint32_t device_id)
+{
+	bool external = DeviceExternal(device_id);
+
+	if (_device_id != device_id || _external != external) {
+
+		_device_id = device_id;
+		_external = external;
+
+		Reset();
+
+		ParametersUpdate();
+	}
+}
+
+bool Barometer::set_configuration_index(int configuration_index)
+{
+	if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) {
+		_configuration_index = configuration_index;
+		return true;
+	}
+
+	return false;
+}
+
+void Barometer::ParametersUpdate()
+{
+	if (_device_id == 0) {
+		return;
+	}
+
+	_configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id);
+
+	if (_configuration_index == -1) {
+		// no saved configuration available
+		Reset();
+
+	} else {
+		ParametersLoad();
+	}
+}
+
+bool Barometer::ParametersLoad()
+{
+	if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
+		// SENS_{}n_PRIO
+		_priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index);
+
+		if ((_priority < 0) || (_priority > 100)) {
+			// reset to default, -1 is the uninitialized parameter value
+			static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1;
+
+			if (_priority != SENS_PRIO_UNINITIALIZED) {
+				PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
+					_configuration_index, _priority);
+
+				SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED);
+			}
+
+			_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+		}
+
+		return true;
+	}
+
+	return false;
+}
+
+void Barometer::Reset()
+{
+	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+
+	_configuration_index = -1;
+
+	_configuration_count = 0;
+}
+
+bool Barometer::ParametersSave(int desired_configuration_index, bool force)
+{
+	if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) {
+		_configuration_index = desired_configuration_index;
+
+	} else if (!force || (_configuration_index < 0)
+		   || (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) {
+
+		// ensure we have a valid configuration slot (matching existing or first available slot)
+		int8_t configuration_index_prev = _configuration_index;
+		_configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index);
+
+		if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) {
+			PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
+				 configuration_index_prev, _configuration_index);
+		}
+	}
+
+	if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
+		// save configuration
+		bool success = true;
+
+		success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id);
+		success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority);
+
+		return success;
+	}
+
+	return false;
+}
+
+void Barometer::PrintStatus()
+{
+	if (external()) {
+		PX4_INFO_RAW("%s %" PRIu32
+			     " EN: %d, External\n",
+			     SensorString(), device_id(), enabled());
+
+	} else {
+		PX4_INFO_RAW("%s %" PRIu32
+			     " EN: %d, Internal\n",
+			     SensorString(), device_id(), enabled());
+	}
+}
+
+} // namespace configuration
+} // namespace sensor

src/lib/sensor/configuration/Barometer.hpp

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 <px4_platform_common/px4_config.h>
+
+#include <lib/matrix/matrix/math.hpp>
+
+namespace sensor
+{
+namespace configuration
+{
+
+class Barometer
+{
+public:
+	static constexpr int MAX_SENSOR_COUNT = 4;
+
+	static constexpr uint8_t DEFAULT_PRIORITY = 50;
+	static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
+
+	static constexpr const char *SensorString() { return "BARO"; }
+
+	Barometer();
+	explicit Barometer(uint32_t device_id);
+
+	~Barometer() = default;
+
+	void PrintStatus();
+
+	bool set_configuration_index(int configuration_index);
+	void set_device_id(uint32_t device_id);
+
+	bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); }
+	uint8_t configuration_count() const { return _configuration_count; }
+	int8_t configuration_index() const { return _configuration_index; }
+	uint32_t device_id() const { return _device_id; }
+	bool enabled() const { return (_priority > 0); }
+	bool external() const { return _external; }
+
+	const int32_t &priority() const { return _priority; }
+
+	bool ParametersLoad();
+	bool ParametersSave(int desired_configuration_index = -1, bool force = false);
+	void ParametersUpdate();
+
+	void Reset();
+
+private:
+
+	uint32_t _device_id{0};
+	int32_t _priority{-1};
+
+	bool _external{false};
+
+	int8_t _configuration_index{-1};
+	uint8_t _configuration_count{0};
+};
+
+} // namespace configuration
+} // namespace sensor

src/lib/sensor/configuration/CMakeLists.txt

@@ -0,0 +1,53 @@
+############################################################################
+#
+#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(sensor_configuration
+	Accelerometer.cpp
+	Accelerometer.hpp
+	Barometer.cpp
+	Barometer.hpp
+	GNSS.cpp
+	GNSS.hpp
+	Gyroscope.cpp
+	Gyroscope.hpp
+	Magnetometer.cpp
+	Magnetometer.hpp
+	Utilities.cpp
+	Utilities.hpp
+)
+
+target_link_libraries(sensor_configuration
+	PRIVATE
+		conversion
+		sensor_utilities
+)

src/lib/sensor/configuration/GNSS.cpp

@@ -0,0 +1,204 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 "GNSS.hpp"
+
+#include "Utilities.hpp"
+
+#include <lib/parameters/param.h>
+#include <lib/sensor/Utilities.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+using namespace sensor::utilities;
+
+namespace sensor
+{
+namespace configuration
+{
+
+GNSS::GNSS()
+{
+	Reset();
+}
+
+GNSS::GNSS(uint32_t device_id)
+{
+	set_device_id(device_id);
+}
+
+void GNSS::set_device_id(uint32_t device_id)
+{
+	if (_device_id != device_id) {
+
+		_device_id = device_id;
+
+		Reset();
+
+		ParametersUpdate();
+	}
+}
+
+bool GNSS::set_configuration_index(int configuration_index)
+{
+	if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) {
+		_configuration_index = configuration_index;
+		return true;
+	}
+
+	return false;
+}
+
+bool GNSS::set_position(const matrix::Vector3f &position)
+{
+	static constexpr float MIN_POSITION_CHANGE = 1e-6;
+
+	if (position.isAllFinite()
+	    && (Vector3f(_position - position).longerThan(MIN_POSITION_CHANGE) || (_configuration_count == 0))
+	   ) {
+		_position = position;
+
+		_configuration_count++;
+		return true;
+	}
+
+	return false;
+}
+
+void GNSS::ParametersUpdate()
+{
+	if (_device_id == 0) {
+		return;
+	}
+
+	_configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id);
+
+	if (_configuration_index == -1) {
+		// no saved configuration available
+		Reset();
+
+	} else {
+		ParametersLoad();
+	}
+}
+
+bool GNSS::ParametersLoad()
+{
+	if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
+		// SENS_{}n_PRIO
+		_priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index);
+
+		if ((_priority < 0) || (_priority > 100)) {
+			// reset to default, -1 is the uninitialized parameter value
+			static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1;
+
+			if (_priority != SENS_PRIO_UNINITIALIZED) {
+				PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
+					_configuration_index, _priority);
+
+				SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED);
+			}
+
+			_priority = DEFAULT_PRIORITY;
+		}
+
+		// SENS_{}n_{X,Y,Z}POS:
+		Vector3f position{0.f, 0.f, 0.f};
+		position(0) = GetConfigurationParamFloat(SensorString(), "XPOS", _configuration_index);
+		position(1) = GetConfigurationParamFloat(SensorString(), "YPOS", _configuration_index);
+		position(2) = GetConfigurationParamFloat(SensorString(), "ZPOS", _configuration_index);
+		set_position(position);
+
+		return true;
+	}
+
+	return false;
+}
+
+void GNSS::Reset()
+{
+	_position.zero();
+
+	_priority = DEFAULT_PRIORITY;
+
+	_configuration_index = -1;
+
+	_configuration_count = 0;
+}
+
+bool GNSS::ParametersSave(int desired_configuration_index, bool force)
+{
+	if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) {
+		_configuration_index = desired_configuration_index;
+
+	} else if (!force || (_configuration_index < 0)
+		   || (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) {
+
+		// ensure we have a valid configuration slot (matching existing or first available slot)
+		int8_t configuration_index_prev = _configuration_index;
+		_configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index);
+
+		if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) {
+			PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
+				 configuration_index_prev, _configuration_index);
+		}
+	}
+
+	if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
+		// save configuration
+		bool success = true;
+
+		success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id);
+		success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority);
+
+		// SENS_{}n_{X,Y,Z}POS
+		success &= SetConfigurationParam(SensorString(), "XPOS", _configuration_index, _position(0));
+		success &= SetConfigurationParam(SensorString(), "YPOS", _configuration_index, _position(0));
+		success &= SetConfigurationParam(SensorString(), "ZPOS", _configuration_index, _position(0));
+
+		return success;
+	}
+
+	return false;
+}
+
+void GNSS::PrintStatus()
+{
+	PX4_INFO_RAW("%s %" PRIu32
+		     " EN: %d, position: [%05.3f %05.3f %05.3f]\n",
+		     SensorString(), device_id(), enabled(),
+		     (double)_position(0), (double)_position(1), (double)_position(2));
+}
+
+} // namespace configuration
+} // namespace sensor

src/lib/sensor/configuration/GNSS.hpp

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 <px4_platform_common/px4_config.h>
+
+#include <lib/matrix/matrix/math.hpp>
+
+namespace sensor
+{
+namespace configuration
+{
+
+class GNSS
+{
+public:
+	static constexpr int MAX_SENSOR_COUNT = 4;
+
+	static constexpr uint8_t DEFAULT_PRIORITY = 50;
+
+	static constexpr const char *SensorString() { return "GNSS"; }
+
+	GNSS();
+	explicit GNSS(uint32_t device_id);
+
+	~GNSS() = default;
+
+	void PrintStatus();
+
+	bool set_configuration_index(int configuration_index);
+	void set_device_id(uint32_t device_id);
+
+	bool set_position(const matrix::Vector3f &position);
+
+	bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); }
+	uint8_t configuration_count() const { return _configuration_count; }
+	int8_t configuration_index() const { return _configuration_index; }
+	uint32_t device_id() const { return _device_id; }
+	bool enabled() const { return (_priority > 0); }
+
+	const matrix::Vector3f &position() const { return _position; }
+	const int32_t &priority() const { return _priority; }
+
+	bool ParametersLoad();
+	bool ParametersSave(int desired_configuration_index = -1, bool force = false);
+	void ParametersUpdate();
+
+	void Reset();
+
+private:
+
+	uint32_t _device_id{0};
+	int32_t _priority{-1};
+
+	matrix::Vector3f _position{};
+
+	int8_t _configuration_index{-1};
+	uint8_t _configuration_count{0};
+};
+
+} // namespace configuration
+} // namespace sensor

src/lib/sensor/configuration/Gyroscope.cpp

@@ -0,0 +1,223 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2023 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 "Gyroscope.hpp"
+
+#include "Utilities.hpp"
+
+#include <lib/parameters/param.h>
+#include <lib/sensor/Utilities.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+using namespace sensor::utilities;
+
+namespace sensor
+{
+namespace configuration
+{
+
+Gyroscope::Gyroscope()
+{
+	Reset();
+}
+
+Gyroscope::Gyroscope(uint32_t device_id)
+{
+	set_device_id(device_id);
+}
+
+void Gyroscope::set_device_id(uint32_t device_id)
+{
+	bool external = DeviceExternal(device_id);
+
+	if (_device_id != device_id || _external != external) {
+
+		_device_id = device_id;
+		_external = external;
+
+		Reset();
+
+		ParametersUpdate();
+	}
+}
+
+bool Gyroscope::set_configuration_index(int configuration_index)
+{
+	if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) {
+		_configuration_index = configuration_index;
+		return true;
+	}
+
+	return false;
+}
+
+void Gyroscope::set_rotation(Rotation rotation)
+{
+	_rotation_enum = rotation;
+
+	// always apply board level adjustments
+	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
+}
+
+void Gyroscope::ParametersUpdate()
+{
+	if (_device_id == 0) {
+		return;
+	}
+
+	_configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id);
+
+	if (_configuration_index == -1) {
+		// no saved configuration available
+		Reset();
+
+	} else {
+		ParametersLoad();
+	}
+}
+
+bool Gyroscope::ParametersLoad()
+{
+	if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
+		// SENS_{}n_ROT
+		int32_t rotation_value = GetConfigurationParamInt32(SensorString(), "ROT", _configuration_index);
+
+		if (_external) {
+			if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
+				// invalid rotation, resetting
+				rotation_value = ROTATION_NONE;
+			}
+
+			set_rotation(static_cast<Rotation>(rotation_value));
+
+		} else {
+			// internal sensors follow board rotation
+			set_rotation(GetBoardRotation());
+		}
+
+		// SENS_{}n_PRIO
+		_priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index);
+
+		if ((_priority < 0) || (_priority > 100)) {
+			// reset to default, -1 is the uninitialized parameter value
+			static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1;
+
+			if (_priority != SENS_PRIO_UNINITIALIZED) {
+				PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
+					_configuration_index, _priority);
+
+				SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED);
+			}
+
+			_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+		}
+
+		return true;
+	}
+
+	return false;
+}
+
+void Gyroscope::Reset()
+{
+	if (_external) {
+		set_rotation(ROTATION_NONE);
+
+	} else {
+		// internal sensors follow board rotation
+		set_rotation(GetBoardRotation());
+	}
+
+	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+
+	_configuration_index = -1;
+
+	_configuration_count = 0;
+}
+
+bool Gyroscope::ParametersSave(int desired_configuration_index, bool force)
+{
+	if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) {
+		_configuration_index = desired_configuration_index;
+
+	} else if (!force || (_configuration_index < 0)
+		   || (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) {
+
+		// ensure we have a valid configuration slot (matching existing or first available slot)
+		int8_t configuration_index_prev = _configuration_index;
+		_configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index);
+
+		if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) {
+			PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
+				 configuration_index_prev, _configuration_index);
+		}
+	}
+
+	if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
+		// save configuration
+		bool success = true;
+
+		success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id);
+		success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority);
+
+		if (_external) {
+			success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, (int32_t)_rotation_enum);
+
+		} else {
+			success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, -1); // internal
+		}
+
+		return success;
+	}
+
+	return false;
+}
+
+void Gyroscope::PrintStatus()
+{
+	if (external()) {
+		PX4_INFO_RAW("%s %" PRIu32
+			     " EN: %d, Ext ROT: %d\n",
+			     SensorString(), device_id(), enabled(),
+			     rotation_enum());
+
+	} else {
+		PX4_INFO_RAW("%s %" PRIu32
+			     " EN: %d, Internal\n",
+			     SensorString(), device_id(), enabled());
+	}
+}
+
+} // namespace configuration
+} // namespace sensor

src/lib/sensor/configuration/Gyroscope.hpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2023 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 <px4_platform_common/px4_config.h>
+
+#include <lib/conversion/rotation.h>
+#include <lib/matrix/matrix/math.hpp>
+
+namespace sensor
+{
+namespace configuration
+{
+
+class Gyroscope
+{
+public:
+	static constexpr int MAX_SENSOR_COUNT = 4;
+
+	static constexpr uint8_t DEFAULT_PRIORITY = 50;
+	static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
+
+	static constexpr const char *SensorString() { return "GYRO"; }
+
+	Gyroscope();
+	explicit Gyroscope(uint32_t device_id);
+
+	~Gyroscope() = default;
+
+	void PrintStatus();
+
+	bool set_configuration_index(int configuration_index);
+	void set_device_id(uint32_t device_id);
+
+	void set_rotation(Rotation rotation);
+
+	bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); }
+	uint8_t configuration_count() const { return _configuration_count; }
+	int8_t configuration_index() const { return _configuration_index; }
+	uint32_t device_id() const { return _device_id; }
+	bool enabled() const { return (_priority > 0); }
+	bool external() const { return _external; }
+
+	const int32_t &priority() const { return _priority; }
+	const matrix::Dcmf &rotation() const { return _rotation; }
+	const Rotation &rotation_enum() const { return _rotation_enum; }
+
+	bool ParametersLoad();
+	bool ParametersSave(int desired_configuration_index = -1, bool force = false);
+	void ParametersUpdate();
+
+	void Reset();
+
+private:
+	Rotation _rotation_enum{ROTATION_NONE};
+
+	matrix::Dcmf _rotation;
+
+	uint32_t _device_id{0};
+	int32_t _priority{-1};
+
+	bool _external{false};
+
+	int8_t _configuration_index{-1};
+	uint8_t _configuration_count{0};
+};
+
+} // namespace configuration
+} // namespace sensor

src/lib/sensor/configuration/Magnetometer.cpp

@@ -0,0 +1,283 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2023 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 "Magnetometer.hpp"
+
+#include "Utilities.hpp"
+
+#include <lib/parameters/param.h>
+#include <lib/sensor/Utilities.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+using namespace sensor::utilities;
+
+namespace sensor
+{
+namespace configuration
+{
+
+Magnetometer::Magnetometer()
+{
+	Reset();
+}
+
+Magnetometer::Magnetometer(uint32_t device_id)
+{
+	set_device_id(device_id);
+}
+
+void Magnetometer::set_device_id(uint32_t device_id)
+{
+	bool external = DeviceExternal(device_id);
+
+	if (_device_id != device_id || _external != external) {
+
+		_device_id = device_id;
+		_external = external;
+
+		Reset();
+
+		ParametersUpdate();
+	}
+}
+
+bool Magnetometer::set_configuration_index(int configuration_index)
+{
+	if ((configuration_index >= 0) && (configuration_index < MAX_SENSOR_COUNT)) {
+		_configuration_index = configuration_index;
+		return true;
+	}
+
+	return false;
+}
+
+void Magnetometer::set_rotation(const Rotation rotation)
+{
+	if (rotation < ROTATION_MAX) {
+		_rotation_enum = rotation;
+
+	} else {
+		// invalid rotation, resetting
+		_rotation_enum = ROTATION_NONE;
+	}
+
+	// always apply level adjustments
+	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(_rotation_enum);
+
+	// clear any custom rotation
+	_rotation_custom_euler.zero();
+}
+
+void Magnetometer::set_custom_rotation(const Eulerf &rotation)
+{
+	_rotation_enum = ROTATION_CUSTOM;
+
+	// store custom rotation
+	_rotation_custom_euler = rotation;
+
+	// always apply board level adjustments
+	_rotation = Dcmf(GetSensorLevelAdjustment()) * Dcmf(_rotation_custom_euler);
+
+	// TODO: Note that ideally this shouldn't be necessary for an external sensors, as the definition of *rotation
+	// between sensor frame & vehicle's body frame isn't affected by the rotation of the Autopilot.
+	// however, since while doing the 'level-configuration', users don't put the vehicle truly *horizontal, the
+	// measured board roll/pitch offset isn't true. So this affects external sensors as well (which is why we apply
+	// internal SensorLevelAdjustment to all the sensors). We need to figure out how to set the sensor board offset
+	// values properly (i.e. finding Vehicle's true Forward-Right-Down frame in a user's perspective)
+}
+
+void Magnetometer::ParametersUpdate()
+{
+	if (_device_id == 0) {
+		return;
+	}
+
+	_configuration_index = FindCurrentConfigurationIndex(SensorString(), _device_id);
+
+	if (_configuration_index == -1) {
+		// no saved configuration available
+		Reset();
+
+	} else {
+		ParametersLoad();
+	}
+}
+
+bool Magnetometer::ParametersLoad()
+{
+	if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
+		// SENS_{}n_ROT
+		int32_t rotation_value = GetConfigurationParamInt32(SensorString(), "ROT", _configuration_index);
+
+		const float euler_roll_deg = GetConfigurationParamFloat(SensorString(), "ROLL", _configuration_index);
+		const float euler_pitch_deg = GetConfigurationParamFloat(SensorString(), "PITCH", _configuration_index);
+		const float euler_yaw_deg = GetConfigurationParamFloat(SensorString(), "YAW", _configuration_index);
+
+		if (_external) {
+			if (((rotation_value >= ROTATION_MAX) && (rotation_value != ROTATION_CUSTOM)) || (rotation_value < 0)) {
+				// invalid rotation, resetting
+				rotation_value = ROTATION_NONE;
+			}
+
+			// if SENS_{}n_{ROLL,PITCH,YAW} manually set then SENS_{}n_ROT needs to be ROTATION_CUSTOM
+			if ((rotation_value != ROTATION_CUSTOM)
+			    && ((fabsf(euler_roll_deg) > FLT_EPSILON)
+				|| (fabsf(euler_pitch_deg) > FLT_EPSILON)
+				|| (fabsf(euler_yaw_deg) > FLT_EPSILON))) {
+
+				rotation_value = ROTATION_CUSTOM;
+				SetConfigurationParam(SensorString(), "ROT", _configuration_index, rotation_value);
+			}
+
+			// Handle custom specified euler angle
+			if (rotation_value == ROTATION_CUSTOM) {
+
+				const matrix::Eulerf rotation_custom_euler{
+					math::radians(euler_roll_deg),
+					math::radians(euler_pitch_deg),
+					math::radians(euler_yaw_deg)};
+
+				set_custom_rotation(rotation_custom_euler);
+
+			} else {
+				set_rotation(static_cast<Rotation>(rotation_value));
+			}
+
+		} else {
+			// internal sensors follow board rotation
+			set_rotation(GetBoardRotation());
+		}
+
+		// SENS_{}n_PRIO
+		_priority = GetConfigurationParamInt32(SensorString(), "PRIO", _configuration_index);
+
+		if ((_priority < 0) || (_priority > 100)) {
+			// reset to default, -1 is the uninitialized parameter value
+			static constexpr int32_t SENS_PRIO_UNINITIALIZED = -1;
+
+			if (_priority != SENS_PRIO_UNINITIALIZED) {
+				PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
+					_configuration_index, _priority);
+
+				SetConfigurationParam(SensorString(), "PRIO", _configuration_index, SENS_PRIO_UNINITIALIZED);
+			}
+
+			_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+		}
+
+		return true;
+	}
+
+	return false;
+}
+
+void Magnetometer::Reset()
+{
+	if (_external) {
+		set_rotation(ROTATION_NONE);
+
+	} else {
+		// internal sensors follow board rotation
+		set_rotation(GetBoardRotation());
+	}
+
+	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+
+	_configuration_index = -1;
+
+	_configuration_count = 0;
+}
+
+bool Magnetometer::ParametersSave(int desired_configuration_index, bool force)
+{
+	if (force && desired_configuration_index >= 0 && desired_configuration_index < MAX_SENSOR_COUNT) {
+		_configuration_index = desired_configuration_index;
+
+	} else if (!force || (_configuration_index < 0)
+		   || (desired_configuration_index != -1 && desired_configuration_index != _configuration_index)) {
+
+		// ensure we have a valid configuration slot (matching existing or first available slot)
+		int8_t configuration_index_prev = _configuration_index;
+		_configuration_index = FindAvailableConfigurationIndex(SensorString(), _device_id, desired_configuration_index);
+
+		if (configuration_index_prev >= 0 && (configuration_index_prev != _configuration_index)) {
+			PX4_WARN("%s %" PRIu32 " configuration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
+				 configuration_index_prev, _configuration_index);
+		}
+	}
+
+	if (_configuration_index >= 0 && _configuration_index < MAX_SENSOR_COUNT) {
+		// save configuration
+		bool success = true;
+
+		success &= SetConfigurationParam(SensorString(), "ID", _configuration_index, _device_id);
+		success &= SetConfigurationParam(SensorString(), "PRIO", _configuration_index, _priority);
+
+		if (_external) {
+			success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, (int32_t)_rotation_enum);
+
+		} else {
+			success &= SetConfigurationParam(SensorString(), "ROT", _configuration_index, -1); // internal
+		}
+
+		success &= SetConfigurationParam(SensorString(), "ROLL", _configuration_index,
+						 math::degrees(_rotation_custom_euler(0)));
+		success &= SetConfigurationParam(SensorString(), "PITCH", _configuration_index,
+						 math::degrees(_rotation_custom_euler(1)));
+		success &= SetConfigurationParam(SensorString(), "YAW", _configuration_index,
+						 math::degrees(_rotation_custom_euler(2)));
+
+		return success;
+	}
+
+	return false;
+}
+
+void Magnetometer::PrintStatus()
+{
+	if (external()) {
+		PX4_INFO_RAW("%s %" PRIu32
+			     " EN: %d, Ext ROT: %d\n",
+			     SensorString(), device_id(), enabled(),
+			     rotation_enum());
+
+	} else {
+		PX4_INFO_RAW("%s %" PRIu32
+			     " EN: %d, Internal\n",
+			     SensorString(), device_id(), enabled());
+	}
+}
+
+} // namespace configuration
+} // namespace sensor

src/lib/sensor/configuration/Magnetometer.hpp

@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2023 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 <px4_platform_common/px4_config.h>
+
+#include <lib/conversion/rotation.h>
+#include <lib/matrix/matrix/math.hpp>
+
+namespace sensor
+{
+namespace configuration
+{
+
+class Magnetometer
+{
+public:
+	static constexpr int MAX_SENSOR_COUNT = 4;
+
+	static constexpr uint8_t DEFAULT_PRIORITY = 50;
+	static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
+
+	static constexpr const char *SensorString() { return "MAG"; }
+
+	Magnetometer();
+	explicit Magnetometer(uint32_t device_id);
+
+	~Magnetometer() = default;
+
+	void PrintStatus();
+
+	bool set_configuration_index(int configuration_index);
+	void set_device_id(uint32_t device_id);
+
+	void set_rotation(Rotation rotation);
+
+	/**
+	 * @brief Set the custom rotation & rotation enum to ROTATION_CUSTOM for Magnetometer
+	 *
+	 * @param rotation Rotation euler angles
+	 */
+	void set_custom_rotation(const matrix::Eulerf &rotation);
+
+	bool configured() const { return (_device_id != 0) && (_configuration_index >= 0); }
+	uint8_t configuration_count() const { return _configuration_count; }
+	int8_t configuration_index() const { return _configuration_index; }
+	uint32_t device_id() const { return _device_id; }
+	bool enabled() const { return (_priority > 0); }
+	bool external() const { return _external; }
+
+	const int32_t &priority() const { return _priority; }
+	const matrix::Dcmf &rotation() const { return _rotation; }
+	const Rotation &rotation_enum() const { return _rotation_enum; }
+
+	bool ParametersLoad();
+	bool ParametersSave(int desired_configuration_index = -1, bool force = false);
+	void ParametersUpdate();
+
+	void Reset();
+
+private:
+	Rotation _rotation_enum{ROTATION_NONE};
+
+	/**
+	 * @brief 3 x 3 Rotation matrix that translates from sensor frame (XYZ) to vehicle body frame (FRD)
+	 */
+	matrix::Dcmf _rotation;
+
+	matrix::Eulerf _rotation_custom_euler{0.f, 0.f, 0.f}; // custom rotation euler angles (optional)
+
+	uint32_t _device_id{0};
+	int32_t _priority{-1};
+
+	bool _external{false};
+
+	int8_t _configuration_index{-1};
+	uint8_t _configuration_count{0};
+};
+
+} // namespace configuration
+} // namespace sensor

src/lib/sensor/configuration/Utilities.cpp

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 "Utilities.hpp"
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <lib/conversion/rotation.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/parameters/param.h>
+
+using matrix::Eulerf;
+using matrix::Dcmf;
+using matrix::Vector3f;
+
+namespace sensor
+{
+namespace configuration
+{
+
+static constexpr int MAX_SENSOR_COUNT = 4; // TODO: per sensor?
+
+int8_t FindCurrentConfigurationIndex(const char *sensor_type, uint32_t device_id)
+{
+	if (device_id == 0) {
+		return -1;
+	}
+
+	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
+		char str[16 + 1] {};
+		snprintf(str, sizeof(str), "SENS_%s%u_ID", sensor_type, i);
+
+		int32_t device_id_val = 0;
+
+		param_t param_handle = param_find_no_notification(str);
+
+		if (param_handle == PARAM_INVALID) {
+			continue;
+		}
+
+		// find again and get value, but this time mark it active
+		if (param_get(param_find(str), &device_id_val) != OK) {
+			continue;
+		}
+
+		if ((uint32_t)device_id_val == device_id) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+int8_t FindAvailableConfigurationIndex(const char *sensor_type, uint32_t device_id, int8_t preferred_index)
+{
+	// if this device is already using a configuration slot then keep it
+	int configuration_index = FindCurrentConfigurationIndex(sensor_type, device_id);
+
+	if (configuration_index >= 0) {
+		return configuration_index;
+	}
+
+
+	// device isn't currently using a configuration slot, select user preference (preferred_index)
+	//  if available, otherwise use the first available slot
+	uint32_t conf_device_ids[MAX_SENSOR_COUNT] {};
+
+	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
+		char str[16 + 1] {};
+		snprintf(str, sizeof(str), "SENS_%s%u_ID", sensor_type, i);
+		int32_t device_id_val = 0;
+
+		if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
+			conf_device_ids[i] = device_id_val;
+		}
+	}
+
+	// use preferred_index if it's available
+	if ((preferred_index >= 0) && (preferred_index < MAX_SENSOR_COUNT)
+	    && (conf_device_ids[preferred_index] == 0)) {
+
+		configuration_index = preferred_index;
+
+	} else {
+		// otherwise select first available slot
+		for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+			if (conf_device_ids[i] == 0) {
+				configuration_index = i;
+				break;
+			}
+		}
+	}
+
+	if (configuration_index == -1) {
+		PX4_ERR("no %s configuration slots available", sensor_type);
+	}
+
+	return configuration_index;
+}
+
+int32_t GetConfigurationParamInt32(const char *sensor_type, const char *conf_type, uint8_t instance)
+{
+	// eg SENS_{}n_ID
+	char str[16 + 1] {};
+	snprintf(str, sizeof(str), "SENS_%s%" PRIu8 "_%s", sensor_type, instance, conf_type);
+
+	int32_t value = 0;
+
+	if (param_get(param_find(str), &value) != 0) {
+		PX4_ERR("failed to get %s", str);
+	}
+
+	return value;
+}
+
+float GetConfigurationParamFloat(const char *sensor_type, const char *conf_type, uint8_t instance)
+{
+	// eg SENS_{}n_OFF
+	char str[16 + 1] {};
+	snprintf(str, sizeof(str), "SENS_%s%" PRIu8 "_%s", sensor_type, instance, conf_type);
+
+	float value = NAN;
+
+	if (param_get(param_find(str), &value) != 0) {
+		PX4_ERR("failed to get %s", str);
+	}
+
+	return value;
+}
+
+} // namespace configuration
+} // namespace sensor

src/lib/sensor/configuration/Utilities.hpp

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <lib/conversion/rotation.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/parameters/param.h>
+#include <matrix/math.hpp>
+
+namespace sensor
+{
+namespace configuration
+{
+
+/**
+ * @brief Find sensor's configuration index if it exists.
+ *
+ * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG")
+ * @param device_id
+ * @return int8_t Valid configuration index on success, -1 otherwise
+ */
+int8_t FindCurrentConfigurationIndex(const char *sensor_type, uint32_t device_id);
+
+/**
+ * @brief Find sensor's configuration index if it exists, otherwise select an available slot.
+ *
+ * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG")
+ * @param device_id
+ * @param preferred_index preferred index (optional)
+ * @return int8_t Valid configuration index on success, -1 otherwise
+ */
+int8_t FindAvailableConfigurationIndex(const char *sensor_type, uint32_t device_id, int8_t preferred_index = -1);
+
+/**
+ * @brief Get sensor configuration parameter value.
+ *
+ * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GYRO", "MAG")
+ * @param conf_type Configuration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO")
+ * @param instance
+ * @return int32_t The configuration value.
+ */
+int32_t GetConfigurationParamInt32(const char *sensor_type, const char *conf_type, uint8_t instance);
+float GetConfigurationParamFloat(const char *sensor_type, const char *conf_type, uint8_t instance);
+
+/**
+ * @brief Set a single configuration paramter.
+ *
+ * @param sensor_type Configuration parameter abbreviated sensor string ("ACC", "GNSS", "GYRO", "MAG")
+ * @param conf_type Configuration parameter abbreviated type ("OFF", "SCALE", "ROT", "PRIO")
+ * @param instance Configuration index (0 - 3)
+ * @param value int32_t parameter value
+ * @return true if the parameter name was valid and value saved successfully, false otherwise.
+ */
+template<typename T>
+bool SetConfigurationParam(const char *sensor_type, const char *conf_type, uint8_t instance, T value)
+{
+	char str[16 + 1] {};
+
+	// eg SENS_{}n_ID
+	snprintf(str, sizeof(str), "SENS_%s%u_%s", sensor_type, instance, conf_type);
+
+	int ret = param_set_no_notification(param_find(str), &value);
+
+	if (ret != PX4_OK) {
+		PX4_ERR("failed to set %s", str);
+	}
+
+	return ret == PX4_OK;
+}
+
+} // namespace utilities
+} // namespace sensor

src/lib/sensor_calibration/Magnetometer.cpp

@@ -1,400 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2022 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 "Magnetometer.hpp"
-
-#include "Utilities.hpp"
-
-#include <lib/parameters/param.h>
-
-using namespace matrix;
-using namespace time_literals;
-
-namespace calibration
-{
-
-Magnetometer::Magnetometer()
-{
-	Reset();
-}
-
-Magnetometer::Magnetometer(uint32_t device_id)
-{
-	set_device_id(device_id);
-}
-
-void Magnetometer::set_device_id(uint32_t device_id)
-{
-	bool external = DeviceExternal(device_id);
-
-	if (_device_id != device_id || _external != external) {
-
-		_device_id = device_id;
-		_external = external;
-
-		Reset();
-
-		ParametersUpdate();
-		SensorCorrectionsUpdate(true);
-	}
-}
-
-void Magnetometer::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 (corrections.mag_device_ids[i] == _device_id) {
-					switch (i) {
-					case 0:
-						_thermal_offset = Vector3f{corrections.mag_offset_0};
-						return;
-					case 1:
-						_thermal_offset = Vector3f{corrections.mag_offset_1};
-						return;
-					case 2:
-						_thermal_offset = Vector3f{corrections.mag_offset_2};
-						return;
-					case 3:
-						_thermal_offset = Vector3f{corrections.mag_offset_3};
-						return;
-					}
-				}
-			}
-		}
-
-		// zero thermal offset if not found
-		_thermal_offset.zero();
-	}
-}
-
-bool Magnetometer::set_offset(const Vector3f &offset)
-{
-	if (Vector3f(_offset - offset).longerThan(0.005f)) {
-		if (offset.isAllFinite()) {
-			_offset = offset;
-			_calibration_count++;
-			return true;
-		}
-	}
-
-	return false;
-}
-
-bool Magnetometer::set_scale(const Vector3f &scale)
-{
-	if (Vector3f(_scale.diag() - scale).longerThan(0.01f)) {
-		if (scale.isAllFinite() && (scale(0) > 0.f) && (scale(1) > 0.f) && (scale(2) > 0.f)) {
-			_scale(0, 0) = scale(0);
-			_scale(1, 1) = scale(1);
-			_scale(2, 2) = scale(2);
-
-			_calibration_count++;
-			return true;
-		}
-	}
-
-	return false;
-}
-
-bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal)
-{
-	if (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiagonal).longerThan(0.01f)) {
-		if (offdiagonal.isAllFinite()) {
-
-			_scale(0, 1) = offdiagonal(0);
-			_scale(1, 0) = offdiagonal(0);
-
-			_scale(0, 2) = offdiagonal(1);
-			_scale(2, 0) = offdiagonal(1);
-
-			_scale(1, 2) = offdiagonal(2);
-			_scale(2, 1) = offdiagonal(2);
-
-			_calibration_count++;
-			return true;
-		}
-	}
-
-	return false;
-}
-
-void Magnetometer::set_rotation(const Rotation rotation)
-{
-	if (rotation < ROTATION_MAX) {
-		_rotation_enum = rotation;
-
-	} else {
-		// invalid rotation, resetting
-		_rotation_enum = ROTATION_NONE;
-	}
-
-	// always apply level adjustments
-	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(_rotation_enum);
-
-	// clear any custom rotation
-	_rotation_custom_euler.zero();
-}
-
-void Magnetometer::set_custom_rotation(const Eulerf &rotation)
-{
-	_rotation_enum = ROTATION_CUSTOM;
-
-	// store custom rotation
-	_rotation_custom_euler = rotation;
-
-	// always apply board level adjustments
-	_rotation = Dcmf(GetSensorLevelAdjustment()) * Dcmf(_rotation_custom_euler);
-
-	// TODO: Note that ideally this shouldn't be necessary for an external sensors, as the definition of *rotation
-	// between sensor frame & vehicle's body frame isn't affected by the rotation of the Autopilot.
-	// however, since while doing the 'level-calibration', users don't put the vehicle truly *horizontal, the
-	// measured board roll/pitch offset isn't true. So this affects external sensors as well (which is why we apply
-	// internal SensorLevelAdjustment to all the sensors). We need to figure out how to set the sensor board offset
-	// values properly (i.e. finding Vehicle's true Forward-Right-Down frame in a user's perspective)
-}
-
-bool Magnetometer::set_calibration_index(int calibration_index)
-{
-	if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
-		_calibration_index = calibration_index;
-		return true;
-	}
-
-	return false;
-}
-
-void Magnetometer::ParametersUpdate()
-{
-	if (_device_id == 0) {
-		return;
-	}
-
-	_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
-
-	if (_calibration_index == -1) {
-		// no saved calibration available
-		Reset();
-
-	} else {
-		ParametersLoad();
-	}
-}
-
-bool Magnetometer::ParametersLoad()
-{
-	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
-		// CAL_MAGx_ROT
-		int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index);
-
-		const float euler_roll_deg = GetCalibrationParamFloat(SensorString(), "ROLL", _calibration_index);
-		const float euler_pitch_deg = GetCalibrationParamFloat(SensorString(), "PITCH", _calibration_index);
-		const float euler_yaw_deg = GetCalibrationParamFloat(SensorString(), "YAW", _calibration_index);
-
-		if (_external) {
-			if (((rotation_value >= ROTATION_MAX) && (rotation_value != ROTATION_CUSTOM)) || (rotation_value < 0)) {
-				// invalid rotation, resetting
-				rotation_value = ROTATION_NONE;
-			}
-
-			// if CAL_MAGx_{ROLL,PITCH,YAW} manually set then CAL_MAGx_ROT needs to be ROTATION_CUSTOM
-			if ((rotation_value != ROTATION_CUSTOM)
-			    && ((fabsf(euler_roll_deg) > FLT_EPSILON)
-				|| (fabsf(euler_pitch_deg) > FLT_EPSILON)
-				|| (fabsf(euler_yaw_deg) > FLT_EPSILON))) {
-
-				rotation_value = ROTATION_CUSTOM;
-				SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
-			}
-
-			// Handle custom specified euler angle
-			if (rotation_value == ROTATION_CUSTOM) {
-
-				const matrix::Eulerf rotation_custom_euler{
-					math::radians(euler_roll_deg),
-					math::radians(euler_pitch_deg),
-					math::radians(euler_yaw_deg)};
-
-				set_custom_rotation(rotation_custom_euler);
-
-			} else {
-				set_rotation(static_cast<Rotation>(rotation_value));
-			}
-
-		} else {
-			// internal sensors follow board rotation
-			set_rotation(GetBoardRotation());
-		}
-
-		// CAL_MAGx_PRIO
-		_priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index);
-
-		if ((_priority < 0) || (_priority > 100)) {
-			// reset to default, -1 is the uninitialized parameter value
-			static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1;
-
-			if (_priority != CAL_PRIO_UNINITIALIZED) {
-				PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
-					_calibration_index, _priority);
-
-				SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED);
-			}
-
-			_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
-		}
-
-		// CAL_MAGx_OFF{X,Y,Z}
-		set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
-
-		// CAL_MAGx_SCALE{X,Y,Z}
-		set_scale(GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index));
-
-		// CAL_MAGx_ODIAG{X,Y,Z}
-		set_offdiagonal(GetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index));
-
-		// CAL_MAGx_COMP{X,Y,Z}
-		_power_compensation = GetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index);
-
-		return true;
-	}
-
-	return false;
-}
-
-void Magnetometer::Reset()
-{
-	if (_external) {
-		set_rotation(ROTATION_NONE);
-
-	} else {
-		// internal sensors follow board rotation
-		set_rotation(GetBoardRotation());
-	}
-
-	_offset.zero();
-	_scale.setIdentity();
-
-	_power_compensation.zero();
-	_power = 0.f;
-
-	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
-
-	_calibration_index = -1;
-
-	_calibration_count = 0;
-}
-
-bool Magnetometer::ParametersSave(int desired_calibration_index, bool force)
-{
-	if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
-		_calibration_index = desired_calibration_index;
-
-	} else if (!force || (_calibration_index < 0)
-		   || (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) {
-
-		// ensure we have a valid calibration slot (matching existing or first available slot)
-		int8_t calibration_index_prev = _calibration_index;
-		_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
-
-		if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
-			PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
-				 calibration_index_prev, _calibration_index);
-		}
-	}
-
-	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
-		// save calibration
-		bool success = true;
-		success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
-		success &= SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
-		success &= SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
-
-		const Vector3f scale{_scale.diag()};
-		success &= SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, scale);
-
-		const Vector3f off_diag{_scale(0, 1), _scale(0, 2), _scale(1, 2)};
-		success &= SetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index, off_diag);
-
-		success &= SetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index, _power_compensation);
-
-		if (_external) {
-			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
-
-		} else {
-			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
-		}
-
-		success &= SetCalibrationParam(SensorString(), "ROLL", _calibration_index, math::degrees(_rotation_custom_euler(0)));
-		success &= SetCalibrationParam(SensorString(), "PITCH", _calibration_index, math::degrees(_rotation_custom_euler(1)));
-		success &= SetCalibrationParam(SensorString(), "YAW", _calibration_index, math::degrees(_rotation_custom_euler(2)));
-
-		return success;
-	}
-
-	return false;
-}
-
-void Magnetometer::PrintStatus()
-{
-	if (external()) {
-		PX4_INFO_RAW("%s %" PRIu32
-			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Ext ROT: %d\n",
-			     SensorString(), device_id(), enabled(),
-			     (double)_offset(0), (double)_offset(1), (double)_offset(2),
-			     (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2),
-			     rotation_enum());
-
-	} else {
-		PX4_INFO_RAW("%s %" PRIu32
-			     " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Internal\n",
-			     SensorString(), device_id(), enabled(),
-			     (double)_offset(0), (double)_offset(1), (double)_offset(2),
-			     (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2));
-	}
-
-#if defined(DEBUG_BUILD)
-	_scale.print();
-#endif // DEBUG_BUILD
-}
-
-} // namespace calibration

src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp

@@ -33,7 +33,7 @@
 
 #include "accelerometerCheck.hpp"
 
-#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/sensor/calibration/Utilities.hpp>
 
 using namespace time_literals;
 
@@ -59,7 +59,7 @@ void AccelerometerChecks::checkAndReport(const Context &context, Report &reporte
 				is_calibration_valid = true;
 
 			} else {
-				is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel_data.device_id) >= 0);
+				is_calibration_valid = (sensor::calibration::FindCurrentCalibrationIndex("ACC", accel_data.device_id) >= 0);
 			}
 
 			reporter.setIsPresent(health_component_t::gyro);

src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.hpp

@@ -39,7 +39,7 @@
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/sensor_accel.h>
-#include <lib/sensor_calibration/Accelerometer.hpp>
+#include <lib/sensor/calibration/Accelerometer.hpp>
 
 class AccelerometerChecks : public HealthAndArmingCheckBase
 {
@@ -52,6 +52,6 @@ public:
 private:
 	bool isAccelRequired(int instance);
 
-	uORB::SubscriptionMultiArray<sensor_accel_s, calibration::Accelerometer::MAX_SENSOR_COUNT> _sensor_accel_sub{ORB_ID::sensor_accel};
+	uORB::SubscriptionMultiArray<sensor_accel_s, sensor::calibration::Accelerometer::MAX_SENSOR_COUNT> _sensor_accel_sub{ORB_ID::sensor_accel};
 	uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
 };

src/modules/commander/HealthAndArmingChecks/checks/baroCheck.hpp

@@ -39,7 +39,7 @@
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/estimator_status.h>
-#include <lib/sensor_calibration/Barometer.hpp>
+#include <lib/sensor/calibration/Barometer.hpp>
 
 class BaroChecks : public HealthAndArmingCheckBase
 {
@@ -52,7 +52,7 @@ public:
 private:
 	bool isBaroRequired(int instance);
 
-	uORB::SubscriptionMultiArray<sensor_baro_s, calibration::Barometer::MAX_SENSOR_COUNT> _sensor_baro_sub{ORB_ID::sensor_baro};
+	uORB::SubscriptionMultiArray<sensor_baro_s, sensor::calibration::Barometer::MAX_SENSOR_COUNT> _sensor_baro_sub{ORB_ID::sensor_baro};
 	uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,

src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.cpp

@@ -33,7 +33,7 @@
 
 #include "gyroCheck.hpp"
 
-#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/sensor/calibration/Utilities.hpp>
 
 using namespace time_literals;
 
@@ -59,7 +59,7 @@ void GyroChecks::checkAndReport(const Context &context, Report &reporter)
 				is_calibration_valid = true;
 
 			} else {
-				is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro_data.device_id) >= 0);
+				is_calibration_valid = (sensor::calibration::FindCurrentCalibrationIndex("GYRO", gyro_data.device_id) >= 0);
 			}
 
 			reporter.setIsPresent(health_component_t::gyro);

src/modules/commander/HealthAndArmingChecks/checks/gyroCheck.hpp

@@ -39,7 +39,7 @@
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/sensor_gyro.h>
-#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <lib/sensor/calibration/Gyroscope.hpp>
 
 class GyroChecks : public HealthAndArmingCheckBase
 {
@@ -52,6 +52,6 @@ public:
 private:
 	bool isGyroRequired(int instance);
 
-	uORB::SubscriptionMultiArray<sensor_gyro_s, calibration::Gyroscope::MAX_SENSOR_COUNT> _sensor_gyro_sub{ORB_ID::sensor_gyro};
+	uORB::SubscriptionMultiArray<sensor_gyro_s, sensor::calibration::Gyroscope::MAX_SENSOR_COUNT> _sensor_gyro_sub{ORB_ID::sensor_gyro};
 	uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
 };

src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp

@@ -33,7 +33,8 @@
 
 #include "magnetometerCheck.hpp"
 
-#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/sensor/calibration/Utilities.hpp>
+#include <lib/sensor/configuration/Utilities.hpp>
 
 using namespace time_literals;
 
@@ -68,14 +69,18 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter
 				num_enabled_and_valid_calibration++;
 
 			} else {
-				int calibration_index = calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id);
+				int calibration_index = sensor::calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id);
 				is_calibration_valid = (calibration_index >= 0);
 
 				if (is_calibration_valid) {
-					int priority = calibration::GetCalibrationParamInt32("MAG", "PRIO", calibration_index);
+					int configuration_index = sensor::configuration::FindCurrentConfigurationIndex("MAG", mag_data.device_id);
 
-					if (priority > 0) {
+					if (configuration_index >= 0) {
-						++num_enabled_and_valid_calibration;
+						int priority = sensor::configuration::GetConfigurationParamInt32("MAG", "PRIO", configuration_index);
+
+						if (priority > 0) {
+							++num_enabled_and_valid_calibration;
+						}
 					}
 				}
 			}

src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.hpp

@@ -40,7 +40,7 @@
 #include <uORB/topics/sensor_preflight_mag.h>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/estimator_status.h>
-#include <lib/sensor_calibration/Magnetometer.hpp>
+#include <lib/sensor/calibration/Magnetometer.hpp>
 
 class MagnetometerChecks : public HealthAndArmingCheckBase
 {
@@ -54,7 +54,7 @@ private:
 	bool isMagRequired(int instance, bool &mag_fault);
 	void consistencyCheck(const Context &context, Report &reporter);
 
-	uORB::SubscriptionMultiArray<sensor_mag_s, calibration::Magnetometer::MAX_SENSOR_COUNT> _sensor_mag_sub{ORB_ID::sensor_mag};
+	uORB::SubscriptionMultiArray<sensor_mag_s, sensor::calibration::Magnetometer::MAX_SENSOR_COUNT> _sensor_mag_sub{ORB_ID::sensor_mag};
 	uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
 
 	uORB::Subscription _sensor_preflight_mag_sub{ORB_ID(sensor_preflight_mag)};

src/modules/commander/accelerometer_calibration.cpp

@@ -131,8 +131,8 @@
 #include <px4_platform_common/time.h>
 
 #include <drivers/drv_hrt.h>
-#include <lib/sensor_calibration/Accelerometer.hpp>
+#include <lib/sensor/calibration/Accelerometer.hpp>
-#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/sensor/Utilities.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <matrix/math.hpp>
@@ -229,7 +229,7 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
 	}
 
 	// rotate sensor measurements from sensor to body frame using board rotation matrix
-	const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
+	const Dcmf board_rotation = sensor::utilities::GetBoardRotationMatrix();
 
 	for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
 		accel_sum[s] = board_rotation * accel_sum[s];
@@ -325,7 +325,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 {
 	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
 
-	calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {};
+	sensor::calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {};
 	unsigned active_sensors = 0;
 
 	for (uint8_t cur_accel = 0; cur_accel < MAX_ACCEL_SENS; cur_accel++) {
@@ -360,7 +360,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 	if (calibrate_from_orientation(mavlink_log_pub, data_collected, accel_calibration_worker, &worker_data,
 				       false) == calibrate_return_ok) {
 
-		const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
+		const Dcmf board_rotation = sensor::utilities::GetBoardRotationMatrix();
 		const Dcmf board_rotation_t = board_rotation.transpose();
 
 		bool param_save = false;
@@ -556,7 +556,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 				calibrated = true;
 			}
 
-			calibration::Accelerometer calibration{arp.device_id};
+			sensor::calibration::Accelerometer calibration{arp.device_id};
 
 			if (!calibrated || (offset.norm() > CONSTANTS_ONE_G) || !offset.isAllFinite()) {
 				PX4_ERR("accel %d quick calibrate failed", accel_index);

src/modules/commander/baro_calibration.cpp

@@ -49,8 +49,8 @@
 #include <drivers/drv_hrt.h>
 #include <matrix/math.hpp>
 #include <lib/atmosphere/atmosphere.h>
-#include <lib/sensor_calibration/Barometer.hpp>
+#include <lib/sensor/calibration/Barometer.hpp>
-#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/sensor/Utilities.hpp>
 #include <lib/systemlib/mavlink_log.h>
 #include <lib/systemlib/err.h>
 #include <uORB/Subscription.hpp>
@@ -77,7 +77,7 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
 
 
 	uORB::SubscriptionMultiArray<sensor_baro_s, MAX_SENSOR_COUNT> sensor_baro_subs{ORB_ID::sensor_baro};
-	calibration::Barometer calibration[MAX_SENSOR_COUNT] {};
+	sensor::calibration::Barometer calibration[MAX_SENSOR_COUNT] {};
 
 	uint64_t timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
 	float data_sum[MAX_SENSOR_COUNT] {};

src/modules/commander/gyro_calibration.cpp

@@ -52,8 +52,8 @@
 #include <lib/mathlib/math/filter/MedianFilter.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
-#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <lib/sensor/calibration/Gyroscope.hpp>
-#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/sensor/Utilities.hpp>
 #include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionBlocking.hpp>
@@ -68,7 +68,7 @@ using matrix::Vector3f;
 struct gyro_worker_data_t {
 	orb_advert_t *mavlink_log_pub{nullptr};
 
-	calibration::Gyroscope calibrations[MAX_GYROS] {};
+	sensor::calibration::Gyroscope calibrations[MAX_GYROS] {};
 
 	Vector3f offset[MAX_GYROS] {};
 

src/modules/commander/mag_calibration.cpp

@@ -50,8 +50,8 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
 #include <matrix/math.hpp>
-#include <lib/sensor_calibration/Magnetometer.hpp>
+#include <lib/sensor/calibration/Magnetometer.hpp>
-#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/sensor/Utilities.hpp>
 #include <lib/conversion/rotation.h>
 #include <lib/world_magnetic_model/geo_mag_declination.h>
 #include <lib/systemlib/mavlink_log.h>
@@ -94,7 +94,7 @@ struct mag_worker_data_t {
 	float		*y[MAX_MAGS];
 	float		*z[MAX_MAGS];
 
-	calibration::Magnetometer calibration[MAX_MAGS] {};
+	sensor::calibration::Magnetometer calibration[MAX_MAGS] {};
 };
 
 int do_mag_calibration(orb_advert_t *mavlink_log_pub)
@@ -714,7 +714,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 			// only proceed if there's a valid internal
 			if (internal_index >= 0) {
 
-				const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
+				const Dcmf board_rotation = sensor::utilities::GetBoardRotationMatrix();
 
 				// apply new calibrations to all raw sensor data before comparison
 				for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
@@ -1024,7 +1024,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
 
 			if (mag_sub.advertised() && (mag.timestamp != 0) && (mag.device_id != 0)) {
 
-				calibration::Magnetometer cal{mag.device_id};
+				sensor::calibration::Magnetometer cal{mag.device_id};
 
 				// use any existing scale and store the offset to the expected earth field
 				const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field);

src/modules/ekf2/EKF/common.h

@@ -177,6 +177,7 @@ struct gpsMessage {
 	bool        vel_ned_valid{};    ///< GPS ground speed is valid
 	uint8_t     nsats{};            ///< number of satellites used
 	float       pdop{};             ///< position dilution of precision
+	Vector3f    position_body{};    ///< xyz position of the GPS antenna in body frame (m)
 };
 
 struct imuSample {
@@ -198,6 +199,7 @@ struct gpsSample {
 	float       vacc{};     ///< 1-std vertical position error (m)
 	float       sacc{};     ///< 1-std speed error (m/sec)
 	float       yaw_acc{};  ///< 1-std yaw error (rad)
+	Vector3f    position_body{}; ///< xyz position of the GPS antenna in body frame (m)
 };
 
 struct magSample {

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -217,6 +217,8 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 			gps_sample_new.pos(1) = 0.0f;
 		}
 
+		gps_sample_new.position_body = gps.position_body;
+
 		_gps_buffer->push(gps_sample_new);
 		_time_last_gps_buffer_push = _time_latest_us;
 

src/modules/ekf2/EKF/gps_control.cpp

@@ -53,7 +53,8 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 
 	if (_gps_data_ready) {
 		// correct velocity for offset relative to IMU
-		const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
+		const Vector3f gps_pos_body = _gps_sample_delayed.position_body;
+		const Vector3f pos_offset_body = gps_pos_body - _params.imu_pos_body;
 		const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
 		const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 		_gps_sample_delayed.vel -= vel_offset_earth;

src/modules/ekf2/EKF2.cpp

@@ -76,9 +76,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 #if defined(CONFIG_EKF2_GNSS)
 	_param_ekf2_gps_ctrl(_params->gnss_ctrl),
 	_param_ekf2_gps_delay(_params->gps_delay_ms),
-	_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
-	_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
-	_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
 	_param_ekf2_gps_v_noise(_params->gps_vel_noise),
 	_param_ekf2_gps_p_noise(_params->gps_pos_noise),
 	_param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
@@ -2454,6 +2451,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			.nsats = vehicle_gps_position.satellites_used,
 			.pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop
 				      + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
+			.position_body = Vector3f{vehicle_gps_position.position_offset},
 		};
 		_ekf.setGpsData(gps_msg);
 

src/modules/ekf2/EKF2.hpp

@@ -545,10 +545,6 @@ private:
 		(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,
 		(ParamExtFloat<px4::params::EKF2_GPS_DELAY>) _param_ekf2_gps_delay,
 
-		(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x,
-		(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y,
-		(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z,
-
 		(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>) _param_ekf2_gps_v_noise,
 		(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>) _param_ekf2_gps_p_noise,
 

src/modules/ekf2/ekf2_params.c

@@ -941,33 +941,6 @@ PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Y, 0.0f);
  */
 PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Z, 0.0f);
 
-/**
- * X position of GPS antenna in body frame (forward axis with origin relative to vehicle centre of gravity)
- *
- * @group EKF2
- * @unit m
- * @decimal 3
- */
-PARAM_DEFINE_FLOAT(EKF2_GPS_POS_X, 0.0f);
-
-/**
- * Y position of GPS antenna in body frame (right axis with origin relative to vehicle centre of gravity)
- *
- * @group EKF2
- * @unit m
- * @decimal 3
- */
-PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Y, 0.0f);
-
-/**
- * Z position of GPS antenna in body frame (down axis with origin relative to vehicle centre of gravity)
- *
- * @group EKF2
- * @unit m
- * @decimal 3
- */
-PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Z, 0.0f);
-
 /**
  * X position of range finder origin in body frame (forward axis with origin relative to vehicle centre of gravity)
  *

src/modules/gyro_calibration/GyroCalibration.hpp

@@ -41,7 +41,7 @@
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/math/WelfordMeanVector.hpp>
 #include <lib/perf/perf_counter.h>
-#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <lib/sensor/calibration/Gyroscope.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
@@ -96,7 +96,7 @@ private:
 	uORB::SubscriptionMultiArray<sensor_accel_s, MAX_SENSORS> _sensor_accel_subs{ORB_ID::sensor_accel};
 	uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_SENSORS>  _sensor_gyro_subs{ORB_ID::sensor_gyro};
 
-	calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
+	sensor::calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
 	math::WelfordMeanVector<float, 3> _gyro_mean[MAX_SENSORS] {};
 	matrix::Vector3f _gyro_cal_variance[MAX_SENSORS] {};
 	float _temperature[MAX_SENSORS] {};

src/modules/mag_bias_estimator/MagBiasEstimator.hpp

@@ -37,7 +37,7 @@
 #include <lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
-#include <lib/sensor_calibration/Magnetometer.hpp>
+#include <lib/sensor/calibration/Magnetometer.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
@@ -93,7 +93,7 @@ private:
 
 	uORB::Publication<magnetometer_bias_estimate_s> _magnetometer_bias_estimate_pub{ORB_ID(magnetometer_bias_estimate)};
 
-	calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
+	sensor::calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
 
 	hrt_abstime _time_valid[MAX_SENSOR_COUNT] {};
 

src/modules/mavlink/streams/MAG_CAL_REPORT.hpp

@@ -34,7 +34,7 @@
 #ifndef MAG_CAL_REPORT_HPP
 #define MAG_CAL_REPORT_HPP
 
-#include <lib/sensor_calibration/Magnetometer.hpp>
+#include <lib/sensor/calibration/Magnetometer.hpp>
 
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_mag.h>
@@ -73,7 +73,7 @@ private:
 				sensor_mag_s sensor_mag;
 
 				if (_sensor_mag_subs[mag].update(&sensor_mag) && (sensor_mag.device_id != 0)) {
-					calibration::Magnetometer calibration{sensor_mag.device_id};
+					sensor::calibration::Magnetometer calibration{sensor_mag.device_id};
 
 					if (calibration.calibrated()) {
 						mavlink_mag_cal_report_t msg{};

src/modules/sensors/CMakeLists.txt

@@ -78,6 +78,7 @@ px4_add_module(
 		data_validator
 		mathlib
 		sensor_calibration
+		sensor_configuration
 		vehicle_imu
 	)
 

src/modules/sensors/module.yaml

@@ -14,81 +14,6 @@ parameters:
             category: System
             type: int32
             default: 0
-            decimal: 3
-            num_instances: *max_num_sensor_instances
-            instance_start: 0
-
-        CAL_ACC${i}_PRIO:
-            description:
-                short: Accelerometer ${i} priority
-            category: System
-            type: enum
-            values:
-                -1: Uninitialized
-                0: Disabled
-                1: Min
-                25: Low
-                50: Medium (Default)
-                75: High
-                100: Max
-            default: -1
-            decimal: 3
-            num_instances: *max_num_sensor_instances
-            instance_start: 0
-
-        CAL_ACC${i}_ROT:
-            description:
-                short: Accelerometer ${i} rotation relative to airframe
-                long: |
-                    An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
-            category: System
-            type: enum
-            values:
-                -1: Internal
-                0: No rotation
-                1: Yaw 45°
-                2: Yaw 90°
-                3: Yaw 135°
-                4: Yaw 180°
-                5: Yaw 225°
-                6: Yaw 270°
-                7: Yaw 315°
-                8: Roll 180°
-                9: Roll 180°, Yaw 45°
-                10: Roll 180°, Yaw 90°
-                11: Roll 180°, Yaw 135°
-                12: Pitch 180°
-                13: Roll 180°, Yaw 225°
-                14: Roll 180°, Yaw 270°
-                15: Roll 180°, Yaw 315°
-                16: Roll 90°
-                17: Roll 90°, Yaw 45°
-                18: Roll 90°, Yaw 90°
-                19: Roll 90°, Yaw 135°
-                20: Roll 270°
-                21: Roll 270°, Yaw 45°
-                22: Roll 270°, Yaw 90°
-                23: Roll 270°, Yaw 135°
-                24: Pitch 90°
-                25: Pitch 270°
-                26: Pitch 180°, Yaw 90°
-                27: Pitch 180°, Yaw 270°
-                28: Roll 90°, Pitch 90°
-                29: Roll 180°, Pitch 90°
-                30: Roll 270°, Pitch 90°
-                31: Roll 90°, Pitch 180°
-                32: Roll 270°, Pitch 180°
-                33: Roll 90°, Pitch 270°
-                34: Roll 180°, Pitch 270°
-                35: Roll 270°, Pitch 270°
-                36: Roll 90°, Pitch 180°, Yaw 90°
-                37: Roll 90°, Yaw 270°
-                38: Roll 90°, Pitch 68°, Yaw 293°
-                39: Pitch 315°
-                40: Roll 90°, Pitch 315°
-            min: -1
-            max: 40
-            default: -1
             num_instances: *max_num_sensor_instances
             instance_start: 0
 
@@ -167,6 +92,7 @@ parameters:
             num_instances: *max_num_sensor_instances
             instance_start: 0
 
+
         # Barometer calibration
         CAL_BARO${i}_ID:
             description:
@@ -178,23 +104,6 @@ parameters:
             num_instances: *max_num_sensor_instances
             instance_start: 0
 
-        CAL_BARO${i}_PRIO:
-            description:
-                short: Barometer ${i} priority
-            category: System
-            type: enum
-            values:
-                -1: Uninitialized
-                0: Disabled
-                1: Min
-                25: Low
-                50: Medium (Default)
-                75: High
-                100: Max
-            default: -1
-            num_instances: *max_num_sensor_instances
-            instance_start: 0
-
         CAL_BARO${i}_OFF:
             description:
                 short: Barometer ${i} offset
@@ -207,6 +116,7 @@ parameters:
             num_instances: *max_num_sensor_instances
             instance_start: 0
 
+
         # Gyroscope calibration
         CAL_GYRO${i}_ID:
             description:
@@ -218,79 +128,6 @@ parameters:
             num_instances: *max_num_sensor_instances
             instance_start: 0
 
-        CAL_GYRO${i}_PRIO:
-            description:
-                short: Gyroscope ${i} priority
-            category: System
-            type: enum
-            values:
-                -1: Uninitialized
-                0: Disabled
-                1: Min
-                25: Low
-                50: Medium (Default)
-                75: High
-                100: Max
-            default: -1
-            num_instances: *max_num_sensor_instances
-            instance_start: 0
-
-        CAL_GYRO${i}_ROT:
-            description:
-                short: Gyroscope ${i} rotation relative to airframe
-                long: |
-                    An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
-            category: System
-            type: enum
-            values:
-                -1: Internal
-                0: No rotation
-                1: Yaw 45°
-                2: Yaw 90°
-                3: Yaw 135°
-                4: Yaw 180°
-                5: Yaw 225°
-                6: Yaw 270°
-                7: Yaw 315°
-                8: Roll 180°
-                9: Roll 180°, Yaw 45°
-                10: Roll 180°, Yaw 90°
-                11: Roll 180°, Yaw 135°
-                12: Pitch 180°
-                13: Roll 180°, Yaw 225°
-                14: Roll 180°, Yaw 270°
-                15: Roll 180°, Yaw 315°
-                16: Roll 90°
-                17: Roll 90°, Yaw 45°
-                18: Roll 90°, Yaw 90°
-                19: Roll 90°, Yaw 135°
-                20: Roll 270°
-                21: Roll 270°, Yaw 45°
-                22: Roll 270°, Yaw 90°
-                23: Roll 270°, Yaw 135°
-                24: Pitch 90°
-                25: Pitch 270°
-                26: Pitch 180°, Yaw 90°
-                27: Pitch 180°, Yaw 270°
-                28: Roll 90°, Pitch 90°
-                29: Roll 180°, Pitch 90°
-                30: Roll 270°, Pitch 90°
-                31: Roll 90°, Pitch 180°
-                32: Roll 270°, Pitch 180°
-                33: Roll 90°, Pitch 270°
-                34: Roll 180°, Pitch 270°
-                35: Roll 270°, Pitch 270°
-                36: Roll 90°, Pitch 180°, Yaw 90°
-                37: Roll 90°, Yaw 270°
-                38: Roll 90°, Pitch 68°, Yaw 293°
-                39: Pitch 315°
-                40: Roll 90°, Pitch 315°
-            min: -1
-            max: 40
-            default: -1
-            num_instances: *max_num_sensor_instances
-            instance_start: 0
-
         CAL_GYRO${i}_XOFF:
             description:
                 short: Gyroscope ${i} X-axis offset
@@ -327,6 +164,7 @@ parameters:
             num_instances: *max_num_sensor_instances
             instance_start: 0
 
+
         # Magnetometer calibration
         CAL_MAG${i}_ID:
             description:
@@ -338,120 +176,6 @@ parameters:
             num_instances: *max_num_sensor_instances
             instance_start: 0
 
-        CAL_MAG${i}_PRIO:
-            description:
-                short: Magnetometer ${i} priority
-            category: System
-            type: enum
-            values:
-                -1: Uninitialized
-                0: Disabled
-                1: Min
-                25: Low
-                50: Medium (Default)
-                75: High
-                100: Max
-            default: -1
-            num_instances: *max_num_sensor_instances
-            instance_start: 0
-
-        CAL_MAG${i}_ROT:
-            description:
-                short: Magnetometer ${i} rotation relative to airframe
-                long: |
-                    An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
-                    Set to "Custom Euler Angle" to define the rotation using CAL_MAG${i}_ROLL, CAL_MAG${i}_PITCH and CAL_MAG${i}_YAW.
-            category: System
-            type: enum
-            values:
-                -1: Internal
-                0: No rotation
-                1: Yaw 45°
-                2: Yaw 90°
-                3: Yaw 135°
-                4: Yaw 180°
-                5: Yaw 225°
-                6: Yaw 270°
-                7: Yaw 315°
-                8: Roll 180°
-                9: Roll 180°, Yaw 45°
-                10: Roll 180°, Yaw 90°
-                11: Roll 180°, Yaw 135°
-                12: Pitch 180°
-                13: Roll 180°, Yaw 225°
-                14: Roll 180°, Yaw 270°
-                15: Roll 180°, Yaw 315°
-                16: Roll 90°
-                17: Roll 90°, Yaw 45°
-                18: Roll 90°, Yaw 90°
-                19: Roll 90°, Yaw 135°
-                20: Roll 270°
-                21: Roll 270°, Yaw 45°
-                22: Roll 270°, Yaw 90°
-                23: Roll 270°, Yaw 135°
-                24: Pitch 90°
-                25: Pitch 270°
-                26: Pitch 180°, Yaw 90°
-                27: Pitch 180°, Yaw 270°
-                28: Roll 90°, Pitch 90°
-                29: Roll 180°, Pitch 90°
-                30: Roll 270°, Pitch 90°
-                31: Roll 90°, Pitch 180°
-                32: Roll 270°, Pitch 180°
-                33: Roll 90°, Pitch 270°
-                34: Roll 180°, Pitch 270°
-                35: Roll 270°, Pitch 270°
-                36: Roll 90°, Pitch 180°, Yaw 90°
-                37: Roll 90°, Yaw 270°
-                38: Roll 90°, Pitch 68°, Yaw 293°
-                39: Pitch 315°
-                40: Roll 90°, Pitch 315°
-                100: Custom Euler Angle
-            min: -1
-            max: 100
-            default: -1
-            num_instances: *max_num_sensor_instances
-            instance_start: 0
-
-        CAL_MAG${i}_ROLL:
-            description:
-                short: Magnetometer ${i} Custom Euler Roll Angle
-                long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
-            category: System
-            type: float
-            default: 0.0
-            min: -180
-            max: 180
-            unit: deg
-            num_instances: *max_num_sensor_instances
-            instance_start: 0
-
-        CAL_MAG${i}_PITCH:
-            description:
-                short: Magnetometer ${i} Custom Euler Pitch Angle
-                long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
-            category: System
-            type: float
-            default: 0.0
-            min: -180
-            max: 180
-            unit: deg
-            num_instances: *max_num_sensor_instances
-            instance_start: 0
-
-        CAL_MAG${i}_YAW:
-            description:
-                short: Magnetometer ${i} Custom Euler Yaw Angle
-                long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
-            category: System
-            type: float
-            default: 0.0
-            min: -180
-            max: 180
-            unit: deg
-            num_instances: *max_num_sensor_instances
-            instance_start: 0
-
         CAL_MAG${i}_XOFF:
             description:
                 short: Magnetometer ${i} X-axis offset
@@ -609,3 +333,399 @@ parameters:
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
+
+
+    - group: Sensor Configuration
+      definitions:
+
+        # Accelerometer configuration
+        SENS_ACC${i}_ID:
+            description:
+                short: Accelerometer ${i} configuration device ID
+                long: Device ID of the accelerometer this configuration applies to.
+            category: System
+            type: int32
+            default: 0
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_ACC${i}_PRIO:
+            description:
+                short: Accelerometer ${i} priority
+            category: System
+            type: enum
+            values:
+                -1: Uninitialized
+                0: Disabled
+                1: Min
+                25: Low
+                50: Medium (Default)
+                75: High
+                100: Max
+            default: -1
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_ACC${i}_ROT:
+            description:
+                short: Accelerometer ${i} rotation relative to airframe
+                long: |
+                    An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
+            category: System
+            type: enum
+            values:
+                -1: Internal
+                0: No rotation
+                1: Yaw 45°
+                2: Yaw 90°
+                3: Yaw 135°
+                4: Yaw 180°
+                5: Yaw 225°
+                6: Yaw 270°
+                7: Yaw 315°
+                8: Roll 180°
+                9: Roll 180°, Yaw 45°
+                10: Roll 180°, Yaw 90°
+                11: Roll 180°, Yaw 135°
+                12: Pitch 180°
+                13: Roll 180°, Yaw 225°
+                14: Roll 180°, Yaw 270°
+                15: Roll 180°, Yaw 315°
+                16: Roll 90°
+                17: Roll 90°, Yaw 45°
+                18: Roll 90°, Yaw 90°
+                19: Roll 90°, Yaw 135°
+                20: Roll 270°
+                21: Roll 270°, Yaw 45°
+                22: Roll 270°, Yaw 90°
+                23: Roll 270°, Yaw 135°
+                24: Pitch 90°
+                25: Pitch 270°
+                26: Pitch 180°, Yaw 90°
+                27: Pitch 180°, Yaw 270°
+                28: Roll 90°, Pitch 90°
+                29: Roll 180°, Pitch 90°
+                30: Roll 270°, Pitch 90°
+                31: Roll 90°, Pitch 180°
+                32: Roll 270°, Pitch 180°
+                33: Roll 90°, Pitch 270°
+                34: Roll 180°, Pitch 270°
+                35: Roll 270°, Pitch 270°
+                36: Roll 90°, Pitch 180°, Yaw 90°
+                37: Roll 90°, Yaw 270°
+                38: Roll 90°, Pitch 68°, Yaw 293°
+                39: Pitch 315°
+                40: Roll 90°, Pitch 315°
+            min: -1
+            max: 40
+            default: -1
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+
+        # Barometer configuration
+        SENS_BARO${i}_ID:
+            description:
+                short: Barometer ${i} configuration device ID
+                long: Device ID of the barometer this configuration applies to.
+            category: System
+            type: int32
+            default: 0
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_BARO${i}_PRIO:
+            description:
+                short: Barometer ${i} priority
+            category: System
+            type: enum
+            values:
+                -1: Uninitialized
+                0: Disabled
+                1: Min
+                25: Low
+                50: Medium (Default)
+                75: High
+                100: Max
+            default: -1
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+
+        # GNSS configuration
+        SENS_GNSS${i}_ID:
+            description:
+                short: GNSS ${i} configuration device ID
+                long: Device ID of the GNSS this configuration applies to.
+            category: System
+            type: int32
+            default: 0
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_GNSS${i}_PRIO:
+            description:
+                short: GNSS ${i} priority
+            category: System
+            type: enum
+            values:
+                -1: Uninitialized
+                0: Disabled
+                1: Min
+                25: Low
+                50: Medium (Default)
+                75: High
+                100: Max
+            default: -1
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_GNSS${i}_XPOS:
+            description:
+                short: GNSS ${i} antenna X position
+                long: |
+                    X position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity)
+            category: System
+            type: float
+            default: 0.0
+            unit: m
+            volatile: true
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_GNSS${i}_YPOS:
+            description:
+                short: GNSS ${i} antenna Y position
+                long: |
+                    Y position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity)
+            category: System
+            type: float
+            default: 0.0
+            unit: m
+            volatile: true
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_GNSS${i}_ZPOS:
+            description:
+                short: GNSS ${i} antenna Z position
+                long: |
+                    Z position of GNSS antenna in body frame (forward axis with origin relative to vehicle centre of gravity)
+            category: System
+            type: float
+            default: 0.0
+            unit: m
+            volatile: true
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+
+        # Gyroscope configuration
+        SENS_GYRO${i}_ID:
+            description:
+                short: Gyroscope ${i} configuration device ID
+                long: Device ID of the gyroscope this configuration applies to.
+            category: System
+            type: int32
+            default: 0
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_GYRO${i}_PRIO:
+            description:
+                short: Gyroscope ${i} priority
+            category: System
+            type: enum
+            values:
+                -1: Uninitialized
+                0: Disabled
+                1: Min
+                25: Low
+                50: Medium (Default)
+                75: High
+                100: Max
+            default: -1
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_GYRO${i}_ROT:
+            description:
+                short: Gyroscope ${i} rotation relative to airframe
+                long: |
+                    An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
+            category: System
+            type: enum
+            values:
+                -1: Internal
+                0: No rotation
+                1: Yaw 45°
+                2: Yaw 90°
+                3: Yaw 135°
+                4: Yaw 180°
+                5: Yaw 225°
+                6: Yaw 270°
+                7: Yaw 315°
+                8: Roll 180°
+                9: Roll 180°, Yaw 45°
+                10: Roll 180°, Yaw 90°
+                11: Roll 180°, Yaw 135°
+                12: Pitch 180°
+                13: Roll 180°, Yaw 225°
+                14: Roll 180°, Yaw 270°
+                15: Roll 180°, Yaw 315°
+                16: Roll 90°
+                17: Roll 90°, Yaw 45°
+                18: Roll 90°, Yaw 90°
+                19: Roll 90°, Yaw 135°
+                20: Roll 270°
+                21: Roll 270°, Yaw 45°
+                22: Roll 270°, Yaw 90°
+                23: Roll 270°, Yaw 135°
+                24: Pitch 90°
+                25: Pitch 270°
+                26: Pitch 180°, Yaw 90°
+                27: Pitch 180°, Yaw 270°
+                28: Roll 90°, Pitch 90°
+                29: Roll 180°, Pitch 90°
+                30: Roll 270°, Pitch 90°
+                31: Roll 90°, Pitch 180°
+                32: Roll 270°, Pitch 180°
+                33: Roll 90°, Pitch 270°
+                34: Roll 180°, Pitch 270°
+                35: Roll 270°, Pitch 270°
+                36: Roll 90°, Pitch 180°, Yaw 90°
+                37: Roll 90°, Yaw 270°
+                38: Roll 90°, Pitch 68°, Yaw 293°
+                39: Pitch 315°
+                40: Roll 90°, Pitch 315°
+            min: -1
+            max: 40
+            default: -1
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+
+        # Magnetometer configuration
+        SENS_MAG${i}_ID:
+            description:
+                short: Magnetometer ${i} configuration device ID
+                long: Device ID of the magnetometer this configuration applies to.
+            category: System
+            type: int32
+            default: 0
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_MAG${i}_PRIO:
+            description:
+                short: Magnetometer ${i} priority
+            category: System
+            type: enum
+            values:
+                -1: Uninitialized
+                0: Disabled
+                1: Min
+                25: Low
+                50: Medium (Default)
+                75: High
+                100: Max
+            default: -1
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_MAG${i}_ROT:
+            description:
+                short: Magnetometer ${i} rotation relative to airframe
+                long: |
+                    An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
+                    Set to "Custom Euler Angle" to define the rotation using CAL_MAG${i}_ROLL, CAL_MAG${i}_PITCH and CAL_MAG${i}_YAW.
+            category: System
+            type: enum
+            values:
+                -1: Internal
+                0: No rotation
+                1: Yaw 45°
+                2: Yaw 90°
+                3: Yaw 135°
+                4: Yaw 180°
+                5: Yaw 225°
+                6: Yaw 270°
+                7: Yaw 315°
+                8: Roll 180°
+                9: Roll 180°, Yaw 45°
+                10: Roll 180°, Yaw 90°
+                11: Roll 180°, Yaw 135°
+                12: Pitch 180°
+                13: Roll 180°, Yaw 225°
+                14: Roll 180°, Yaw 270°
+                15: Roll 180°, Yaw 315°
+                16: Roll 90°
+                17: Roll 90°, Yaw 45°
+                18: Roll 90°, Yaw 90°
+                19: Roll 90°, Yaw 135°
+                20: Roll 270°
+                21: Roll 270°, Yaw 45°
+                22: Roll 270°, Yaw 90°
+                23: Roll 270°, Yaw 135°
+                24: Pitch 90°
+                25: Pitch 270°
+                26: Pitch 180°, Yaw 90°
+                27: Pitch 180°, Yaw 270°
+                28: Roll 90°, Pitch 90°
+                29: Roll 180°, Pitch 90°
+                30: Roll 270°, Pitch 90°
+                31: Roll 90°, Pitch 180°
+                32: Roll 270°, Pitch 180°
+                33: Roll 90°, Pitch 270°
+                34: Roll 180°, Pitch 270°
+                35: Roll 270°, Pitch 270°
+                36: Roll 90°, Pitch 180°, Yaw 90°
+                37: Roll 90°, Yaw 270°
+                38: Roll 90°, Pitch 68°, Yaw 293°
+                39: Pitch 315°
+                40: Roll 90°, Pitch 315°
+                100: Custom Euler Angle
+            min: -1
+            max: 100
+            default: -1
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_MAG${i}_ROLL:
+            description:
+                short: Magnetometer ${i} Custom Euler Roll Angle
+                long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
+            category: System
+            type: float
+            default: 0.0
+            min: -180
+            max: 180
+            unit: deg
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_MAG${i}_PITCH:
+            description:
+                short: Magnetometer ${i} Custom Euler Pitch Angle
+                long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
+            category: System
+            type: float
+            default: 0.0
+            min: -180
+            max: 180
+            unit: deg
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        SENS_MAG${i}_YAW:
+            description:
+                short: Magnetometer ${i} Custom Euler Yaw Angle
+                long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
+            category: System
+            type: float
+            default: 0.0
+            min: -180
+            max: 180
+            unit: deg
+            num_instances: *max_num_sensor_instances
+            instance_start: 0

src/modules/sensors/sensors.cpp

@@ -43,6 +43,8 @@
 
 #include "sensors.hpp"
 
+#include <lib/sensor/configuration/Utilities.hpp>
+
 Sensors::Sensors(bool hil_enabled) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
@@ -184,58 +186,96 @@ int Sensors::parameters_update()
 	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
 		// sensor_accel
 		{
-			const uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC",  "ID", i);
+			const uint32_t device_id_accel = sensor::calibration::GetCalibrationParamInt32("ACC", "ID", i);
 
 			if (device_id_accel != 0) {
-				calibration::Accelerometer accel_cal(device_id_accel);
+				sensor::calibration::Accelerometer accel_cal(device_id_accel);
 			}
 
 			uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
 
 			if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) {
-				calibration::Accelerometer cal;
+				sensor::calibration::Accelerometer cal;
 				cal.set_calibration_index(i);
 				cal.ParametersLoad();
 			}
+
+			uint32_t device_id_acc_cfg = sensor::configuration::GetConfigurationParamInt32("ACC", "ID", i);
+
+			if (device_id_acc_cfg != 0) {
+				sensor::configuration::Accelerometer mag_cal(device_id_acc_cfg);
+			}
 		}
 
 
 		// sensor_gyro
 		{
-			const uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i);
+			const uint32_t device_id_gyro = sensor::calibration::GetCalibrationParamInt32("GYRO", "ID", i);
 
 			if (device_id_gyro != 0) {
-				calibration::Gyroscope gyro_cal(device_id_gyro);
+				sensor::calibration::Gyroscope gyro_cal(device_id_gyro);
 			}
 
 			uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
 
 			if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) {
-				calibration::Gyroscope cal;
+				sensor::calibration::Gyroscope cal;
 				cal.set_calibration_index(i);
 				cal.ParametersLoad();
 			}
+
+			uint32_t device_id_gyro_cfg = sensor::configuration::GetConfigurationParamInt32("GYRO", "ID", i);
+
+			if (device_id_gyro_cfg != 0) {
+				sensor::configuration::Accelerometer mag_cal(device_id_gyro_cfg);
+			}
 		}
 
 
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 		// sensor_mag
 		{
-			uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG",  "ID", i);
+			uint32_t device_id_mag_cal = sensor::calibration::GetCalibrationParamInt32("MAG", "ID", i);
 
-			if (device_id_mag != 0) {
+			if (device_id_mag_cal != 0) {
-				calibration::Magnetometer mag_cal(device_id_mag);
+				sensor::calibration::Magnetometer mag_cal(device_id_mag_cal);
 			}
 
 			uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
 
 			if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) {
-				calibration::Magnetometer cal;
+				sensor::calibration::Magnetometer cal;
 				cal.set_calibration_index(i);
 				cal.ParametersLoad();
 			}
+
+			uint32_t device_id_mag_cfg = sensor::configuration::GetConfigurationParamInt32("MAG", "ID", i);
+
+			if (device_id_mag_cfg != 0) {
+				sensor::configuration::Magnetometer mag_cal(device_id_mag_cfg);
+			}
 		}
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
+
+
+#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
+		// sensor_gps
+		{
+			uint32_t device_id_gnss = sensor::configuration::GetConfigurationParamInt32("GNSS", "ID", i);
+
+			if (device_id_gnss != 0) {
+				sensor::configuration::GNSS gnss_cal(device_id_gnss);
+			}
+
+			uORB::SubscriptionData<sensor_gps_s> sensor_gps_sub{ORB_ID(sensor_gps), i};
+
+			if (sensor_gps_sub.advertised() && (sensor_gps_sub.get().device_id != 0)) {
+				sensor::configuration::GNSS conf;
+				conf.set_configuration_index(i);
+				conf.ParametersLoad();
+			}
+		}
+#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
 	}
 
 #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
@@ -713,6 +753,15 @@ int Sensors::print_status()
 	_airspeed_validator.print();
 #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 
+#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
+
+	if (_vehicle_gps_position) {
+		PX4_INFO_RAW("\n");
+		_vehicle_gps_position->PrintStatus();
+	}
+
+#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
+
 #if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
 
 	if (_vehicle_optical_flow) {
@@ -732,15 +781,6 @@ int Sensors::print_status()
 	_vehicle_angular_velocity.PrintStatus();
 #endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY
 
-#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
-
-	if (_vehicle_gps_position) {
-		PX4_INFO_RAW("\n");
-		_vehicle_gps_position->PrintStatus();
-	}
-
-#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
-
 	PX4_INFO_RAW("\n");
 
 	for (auto &i : _vehicle_imu_list) {

src/modules/sensors/sensors.hpp

@@ -41,7 +41,7 @@
  * @author Beat Küng <beat-kueng@gmx.net>
  */
 
-#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/sensor/calibration/Utilities.hpp>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
@@ -80,7 +80,7 @@
 
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 # include "vehicle_magnetometer/VehicleMagnetometer.hpp"
-# include <lib/sensor_calibration/Magnetometer.hpp>
+# include <lib/sensor/calibration/Magnetometer.hpp>
 # include <uORB/topics/sensor_mag.h>
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 
@@ -263,4 +263,4 @@ private:
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)
-};
+};

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include <lib/sensor_calibration/Accelerometer.hpp>
+#include <lib/sensor/calibration/Accelerometer.hpp>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
@@ -87,7 +87,7 @@ private:
 	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
 	uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_accel)};
 
-	calibration::Accelerometer _calibration{};
+	sensor::calibration::Accelerometer _calibration{};
 
 	matrix::Vector3f _bias{};
 

src/modules/sensors/vehicle_air_data/VehicleAirData.hpp

@@ -35,7 +35,7 @@
 
 #include "data_validator/DataValidatorGroup.hpp"
 
-#include <lib/sensor_calibration/Barometer.hpp>
+#include <lib/sensor/calibration/Barometer.hpp>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
@@ -96,7 +96,7 @@ private:
 		{this, ORB_ID(sensor_baro), 3},
 	};
 
-	calibration::Barometer _calibration[MAX_SENSOR_COUNT];
+	sensor::calibration::Barometer _calibration[MAX_SENSOR_COUNT];
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -34,7 +34,7 @@
 #pragma once
 
 #include <containers/Bitset.hpp>
-#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <lib/sensor/calibration/Gyroscope.hpp>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
@@ -113,7 +113,7 @@ private:
 	uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)};
 	uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)};
 
-	calibration::Gyroscope _calibration{};
+	sensor::calibration::Gyroscope _calibration{};
 
 	matrix::Vector3f _bias{};
 

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

@@ -91,11 +91,29 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
 			}
 		}
 
+		for (int instance = 0; instance < GPS_MAX_RECEIVERS; instance++) {
+			_configuration[instance].ParametersUpdate();
+		}
+
 		_gps_blending.setBlendingUseSpeedAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC);
 		_gps_blending.setBlendingUseHPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC);
 		_gps_blending.setBlendingUseVPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC);
 		_gps_blending.setBlendingTimeConstant(_param_sens_gps_tau.get());
-		_gps_blending.setPrimaryInstance(_param_sens_gps_prime.get());
+
+		// TODO: select highest priority
+		int primary_instance = -1;
+		uint8_t highest_priority = 0;
+
+		for (int instance = 0; instance < GPS_MAX_RECEIVERS; instance++) {
+			if (_configuration[instance].enabled() && _configuration[instance].priority() > highest_priority) {
+				primary_instance = instance;
+				highest_priority = _configuration[instance].priority();
+			}
+		}
+
+		if (primary_instance >= 0) {
+			//_gps_blending.setPrimaryInstance(primary_instance);
+		}
 	}
 }
 
@@ -116,11 +134,14 @@ void VehicleGPSPosition::Run()
 		if (gps_updated) {
 			any_gps_updated = true;
 
-			_sensor_gps_sub[i].copy(&gps_data);
+			if (_sensor_gps_sub[i].copy(&gps_data)) {
-			_gps_blending.setGpsData(gps_data, i);
+				_configuration[i].set_device_id(gps_data.device_id);
 
-			if (!_sensor_gps_sub[i].registered()) {
+				_gps_blending.setGpsData(gps_data, i);
-				_sensor_gps_sub[i].registerCallback();
+
+				if (!_sensor_gps_sub[i].registered()) {
+					_sensor_gps_sub[i].registerCallback();
+				}
 			}
 		}
 	}
@@ -131,12 +152,29 @@ void VehicleGPSPosition::Run()
 		if (_gps_blending.isNewOutputDataAvailable()) {
 			sensor_gps_s gps_output{_gps_blending.getOutputGpsData()};
 
+			const int selected_instance = _gps_blending.getSelectedGps();
+
 			// clear device_id if blending
-			if (_gps_blending.getSelectedGps() == GpsBlending::GPS_MAX_RECEIVERS_BLEND) {
+			if (selected_instance == GpsBlending::GPS_MAX_RECEIVERS_BLEND) {
 				gps_output.device_id = 0;
+
+				gps_output.position_offset[0] = 0.f;
+				gps_output.position_offset[1] = 0.f;
+				gps_output.position_offset[2] = 0.f;
+
+			} else if (selected_instance >= 0 && selected_instance < GpsBlending::GPS_MAX_RECEIVERS_BLEND) {
+
+				_configuration[selected_instance].position().copyTo(gps_output.position_offset);
 			}
 
 			_vehicle_gps_position_pub.publish(gps_output);
+
+			// populate initial SENS_GNSSx configuration slots if necessary
+			for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+				if ((_configuration[i].device_id() != 0) && (!_configuration[i].configured())) {
+					_configuration[i].ParametersSave(i);
+				}
+			}
 		}
 	}
 
@@ -148,6 +186,12 @@ void VehicleGPSPosition::Run()
 void VehicleGPSPosition::PrintStatus()
 {
 	PX4_INFO_RAW("[vehicle_gps_position] selected GPS: %d\n", _gps_blending.getSelectedGps());
+
+	for (int i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		if (_configuration[i].device_id() != 0) {
+			_configuration[i].PrintStatus();
+		}
+	}
 }
 
 }; // namespace sensors

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp

@@ -36,6 +36,7 @@
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
+#include <lib/sensor/configuration/GNSS.hpp>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
@@ -92,10 +93,11 @@ private:
 
 	GpsBlending _gps_blending;
 
+	sensor::configuration::GNSS _configuration[GPS_MAX_RECEIVERS] {};
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
-		(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau,
+		(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau
-		(ParamInt<px4::params::SENS_GPS_PRIME>) _param_sens_gps_prime
 	)
 };
 }; // namespace sensors

src/modules/sensors/vehicle_gps_position/gps_blending.cpp

@@ -54,6 +54,7 @@ void GpsBlending::update(uint64_t hrt_now_us)
 		for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
 			if (_gps_state[i].fix_type > best_fix) {
 				best_fix = _gps_state[i].fix_type;
+				gps_select_index = i;
 			}
 		}
 
@@ -73,15 +74,8 @@ void GpsBlending::update(uint64_t hrt_now_us)
 			_hgt_offset_m[i] = 0.0;
 		}
 
-		// Only use a secondary instance if the fallback is allowed
-		if ((_primary_instance > -1)
-		    && (gps_select_index != _primary_instance)
-		    && !_fallback_allowed) {
-			gps_select_index = _primary_instance;
-		}
-
 		_selected_gps = gps_select_index;
-		_is_new_output_data_available =  _gps_updated[gps_select_index];
+		_is_new_output_data_available = _gps_updated[gps_select_index];
 
 		for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
 			_gps_updated[gps_select_index] = false;
@@ -109,7 +103,7 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)
 		float raw_dt = 0.f;
 
 		if (_gps_state[i].timestamp > _time_prev_us[i]) {
-			raw_dt = 1e-6f * (_gps_state[i].timestamp - _time_prev_us[i]);
+			raw_dt = 1e-6f * ((int64_t)_gps_state[i].timestamp - (int64_t)_time_prev_us[i]);
 		}
 
 		float present_dt = 0.f;
@@ -372,6 +366,14 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
 	gps_blended_state.vel_e_m_s = 0;
 	gps_blended_state.vel_d_m_s = 0;
 
+	// these will be a sum of weighted values and must start from zero.
+	gps_blended_state.timestamp = 0;
+	gps_blended_state.timestamp_sample = 0;
+	gps_blended_state.vel_m_s = 0;
+	gps_blended_state.vel_n_m_s = 0;
+	gps_blended_state.vel_e_m_s = 0;
+	gps_blended_state.vel_d_m_s = 0;
+
 	// combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights()
 	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
 		// Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers
@@ -429,11 +431,6 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
 				gps_blended_state.vel_ned_valid = true;
 			}
 		}
-
-		// TODO read parameters for individual GPS antenna positions and blend
-		// Vector3f temp_antenna_offset = _antenna_offset[i];
-		// temp_antenna_offset *= blend_weights[i];
-		// _blended_antenna_offset += temp_antenna_offset;
 	}
 
 	/*
@@ -500,14 +497,15 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state)
 {
 	// Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
 	// A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering
+	// A weighting of 0 will make the offset adjust the slowest, a weighting of 1 will make it adjust with zero filtering
 	float alpha[GPS_MAX_RECEIVERS_BLEND] {};
 	float omega_lpf = 1.0f / fmaxf(_blending_time_constant, 1.0f);
 
 	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
 		if (_gps_state[i].timestamp > _time_prev_us[i]) {
 			// calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter
-			alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]),
+			alpha[i] = constrain(omega_lpf * 1e-6f * (float)((int64_t)_gps_state[i].timestamp - (int64_t)_time_prev_us[i]), 0.f,
-					     0.0f, 1.0f);
+					     1.f);
 
 			_time_prev_us[i] = _gps_state[i].timestamp;
 		}

src/modules/sensors/vehicle_gps_position/gps_blending.hpp

@@ -72,6 +72,7 @@ public:
 			_gps_updated[instance] = true;
 		}
 	}
+
 	void setBlendingUseSpeedAccuracy(bool enabled) { _blend_use_spd_acc = enabled; }
 	void setBlendingUseHPosAccuracy(bool enabled) { _blend_use_hpos_acc = enabled; }
 	void setBlendingUseVPosAccuracy(bool enabled) { _blend_use_vpos_acc = enabled; }
@@ -82,6 +83,7 @@ public:
 
 	bool isNewOutputDataAvailable() const { return _is_new_output_data_available; }
 	int getNumberOfGpsSuitableForBlending() const { return _np_gps_suitable_for_blending; }
+
 	const sensor_gps_s &getOutputGpsData() const
 	{
 		if (_selected_gps < GPS_MAX_RECEIVERS_BLEND) {
@@ -91,6 +93,7 @@ public:
 			return _gps_blended_state;
 		}
 	}
+
 	int getSelectedGps() const { return _selected_gps; }
 
 private:
@@ -122,11 +125,12 @@ private:
 
 	sensor_gps_s _gps_state[GPS_MAX_RECEIVERS_BLEND] {}; ///< internal state data for the physical GPS
 	sensor_gps_s _gps_blended_state {};
+
 	bool _gps_updated[GPS_MAX_RECEIVERS_BLEND] {};
 	int _selected_gps{0};
 	int _np_gps_suitable_for_blending{0};
-	int _primary_instance{0}; ///< if -1, there is no primary isntance and the best receiver is used // TODO: use device_id
+	int _primary_instance{0}; ///< if -1, there is no primary instance and the best receiver is used // TODO: use device_id
-	bool _fallback_allowed{false};
+	bool _fallback_allowed{true};
 
 	bool _is_new_output_data_available{false};
 

src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp

@@ -204,9 +204,9 @@ TEST_F(GpsBlendingTest, dualReceiverBlendingHPos)
 	gps_blending.setGpsData(gps_data1, 1);
 	gps_blending.update(_time_now_us);
 
-	// THEN: the blended instance should be selected (2)
+	// THEN: the blended instance should be selected (3)
 	// and the eph should be adjusted
-	EXPECT_EQ(gps_blending.getSelectedGps(), 2);
+	EXPECT_EQ(gps_blending.getSelectedGps(), 3);
 	EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2);
 	EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
 	EXPECT_LT(gps_blending.getOutputGpsData().eph, gps_data0.eph);

src/modules/sensors/vehicle_gps_position/params.c

@@ -46,7 +46,7 @@
  * @bit 1 use hpos accuracy
  * @bit 2 use vpos accuracy
  */
-PARAM_DEFINE_INT32(SENS_GPS_MASK, 7);
+PARAM_DEFINE_INT32(SENS_GPS_MASK, 0);
 
 /**
  * Multi GPS Blending Time Constant
@@ -61,22 +61,3 @@ PARAM_DEFINE_INT32(SENS_GPS_MASK, 7);
  * @decimal 1
  */
 PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f);
-
-/**
- * Multi GPS primary instance
- *
- * When no blending is active, this defines the preferred GPS receiver instance.
- * The GPS selection logic waits until the primary receiver is available to
- * send data to the EKF even if a secondary instance is already available.
- * The secondary instance is then only used if the primary one times out.
- *
- * To have an equal priority of all the instances, set this parameter to -1 and
- * the best receiver will be used.
- *
- * This parameter has no effect if blending is active.
- *
- * @group Sensors
- * @min -1
- * @max 1
- */
-PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0);

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -35,7 +35,7 @@
 
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/events.h>
-#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/sensor/Utilities.hpp>
 #include <lib/systemlib/mavlink_log.h>
 
 #include <float.h>

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -40,8 +40,8 @@
 #include <lib/mathlib/math/WelfordMeanVector.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
-#include <lib/sensor_calibration/Accelerometer.hpp>
+#include <lib/sensor/calibration/Accelerometer.hpp>
-#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <lib/sensor/calibration/Gyroscope.hpp>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
@@ -111,8 +111,8 @@ private:
 
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 
-	calibration::Accelerometer _accel_calibration{};
+	sensor::calibration::Accelerometer _accel_calibration{};
-	calibration::Gyroscope _gyro_calibration{};
+	sensor::calibration::Gyroscope _gyro_calibration{};
 
 	sensors::Integrator       _accel_integrator{};
 	sensors::IntegratorConing _gyro_integrator{};

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -36,7 +36,7 @@
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/events.h>
 #include <lib/geo/geo.h>
-#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/sensor/calibration/Utilities.hpp>
 
 namespace sensors
 {
@@ -63,7 +63,7 @@ VehicleMagnetometer::VehicleMagnetometer() :
 	// if publishing multiple mags advertise instances immediately for existing calibrations
 	if (!_param_sens_mag_mode.get()) {
 		for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-			uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i);
+			uint32_t device_id_mag = sensor::calibration::GetCalibrationParamInt32("MAG", "ID", i);
 
 			if (device_id_mag != 0) {
 				_vehicle_magnetometer_pub[i].advertise();

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp

@@ -35,7 +35,7 @@
 
 #include "data_validator/DataValidatorGroup.hpp"
 
-#include <lib/sensor_calibration/Magnetometer.hpp>
+#include <lib/sensor/calibration/Magnetometer.hpp>
 #include <lib/conversion/rotation.h>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
@@ -137,7 +137,7 @@ private:
 
 	matrix::Vector3f _calibration_estimator_bias[MAX_SENSOR_COUNT] {};
 
-	calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
+	sensor::calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
 
 	// Magnetometer interference compensation
 	enum class MagCompensationType {

src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp

@@ -41,7 +41,7 @@
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
-#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <lib/sensor/calibration/Gyroscope.hpp>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
@@ -102,7 +102,7 @@ private:
 
 	hrt_abstime _gyro_timestamp_sample_last{0};
 
-	calibration::Gyroscope _gyro_calibration{};
+	sensor::calibration::Gyroscope _gyro_calibration{};
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 

src/modules/sensors/voted_sensors_update.cpp

@@ -39,7 +39,7 @@
 
 #include "voted_sensors_update.h"
 
-#include <lib/sensor_calibration/Utilities.hpp>
+#include <lib/sensor/configuration/Utilities.hpp>
 #include <lib/geo/geo.h>
 #include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
@@ -88,13 +88,14 @@ void VotedSensorsUpdate::parametersUpdate()
 		    && (imu.get().accel_device_id != 0) && (imu.get().gyro_device_id != 0)) {
 
 			// find corresponding configured accel priority
-			int8_t accel_cal_index = calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id);
+			int8_t accel_conf_index = sensor::configuration::FindCurrentConfigurationIndex("ACC", imu.get().accel_device_id);
 
-			if (accel_cal_index >= 0) {
+			if (accel_conf_index >= 0) {
 				// found matching CAL_ACCx_PRIO
 				int32_t accel_priority_old = _accel.priority_configured[uorb_index];
 
-				_accel.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("ACC", "PRIO", accel_cal_index);
+				_accel.priority_configured[uorb_index] = sensor::configuration::GetConfigurationParamInt32("ACC", "PRIO",
+						accel_conf_index);
 
 				if (accel_priority_old != _accel.priority_configured[uorb_index]) {
 					if (_accel.priority_configured[uorb_index] == 0) {
@@ -111,13 +112,14 @@ void VotedSensorsUpdate::parametersUpdate()
 			}
 
 			// find corresponding configured gyro priority
-			int8_t gyro_cal_index = calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id);
+			int8_t gyro_conf_index = sensor::configuration::FindCurrentConfigurationIndex("GYRO", imu.get().gyro_device_id);
 
-			if (gyro_cal_index >= 0) {
+			if (gyro_conf_index >= 0) {
 				// found matching CAL_GYROx_PRIO
 				int32_t gyro_priority_old = _gyro.priority_configured[uorb_index];
 
-				_gyro.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("GYRO", "PRIO", gyro_cal_index);
+				_gyro.priority_configured[uorb_index] = sensor::configuration::GetConfigurationParamInt32("GYRO", "PRIO",
+									gyro_conf_index);
 
 				if (gyro_priority_old != _gyro.priority_configured[uorb_index]) {
 					if (_gyro.priority_configured[uorb_index] == 0) {