fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 18:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

simple gyro auto calibration module

Browse Source
This commit is contained in:
Daniel Agar
2021-03-15 09:46:47 +01:00
committed by Lorenz Meier
parent 01c9a4f24d
commit 2257c3767e
70 changed files with 682 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/gyro_calibration.cpp
+6 -33
View File
@@ -57,7 +57,6 @@
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h>
static constexpr char sensor_name[] {"gyro"};
@@ -83,9 +82,6 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
static constexpr unsigned CALIBRATION_COUNT = 250;
unsigned poll_errcount = 0;
uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
sensor_correction_s sensor_correction{};
uORB::SubscriptionBlocking<sensor_gyro_s> gyro_sub[MAX_GYROS] {
{ORB_ID(sensor_gyro), 0, 0},
{ORB_ID(sensor_gyro), 0, 1},
@@ -115,39 +111,16 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
sensor_gyro_s gyro_report;
while (gyro_sub[gyro_index].update(&gyro_report)) {
// fetch optional thermal offset corrections in sensor frame
const Vector3f &thermal_offset{worker_data.calibrations[gyro_index].thermal_offset()};
// fetch optional thermal offset corrections in sensor/board frame
Vector3f offset{0, 0, 0};
sensor_correction_sub.update(&sensor_correction);
if (sensor_correction.timestamp > 0 && gyro_report.device_id != 0) {
for (uint8_t correction_index = 0; correction_index < MAX_GYROS; correction_index++) {
if (sensor_correction.gyro_device_ids[correction_index] == gyro_report.device_id) {
switch (correction_index) {
case 0:
offset = Vector3f{sensor_correction.gyro_offset_0};
break;
case 1:
offset = Vector3f{sensor_correction.gyro_offset_1};
break;
case 2:
offset = Vector3f{sensor_correction.gyro_offset_2};
break;
case 3:
offset = Vector3f{sensor_correction.gyro_offset_3};
break;
}
}
}
}
worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset;
worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - thermal_offset;
calibration_counter[gyro_index]++;
if (gyro_index == 0) {
worker_data.filter[0].insert(gyro_report.x - offset(0));
worker_data.filter[1].insert(gyro_report.y - offset(1));
worker_data.filter[2].insert(gyro_report.z - offset(2));
worker_data.filter[0].insert(gyro_report.x - thermal_offset(0));
worker_data.filter[1].insert(gyro_report.y - thermal_offset(1));
worker_data.filter[2].insert(gyro_report.z - thermal_offset(2));
}
}