dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

lib/sensor_calibration: add simple offset and scale sanity checks

This commit is contained in:
Daniel Agar 2021-03-09 09:27:21 -05:00 committed by GitHub
parent c4ab2797eb
commit bd1c575ce8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 42 additions and 29 deletions
Show Stats 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;

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

@ -126,10 +126,11 @@ void Gyroscope::SensorCorrectionsUpdate(bool force)
bool Gyroscope::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;

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

@ -86,10 +86,11 @@ void Magnetometer::set_external(bool external)
bool Magnetometer::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;
@ -98,12 +99,16 @@ bool Magnetometer::set_offset(const Vector3f &offset)
bool Magnetometer::set_scale(const Vector3f &scale)
{
if (Vector3f(_scale.diag() - scale).longerThan(0.01f)) {
_scale(0, 0) = scale(0);
_scale(1, 1) = scale(1);
_scale(2, 2) = scale(2);
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(0, 0) = scale(0);
_scale(1, 1) = scale(1);
_scale(2, 2) = scale(2);
_calibration_count++;
return true;
}
}
return false;
@ -112,17 +117,20 @@ bool Magnetometer::set_scale(const Vector3f &scale)
bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal)
{
if (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiagonal).longerThan(0.01f)) {
_scale(0, 1) = offdiagonal(0);
_scale(1, 0) = offdiagonal(0);
if (PX4_ISFINITE(offdiagonal(0)) && PX4_ISFINITE(offdiagonal(1)) && PX4_ISFINITE(offdiagonal(2))) {
_scale(0, 2) = offdiagonal(1);
_scale(2, 0) = offdiagonal(1);
_scale(0, 1) = offdiagonal(0);
_scale(1, 0) = offdiagonal(0);
_scale(1, 2) = offdiagonal(2);
_scale(2, 1) = offdiagonal(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);
_calibration_count++;
return true;
}
}
return false;
@ -275,7 +283,7 @@ void Magnetometer::PrintStatus()
}
#if defined(DEBUG_BUILD)
_scale.print()
_scale.print();
#endif // DEBUG_BUILD
}