fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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 10:51:53 -04:00 committed by Lorenz Meier
parent 5fc20bea5d
commit 9eb0e62787
1 changed files with 22 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -77,7 +77,7 @@ typedef struct {
static calibrate_return gyro_calibration_worker(int cancel_sub, void* 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;
struct gyro_report gyro_report;
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));
/* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) {
/* use slowest gyro to pace, but count correctly per-gyro for statistics */
while (slow_count < calibration_count) {
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
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);
if (poll_ret > 0) {
unsigned update_count = calibration_count;
for (unsigned s = 0; s < max_gyros; s++) {
if (calibration_counter[s] >= calibration_count) {
// Skip if instance has enough samples
continue;
}
bool 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) {
calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
// Maintain the sample count of the slowest sensor
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 {