diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index 15b9c5c70d..b836a14532 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/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_value); - _rotation = get_rot_matrix(_rotation_enum); + set_rotation(static_cast(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 diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index acf4fdb6c2..fbb3725011 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/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_value); - _rotation = get_rot_matrix(_rotation_enum); + set_rotation(static_cast(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 diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index fe0814476c..02d0e2bfb7 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/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_value); - _rotation = get_rot_matrix(_rotation_enum); + set_rotation(static_cast(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 diff --git a/src/lib/sensor_calibration/Utilities.cpp b/src/lib/sensor_calibration/Utilities.cpp index 67384a7e67..28a77dae38 100644 --- a/src/lib/sensor_calibration/Utilities.cpp +++ b/src/lib/sensor_calibration/Utilities.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include 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(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 diff --git a/src/lib/sensor_calibration/Utilities.hpp b/src/lib/sensor_calibration/Utilities.hpp index ff53c9b898..a2a8792a5d 100644 --- a/src/lib/sensor_calibration/Utilities.hpp +++ b/src/lib/sensor_calibration/Utilities.hpp @@ -33,6 +33,7 @@ #pragma once +#include #include 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 diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 8a64825586..c877398d07 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/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; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 4c5ea622d4..59ba29f8b0 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/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++) {