yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
- apply SENS_BOARD_{X,Y,Z}_OFF to external sensors to prevent unnecessary misalignment with internal IMU
This commit is contained in:
Daniel Agar
2020-11-12 10:37:30 -05:00
committed by GitHub
parent 5f0c572c61
commit d14b4221f1
7 changed files with 58 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/lib/sensor_calibration/Accelerometer.cpp
View File

@@ -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

11
src/lib/sensor_calibration/Gyroscope.cpp
View File

@@ -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

11
src/lib/sensor_calibration/Magnetometer.cpp
View File

@@ -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

24
src/lib/sensor_calibration/Utilities.cpp
View File

@@ -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

19
src/lib/sensor_calibration/Utilities.hpp
View File

@@ -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

4
src/modules/commander/accelerometer_calibration.cpp
View File

@@ -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;

2
src/modules/commander/mag_calibration.cpp
View File

@@ -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++) {