Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

commander gyro calibration: Do not require a specific position, automatically start a retry after motion on the first try

Browse Source
This commit is contained in:
Lorenz Meier
2015-05-08 11:01:19 +02:00
parent cc62033190
commit 3be1fc7d42
1 changed files with 53 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/gyro_calibration.cpp
+53 -40
View File
@@ -74,7 +74,7 @@ typedef struct {
struct gyro_report gyro_report_0;
} gyro_worker_data_t;
static calibrate_return gyro_calibration_worker(detect_orientation_return orientation, 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);
unsigned calibration_counter[max_gyros] = { 0 };
@@ -89,6 +89,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient
}
memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));
memset(&worker_data->gyro_scale, 0, sizeof(worker_data->gyro_scale));
/* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) {
@@ -149,7 +150,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient
int do_gyro_calibration(int mavlink_fd)
{
int res = OK;
gyro_worker_data_t worker_data;
gyro_worker_data_t worker_data = {};
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
@@ -196,51 +197,63 @@ int do_gyro_calibration(int mavlink_fd)
for (unsigned s = 0; s < max_gyros; s++) {
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
}
int cancel_sub = calibrate_cancel_subscribe();
unsigned try_count = 0;
unsigned max_tries = 20;
res = ERROR;
// Calibrate right-side up
bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false };
int cancel_sub = calibrate_cancel_subscribe();
calibrate_return cal_return = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output
cancel_sub, // Subscription to vehicle_command for cancel support
side_collected, // Sides to calibrate
gyro_calibration_worker, // Calibration worker
&worker_data, // Opaque data for calibration worked
true); // true: lenient still detection
do {
// Calibrate gyro and ensure user didn't move
calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already sent, we are done here
res = ERROR;
break;
} else if (cal_return == calibrate_return_error) {
res = ERROR;
} else {
/* check offsets */
float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
/* maximum allowable calibration error in radians */
const float maxoff = 0.0055f;
if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
!isfinite(worker_data.gyro_scale[0].y_offset) ||
!isfinite(worker_data.gyro_scale[0].z_offset) ||
fabsf(xdiff) > maxoff ||
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] motion, retrying..");
res = ERROR;
} else {
res = OK;
}
}
try_count++;
} while (res == ERROR && try_count <= max_tries);
if (try_count >= max_tries) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
res = ERROR;
}
calibrate_cancel_unsubscribe(cancel_sub);
for (unsigned s = 0; s < max_gyros; s++) {
close(worker_data.gyro_sensor_sub[s]);
}
if (cal_return == calibrate_return_cancelled) {
// Cancel message already sent, we are done here
return ERROR;
} else if (cal_return == calibrate_return_error) {
res = ERROR;
}
if (res == OK) {
/* check offsets */
float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
/* maximum allowable calibration error in radians */
const float maxoff = 0.0055f;
if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
!isfinite(worker_data.gyro_scale[0].y_offset) ||
!isfinite(worker_data.gyro_scale[0].z_offset) ||
fabsf(xdiff) > maxoff ||
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
res = ERROR;
}
}
if (res == OK) {
/* set offset parameters to new values */
bool failed = false;