sensor calibration: apply board level adjustment to external sensors (#16127)

- apply SENS_BOARD_{X,Y,Z}_OFF to external sensors to prevent unnecessary misalignment with internal IMU

src/lib/sensor_calibration/Accelerometer.cpp

@@ -126,7 +126,9 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
 void Accelerometer::set_rotation(Rotation rotation)
 {
 	_rotation_enum = rotation;
-	_rotation = get_rot_matrix(rotation);
+
+	// always apply board level adjustments
+	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
 }
 
 void Accelerometer::ParametersUpdate()
@@ -151,8 +153,7 @@ void Accelerometer::ParametersUpdate()
 				SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
 			}
 
-			_rotation_enum = static_cast<Rotation>(rotation_value);
-			_rotation = get_rot_matrix(_rotation_enum);
+			set_rotation(static_cast<Rotation>(rotation_value));
 
 		} else {
 			// internal, CAL_ACCx_ROT -1
@@ -162,8 +163,8 @@ void Accelerometer::ParametersUpdate()
 				SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
 			}
 
-			_rotation = GetBoardRotation();
-			_rotation_enum = ROTATION_NONE;
+			// internal sensors follow board rotation
+			set_rotation(GetBoardRotation());
 		}
 
 		// CAL_ACCx_PRIO

src/lib/sensor_calibration/Gyroscope.cpp

@@ -126,7 +126,9 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
 void Gyroscope::set_rotation(Rotation rotation)
 {
 	_rotation_enum = rotation;
-	_rotation = get_rot_matrix(rotation);
+
+	// always apply board level adjustments
+	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
 }
 
 void Gyroscope::ParametersUpdate()
@@ -151,8 +153,7 @@ void Gyroscope::ParametersUpdate()
 				SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
 			}
 
-			_rotation_enum = static_cast<Rotation>(rotation_value);
-			_rotation = get_rot_matrix(_rotation_enum);
+			set_rotation(static_cast<Rotation>(rotation_value));
 
 		} else {
 			// internal, CAL_GYROx_ROT -1
@@ -162,8 +163,8 @@ void Gyroscope::ParametersUpdate()
 				SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
 			}
 
-			_rotation = GetBoardRotation();
-			_rotation_enum = ROTATION_NONE;
+			// internal sensors follow board rotation
+			set_rotation(GetBoardRotation());
 		}
 
 		// CAL_GYROx_PRIO

src/lib/sensor_calibration/Magnetometer.cpp

@@ -105,7 +105,9 @@ void Magnetometer::set_offdiagonal(const Vector3f &offdiagonal)
 void Magnetometer::set_rotation(Rotation rotation)
 {
 	_rotation_enum = rotation;
-	_rotation = get_rot_matrix(rotation);
+
+	// always apply board level adjustments
+	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
 }
 
 void Magnetometer::ParametersUpdate()
@@ -130,8 +132,7 @@ void Magnetometer::ParametersUpdate()
 				SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
 			}
 
-			_rotation_enum = static_cast<Rotation>(rotation_value);
-			_rotation = get_rot_matrix(_rotation_enum);
+			set_rotation(static_cast<Rotation>(rotation_value));
 
 		} else {
 			// internal mag, CAL_MAGx_ROT -1
@@ -141,8 +142,8 @@ void Magnetometer::ParametersUpdate()
 				SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
 			}
 
-			_rotation = GetBoardRotation();
-			_rotation_enum = ROTATION_NONE;
+			// internal sensors follow board rotation
+			set_rotation(GetBoardRotation());
 		}
 
 		// CAL_MAGx_PRIO

src/lib/sensor_calibration/Utilities.cpp

@@ -34,6 +34,7 @@
 #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 math::radians;
@@ -146,7 +147,7 @@ bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type,
 	return ret == PX4_OK;
 }
 
-Dcmf GetBoardRotation()
+Eulerf GetSensorLevelAdjustment()
 {
 	float x_offset = 0.f;
 	float y_offset = 0.f;
@@ -155,13 +156,28 @@ Dcmf GetBoardRotation()
 	param_get(param_find("SENS_BOARD_Y_OFF"), &y_offset);
 	param_get(param_find("SENS_BOARD_Z_OFF"), &z_offset);
 
-	const Dcmf board_rotation_offset(Eulerf(radians(x_offset), radians(y_offset), radians(z_offset)));
+	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 = 0;
+	int32_t board_rot = -1;
 	param_get(param_find("SENS_BOARD_ROT"), &board_rot);
 
-	return board_rotation_offset * get_rot_matrix((enum Rotation)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: %d", board_rot);
+	}
+
+	return Rotation::ROTATION_NONE;
+}
+
+Dcmf GetBoardRotationMatrix()
+{
+	return get_rot_matrix(GetBoardRotation());
 }
 
 } // namespace calibration

src/lib/sensor_calibration/Utilities.hpp

@@ -33,6 +33,7 @@
 
 #pragma once
 
+#include <lib/conversion/rotation.h>
 #include <matrix/math.hpp>
 
 namespace calibration
@@ -91,10 +92,24 @@ bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type,
 				  matrix::Vector3f values);
 
 /**
- * @brief Get the overall board rotation, including level adjustment.
+ * @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 GetBoardRotation();
+matrix::Dcmf GetBoardRotationMatrix();
 
 } // namespace calibration

src/modules/commander/accelerometer_calibration.cpp

@@ -228,7 +228,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::GetBoardRotation();
+	const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
 
 	for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
 		accel_sum[s] = board_rotation * accel_sum[s];
@@ -362,7 +362,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::GetBoardRotation();
+		const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
 		const Dcmf board_rotation_t = board_rotation.transpose();
 
 		bool param_save = false;

src/modules/commander/mag_calibration.cpp

@@ -680,7 +680,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::GetBoardRotation();
+				const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
 
 				// apply new calibrations to all raw sensor data before comparison
 				for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {