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

vehicle_imu/vehicle_magnetometer add calibration indicator to message

Browse Source 
- vehicle_imu/vehicle_magnetometer add monotonically increasing `calibration_count` field so that downstream subscribers are aware of calibration changes
This commit is contained in:
Daniel Agar
2020-09-15 13:12:57 -04:00
committed by GitHub
parent 87471a988d
commit c41f053c7b
10 changed files with 79 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -149,11 +149,27 @@ void Accelerometer::ParametersUpdate()
_priority = DEFAULT_PRIORITY;
}
bool calibration_changed = false;
// CAL_ACCx_OFF{X,Y,Z}
_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
_offset = offset;
}
// CAL_ACCx_SCALE{X,Y,Z}
_scale = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index);
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++;
}
} else {
Reset();
@@ -170,6 +186,8 @@ void Accelerometer::Reset()
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
_calibration_count = 0;
}
bool Accelerometer::ParametersSave()