fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 14:58:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

gyro calibration: slightly stricter motion detection check

Browse Source 
From 0.573 deg to 0.4.

And some cleanup.
This commit is contained in:
Beat Küng
2019-11-29 11:28:17 -05:00
committed by Daniel Agar
parent 1cbcb445ab
commit 4ca5770f36
1 changed files with 27 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/gyro_calibration.cpp
+27 -34
View File
@@ -51,6 +51,7 @@
#include <fcntl.h>
#include <poll.h>
#include <math.h>
#include <mathlib/mathlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
@@ -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) ||