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

lib/sensor_calibration: add simple offset and scale sanity checks

Browse Source
This commit is contained in:
Daniel Agar
2021-03-09 09:27:21 -05:00
committed by GitHub
parent c4ab2797eb
commit bd1c575ce8
3 changed files with 42 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -126,10 +126,11 @@ void Accelerometer::SensorCorrectionsUpdate(bool force)
bool Accelerometer::set_offset(const Vector3f &offset)
{
if (Vector3f(_offset - offset).longerThan(0.01f)) {
_offset = offset;
_calibration_count++;
return true;
if (PX4_ISFINITE(offset(0)) && PX4_ISFINITE(offset(1)) && PX4_ISFINITE(offset(2))) {
_offset = offset;
_calibration_count++;
return true;
}
}
return false;
@@ -138,10 +139,13 @@ bool Accelerometer::set_offset(const Vector3f &offset)
bool Accelerometer::set_scale(const Vector3f &scale)
{
if (Vector3f(_scale - scale).longerThan(0.01f)) {
_scale = scale;
if ((scale(0) > 0.f) && (scale(1) > 0.f) && (scale(2) > 0.f) &&
PX4_ISFINITE(scale(0)) && PX4_ISFINITE(scale(1)) && PX4_ISFINITE(scale(2))) {
_calibration_count++;
return true;
_scale = scale;
_calibration_count++;
return true;
}
}
return false;