fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

save learned mag bias per sensor (Multi-EKF support)

Browse Source 
- handle saving the mag bias per sensor (across all estimator instances using that mag) in sensors/vehicle_magnetometer
 - this is now saving back to the actual mag calibration CAL_MAGn_OFF{X,Y,Z}
 - ekf2 reset mag mag bias on any magnetometer or calibration change
 - use Kalman filter scheme to update stored mag bias parameters using all available bias estimates for that sensor

Co-authored-by: Paul Riseborough <gncsolns@gmail.com>
This commit is contained in:
Daniel Agar
2021-01-07 09:54:13 -05:00
committed by GitHub
co-authored by Paul Riseborough
parent 537ee5b19b
commit 88f8da27ef
17 changed files with 328 additions and 308 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/sensor_calibration/Accelerometer.cpp
+26 -18
View File
@@ -123,6 +123,30 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
}
}
bool Accelerometer::set_offset(const Vector3f &offset)
{
if (Vector3f(_offset - offset).longerThan(0.001f)) {
_offset = offset;
_calibration_count++;
return true;
}
return false;
}
bool Accelerometer::set_scale(const Vector3f &scale)
{
if (Vector3f(_scale - scale).longerThan(0.001f)) {
_scale = scale;
_calibration_count++;
return true;
}
return false;
}
void Accelerometer::set_rotation(Rotation rotation)
{
_rotation_enum = rotation;
@@ -183,27 +207,11 @@ void Accelerometer::ParametersUpdate()
_priority = new_priority;
}
bool calibration_changed = false;
// CAL_ACCx_OFF{X,Y,Z}
const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
_offset = offset;
}
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
// CAL_ACCx_SCALE{X,Y,Z}
const Vector3f scale = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index);
if (Vector3f(_scale - scale).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
_scale = scale;
}
if (calibration_changed) {
_calibration_count++;
}
set_scale(GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index));
} else {
Reset();
src/lib/sensor_calibration/Accelerometer.hpp
+4 -2
View File
@@ -62,17 +62,19 @@ public:
void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
void set_device_id(uint32_t device_id, bool external = false);
void set_external(bool external = true);
void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
void set_scale(const matrix::Vector3f &scale) { _scale = scale; }
bool set_offset(const matrix::Vector3f &offset);
bool set_scale(const matrix::Vector3f &scale);
void set_rotation(Rotation rotation);
uint8_t calibration_count() const { return _calibration_count; }
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; }
// apply offsets and scale
// rotate corrected measurements from sensor to body frame
src/lib/sensor_calibration/Gyroscope.cpp
+13 -12
View File
@@ -123,6 +123,18 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
}
}
bool Gyroscope::set_offset(const Vector3f &offset)
{
if (Vector3f(_offset - offset).longerThan(0.001f)) {
_offset = offset;
_calibration_count++;
return true;
}
return false;
}
void Gyroscope::set_rotation(Rotation rotation)
{
_rotation_enum = rotation;
@@ -183,19 +195,8 @@ void Gyroscope::ParametersUpdate()
_priority = new_priority;
}
bool calibration_changed = false;
// CAL_GYROx_OFF{X,Y,Z}
const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
_offset = offset;
}
if (calibration_changed) {
_calibration_count++;
}
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
} else {
Reset();
src/lib/sensor_calibration/Gyroscope.hpp
+2 -1
View File
@@ -62,13 +62,14 @@ public:
void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
void set_device_id(uint32_t device_id, bool external = false);
void set_external(bool external = true);
void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
bool set_offset(const matrix::Vector3f &offset);
void set_rotation(Rotation rotation);
uint8_t calibration_count() const { return _calibration_count; }
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; }
src/lib/sensor_calibration/Magnetometer.cpp
+40 -42
View File
@@ -83,23 +83,49 @@ void Magnetometer::set_external(bool external)
_external = external;
}
void Magnetometer::set_scale(const Vector3f &scale)
bool Magnetometer::set_offset(const Vector3f &offset)
{
_scale(0, 0) = scale(0);
_scale(1, 1) = scale(1);
_scale(2, 2) = scale(2);
if (Vector3f(_offset - offset).longerThan(0.001f)) {
_offset = offset;
_calibration_count++;
return true;
}
return false;
}
void Magnetometer::set_offdiagonal(const Vector3f &offdiagonal)
bool Magnetometer::set_scale(const Vector3f &scale)
{
_scale(0, 1) = offdiagonal(0);
_scale(1, 0) = offdiagonal(0);
if (Vector3f(_scale.diag() - scale).longerThan(0.001f)) {
_scale(0, 0) = scale(0);
_scale(1, 1) = scale(1);
_scale(2, 2) = scale(2);
_scale(0, 2) = offdiagonal(1);
_scale(2, 0) = offdiagonal(1);
_calibration_count++;
return true;
}
_scale(1, 2) = offdiagonal(2);
_scale(2, 1) = offdiagonal(2);
return false;
}
bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal)
{
if (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiagonal).longerThan(0.001f)) {
_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(Rotation rotation)
@@ -162,42 +188,14 @@ void Magnetometer::ParametersUpdate()
_priority = new_priority;
}
bool calibration_changed = false;
// CAL_MAGx_OFF{X,Y,Z}
const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
_offset = offset;
}
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
// CAL_MAGx_SCALE{X,Y,Z}
const Vector3f diag = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index);
if (Vector3f(_scale.diag() - diag).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
}
set_scale(GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index));
// CAL_MAGx_ODIAG{X,Y,Z}
const Vector3f offdiag = GetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index);
if (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiag).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
}
if (calibration_changed) {
float scale[9] {
diag(0), offdiag(0), offdiag(1),
offdiag(0), diag(1), offdiag(2),
offdiag(1), offdiag(2), diag(2)
};
_scale = Matrix3f{scale};
_calibration_count++;
}
set_offdiagonal(GetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index));
// CAL_MAGx_COMP{X,Y,Z}
_power_compensation = GetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index);
src/lib/sensor_calibration/Magnetometer.hpp
+9 -3
View File
@@ -63,9 +63,9 @@ public:
void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
void set_device_id(uint32_t device_id, bool external = false);
void set_external(bool external = true);
void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
void set_scale(const matrix::Vector3f &scale);
void set_offdiagonal(const matrix::Vector3f &offdiagonal);
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);
uint8_t calibration_count() const { return _calibration_count; }
@@ -85,6 +85,12 @@ public:
return _rotation * (_scale * ((data + _power * _power_compensation) - _offset));
}
// Compute sensor offset from bias (board frame)
matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
{
return _scale.I() * _rotation.I() * bias + _offset;
}
bool ParametersSave();
void ParametersUpdate();