yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00: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
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

44
src/lib/sensor_calibration/Accelerometer.cpp
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();