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

gyro_calibration: increase required samples (to be more conservative) 

- make sure variance is checked across all available sensors
This commit is contained in:
Daniel Agar 2021-12-24 15:07:29 -05:00
parent 3f3836afa8
commit 712353ea56
2 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/gyro_calibration/CMakeLists.txt
View File

@ -35,6 +35,7 @@ px4_add_module(
MODULE modules__gyro_calibration
MAIN gyro_calibration
COMPILE_FLAGS
#-DDEBUG_BUILD
SRCS
GyroCalibration.cpp
GyroCalibration.hpp

18
src/modules/gyro_calibration/GyroCalibration.cpp
View File

@ -200,21 +200,23 @@ void GyroCalibration::Run()
bool sufficient_samples = false;
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
if (_gyro_calibration[gyro].device_id() != 0) {
if ((_gyro_calibration[gyro].device_id() != 0) && _gyro_mean[gyro].valid()) {
// periodically check variance
if ((_gyro_mean[gyro].count() % 100 == 0)) {
if (_gyro_mean[gyro].count() % 100 == 0) {
PX4_DEBUG("gyro %d (%" PRIu32 ") variance, [%.9f, %.9f, %.9f] %.9f", gyro, _gyro_calibration[gyro].device_id(),
(double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2),
(double)_gyro_mean[gyro].variance().length());
if (_gyro_mean[gyro].variance().longerThan(0.001f)) {
// reset all
PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all",
gyro, (double)_gyro_mean[gyro].variance().length());
Reset();
return;
}
}
if (_gyro_mean[gyro].count() > 3000) {
if (_gyro_mean[gyro].count() > 5000) {
sufficient_samples = true;
} else {
@ -231,6 +233,16 @@ void GyroCalibration::Run()
for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
if (_gyro_calibration[gyro].device_id() != 0) {
// check variance again before saving
if (_gyro_mean[gyro].variance().longerThan(0.001f)) {
// reset all
PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all",
gyro, (double)_gyro_mean[gyro].variance().length());
Reset();
return;
}
_gyro_calibration[gyro].set_calibration_index(gyro);
const Vector3f old_offset{_gyro_calibration[gyro].offset()};