fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 20:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support calibration of fast+slow gyros #6998

Browse Source
This commit is contained in:
Nate Weibley
2017-04-13 20:22:35 +02:00
committed by Lorenz Meier
parent 5fc20bea5d
commit 9eb0e62787
1 changed files with 22 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/gyro_calibration.cpp
+22 -6
View File
@@ -77,7 +77,7 @@ typedef struct {
static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
{ {
gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data); gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data);
unsigned calibration_counter[max_gyros] = { 0 }; unsigned calibration_counter[max_gyros] = { 0 }, slow_count = 0;
const unsigned calibration_count = 5000; const unsigned calibration_count = 5000;
struct gyro_report gyro_report; struct gyro_report gyro_report;
unsigned poll_errcount = 0; unsigned poll_errcount = 0;
@@ -102,8 +102,8 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0)); memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));
/* use first gyro to pace, but count correctly per-gyro for statistics */ /* use slowest gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) { while (slow_count < calibration_count) {
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) { if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
return calibrate_return_cancelled; return calibrate_return_cancelled;
} }
@@ -119,8 +119,13 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
int poll_ret = px4_poll(&fds[0], max_gyros, 1000); int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
if (poll_ret > 0) { if (poll_ret > 0) {
unsigned update_count = calibration_count;
for (unsigned s = 0; s < max_gyros; s++) { for (unsigned s = 0; s < max_gyros; s++) {
if (calibration_counter[s] >= calibration_count) {
// Skip if instance has enough samples
continue;
}
bool changed; bool changed;
orb_check(worker_data->gyro_sensor_sub[s], &changed); orb_check(worker_data->gyro_sensor_sub[s], &changed);
@@ -160,9 +165,20 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
} }
if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { // Maintain the sample count of the slowest sensor
calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); if (calibration_counter[s] && calibration_counter[s] < update_count) {
update_count = calibration_counter[s];
} }
}
if (update_count % (calibration_count / 20) == 0) {
calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (update_count * 100) / calibration_count);
}
// Propagate out the slowest sensor's count
if (slow_count < update_count) {
slow_count = update_count;
} }
} else { } else {