From 4ca5770f36120d330215fb70ea7859b462e9e5a5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 28 Nov 2019 18:24:44 +0100 Subject: [PATCH] gyro calibration: slightly stricter motion detection check From 0.573 deg to 0.4. And some cleanup. --- src/modules/commander/gyro_calibration.cpp | 61 ++++++++++------------ 1 file changed, 27 insertions(+), 34 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index bef0e69a1a..3590a197d5 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -71,7 +72,7 @@ typedef struct { int gyro_sensor_sub[max_gyros]; int sensor_correction_sub; struct gyro_calibration_s gyro_scale[max_gyros]; - sensor_gyro_s gyro_report_0; + float last_sample_0[3]; } gyro_worker_data_t; static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) @@ -99,8 +100,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) fds[s].events = POLLIN; } - memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0)); - + memset(&worker_data->last_sample_0, 0, sizeof(worker_data->last_sample_0)); /* use slowest gyro to pace, but count correctly per-gyro for statistics */ while (slow_count < calibration_count) { @@ -132,45 +132,38 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) if (changed) { orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report); + float sample[3]; if (s == 0) { // take a working copy - worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_0[0]) * - sensor_correction.gyro_scale_0[0]; - worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_0[1]) * - sensor_correction.gyro_scale_0[1]; - worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_0[2]) * - sensor_correction.gyro_scale_0[2]; + sample[0] = (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0]; + sample[1] = (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1]; + sample[2] = (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2]; - // take a reference copy of the primary sensor including correction for thermal drift - orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0); - worker_data->gyro_report_0.x = (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0]; - worker_data->gyro_report_0.y = (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1]; - worker_data->gyro_report_0.z = (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2]; + for (int i = 0; i < 3; ++i) { + worker_data->last_sample_0[i] = sample[i]; + } } else if (s == 1) { - worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_1[0]) * - sensor_correction.gyro_scale_1[0]; - worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_1[1]) * - sensor_correction.gyro_scale_1[1]; - worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_1[2]) * - sensor_correction.gyro_scale_1[2]; + sample[0] = (gyro_report.x - sensor_correction.gyro_offset_1[0]) * sensor_correction.gyro_scale_1[0]; + sample[1] = (gyro_report.y - sensor_correction.gyro_offset_1[1]) * sensor_correction.gyro_scale_1[1]; + sample[2] = (gyro_report.z - sensor_correction.gyro_offset_1[2]) * sensor_correction.gyro_scale_1[2]; } else if (s == 2) { - worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_2[0]) * - sensor_correction.gyro_scale_2[0]; - worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_2[1]) * - sensor_correction.gyro_scale_2[1]; - worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_2[2]) * - sensor_correction.gyro_scale_2[2]; + sample[0] = (gyro_report.x - sensor_correction.gyro_offset_2[0]) * sensor_correction.gyro_scale_2[0]; + sample[1] = (gyro_report.y - sensor_correction.gyro_offset_2[1]) * sensor_correction.gyro_scale_2[1]; + sample[2] = (gyro_report.z - sensor_correction.gyro_offset_2[2]) * sensor_correction.gyro_scale_2[2]; } else { - worker_data->gyro_scale[s].x_offset += gyro_report.x; - worker_data->gyro_scale[s].y_offset += gyro_report.y; - worker_data->gyro_scale[s].z_offset += gyro_report.z; + sample[0] = gyro_report.x; + sample[1] = gyro_report.y; + sample[2] = gyro_report.z; } + worker_data->gyro_scale[s].x_offset += sample[0]; + worker_data->gyro_scale[s].y_offset += sample[1]; + worker_data->gyro_scale[s].z_offset += sample[2]; calibration_counter[s]++; } @@ -401,12 +394,12 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) } else { /* check offsets */ - float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset; - float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset; - float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset; + float xdiff = worker_data.last_sample_0[0] - worker_data.gyro_scale[0].x_offset; + float ydiff = worker_data.last_sample_0[1] - worker_data.gyro_scale[0].y_offset; + float zdiff = worker_data.last_sample_0[2] - worker_data.gyro_scale[0].z_offset; - /* maximum allowable calibration error in radians */ - const float maxoff = 0.01f; + /* maximum allowable calibration error */ + const float maxoff = math::radians(0.4f); if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) || !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) ||