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

gyro calibration: add median filter to motion detection 

The bmi088 has a bit more noise and tends to trigger it.
This improves reobustness against noise, and increases the threshold
slightly.
This commit is contained in:
Beat Küng 2020-06-10 16:39:05 +02:00 committed by Daniel Agar
parent 13e34b32e6
commit 16323256bc
1 changed files with 37 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/modules/commander/gyro_calibration.cpp
View File

@ -47,6 +47,7 @@
#include <px4_platform_common/defines.h>
#include <px4_platform_common/time.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <math.h>
@ -65,14 +66,28 @@ static const char *sensor_name = "gyro";
static constexpr unsigned max_gyros = 3;
/// Data passed to calibration worker routine
typedef struct {
struct gyro_worker_data_t {
orb_advert_t *mavlink_log_pub;
int32_t device_id[max_gyros];
int gyro_sensor_sub[max_gyros];
int sensor_correction_sub;
struct gyro_calibration_s gyro_scale[max_gyros];
float last_sample_0[3];
} gyro_worker_data_t;
static constexpr int last_num_samples = 9; ///< number of samples for the motion detection median filter
float last_sample_0_x[last_num_samples];
float last_sample_0_y[last_num_samples];
float last_sample_0_z[last_num_samples];
int last_sample_0_idx;
};
static int float_cmp(const void *elem1, const void *elem2)
{
if (*(const float *)elem1 < * (const float *)elem2) {
return -1;
}
return *(const float *)elem1 > *(const float *)elem2;
}
static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
{
@ -99,7 +114,10 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
fds[s].events = POLLIN;
}
memset(&worker_data->last_sample_0, 0, sizeof(worker_data->last_sample_0));
memset(&worker_data->last_sample_0_x, 0, sizeof(worker_data->last_sample_0_x));
memset(&worker_data->last_sample_0_y, 0, sizeof(worker_data->last_sample_0_y));
memset(&worker_data->last_sample_0_z, 0, sizeof(worker_data->last_sample_0_z));
worker_data->last_sample_0_idx = 0;
/* use slowest gyro to pace, but count correctly per-gyro for statistics */
while (slow_count < calibration_count) {
@ -139,9 +157,10 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
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];
for (int i = 0; i < 3; ++i) {
worker_data->last_sample_0[i] = sample[i];
}
worker_data->last_sample_0_x[worker_data->last_sample_0_idx] = sample[0];
worker_data->last_sample_0_y[worker_data->last_sample_0_idx] = sample[1];
worker_data->last_sample_0_z[worker_data->last_sample_0_idx] = sample[2];
worker_data->last_sample_0_idx = (worker_data->last_sample_0_idx + 1) % gyro_worker_data_t::last_num_samples;
} else if (s == 1) {
sample[0] = (gyro_report.x - sensor_correction.gyro_offset_1[0]) * sensor_correction.gyro_scale_1[0];
@ -364,13 +383,19 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
res = PX4_ERROR;
} else {
/* check offsets */
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;
/* check offsets using a median filter */
qsort(worker_data.last_sample_0_x, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp);
qsort(worker_data.last_sample_0_y, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp);
qsort(worker_data.last_sample_0_z, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp);
float xdiff = worker_data.last_sample_0_x[gyro_worker_data_t::last_num_samples / 2] -
worker_data.gyro_scale[0].x_offset;
float ydiff = worker_data.last_sample_0_y[gyro_worker_data_t::last_num_samples / 2] -
worker_data.gyro_scale[0].y_offset;
float zdiff = worker_data.last_sample_0_z[gyro_worker_data_t::last_num_samples / 2] -
worker_data.gyro_scale[0].z_offset;
/* maximum allowable calibration error */
const float maxoff = math::radians(0.4f);
const float maxoff = math::radians(0.6f);
if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) ||
!PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) ||