fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 13:01:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: gyro calibration use MedianFilter

Browse Source
This commit is contained in:
Daniel Agar
2020-11-02 12:58:46 -05:00
parent d14deb0e5a
commit 817285ec64
1 changed files with 10 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/gyro_calibration.cpp
+10 -32
View File
@@ -49,10 +49,11 @@
#include <px4_platform_common/time.h> #include <px4_platform_common/time.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <lib/sensor_calibration/Gyroscope.hpp> #include <lib/mathlib/math/filter/MedianFilter.hpp>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h> #include <lib/parameters/param.h>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h> #include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp> #include <uORB/SubscriptionBlocking.hpp>
@@ -72,22 +73,9 @@ struct gyro_worker_data_t {
Vector3f offset[MAX_GYROS] {}; Vector3f offset[MAX_GYROS] {};
static constexpr int last_num_samples = 9; ///< number of samples for the motion detection median filter math::MedianFilter<float, 9> filter[3] {};
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(gyro_worker_data_t &worker_data) static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
{ {
const hrt_abstime calibration_started = hrt_absolute_time(); const hrt_abstime calibration_started = hrt_absolute_time();
@@ -105,11 +93,6 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
{ORB_ID(sensor_gyro), 0, 3}, {ORB_ID(sensor_gyro), 0, 3},
}; };
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 */ /* use slowest gyro to pace, but count correctly per-gyro for statistics */
unsigned slow_count = 0; unsigned slow_count = 0;
@@ -162,10 +145,9 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
calibration_counter[gyro_index]++; calibration_counter[gyro_index]++;
if (gyro_index == 0) { if (gyro_index == 0) {
worker_data.last_sample_0_x[worker_data.last_sample_0_idx] = gyro_report.x - offset(0); worker_data.filter[0].insert(gyro_report.x - offset(0));
worker_data.last_sample_0_y[worker_data.last_sample_0_idx] = gyro_report.y - offset(1); worker_data.filter[1].insert(gyro_report.y - offset(1));
worker_data.last_sample_0_z[worker_data.last_sample_0_idx] = gyro_report.z - offset(2); worker_data.filter[2].insert(gyro_report.z - offset(2));
worker_data.last_sample_0_idx = (worker_data.last_sample_0_idx + 1) % gyro_worker_data_t::last_num_samples;
} }
} }
@@ -257,13 +239,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
} else { } else {
/* check offsets using a median filter */ /* check offsets using a median filter */
qsort(worker_data.last_sample_0_x, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp); float xdiff = worker_data.filter[0].median() - worker_data.offset[0](0);
qsort(worker_data.last_sample_0_y, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp); float ydiff = worker_data.filter[1].median() - worker_data.offset[0](1);
qsort(worker_data.last_sample_0_z, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp); float zdiff = worker_data.filter[2].median() - worker_data.offset[0](2);
float xdiff = worker_data.last_sample_0_x[gyro_worker_data_t::last_num_samples / 2] - worker_data.offset[0](0);
float ydiff = worker_data.last_sample_0_y[gyro_worker_data_t::last_num_samples / 2] - worker_data.offset[0](1);
float zdiff = worker_data.last_sample_0_z[gyro_worker_data_t::last_num_samples / 2] - worker_data.offset[0](2);
/* maximum allowable calibration error */ /* maximum allowable calibration error */
static constexpr float maxoff = math::radians(0.6f); static constexpr float maxoff = math::radians(0.6f);