dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: Improve gyro calibration

This commit is contained in:
Lorenz Meier 2015-03-15 17:24:29 +01:00
parent 37de377dcf
commit 904fc9b21c
1 changed files with 29 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -73,16 +73,17 @@ int do_gyro_calibration(int mavlink_fd)
/* wait for the user to respond */ /* wait for the user to respond */
sleep(2); sleep(2);
struct gyro_scale gyro_scale[max_gyros] = { { struct gyro_scale gyro_scale_zero = {
0.0f, 0.0f,
1.0f, 1.0f,
0.0f, 0.0f,
1.0f, 1.0f,
0.0f, 0.0f,
1.0f, 1.0f,
}
}; };
struct gyro_scale gyro_scale[max_gyros];
int res = OK; int res = OK;
/* store board ID */ /* store board ID */
@ -97,7 +98,7 @@ int do_gyro_calibration(int mavlink_fd)
for (unsigned s = 0; s < max_gyros; s++) { for (unsigned s = 0; s < max_gyros; s++) {
/* ensure all scale fields are initialized tha same as the first struct */ /* ensure all scale fields are initialized tha same as the first struct */
(void)memcpy(&gyro_scale[s], &gyro_scale[0], sizeof(gyro_scale[0])); (void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0]));
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */ /* reset all offsets to zero and all scales to one */
@ -109,7 +110,7 @@ int do_gyro_calibration(int mavlink_fd)
device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
close(fd); close(fd);
if (res != OK) { if (res != OK) {
@ -120,6 +121,8 @@ int do_gyro_calibration(int mavlink_fd)
unsigned calibration_counter[max_gyros] = { 0 }; unsigned calibration_counter[max_gyros] = { 0 };
const unsigned calibration_count = 5000; const unsigned calibration_count = 5000;
struct gyro_report gyro_report_0 = {};
if (res == OK) { if (res == OK) {
/* determine gyro mean values */ /* determine gyro mean values */
unsigned poll_errcount = 0; unsigned poll_errcount = 0;
@ -140,7 +143,7 @@ int do_gyro_calibration(int mavlink_fd)
while (calibration_counter[0] < calibration_count) { while (calibration_counter[0] < calibration_count) {
/* wait blocking for new data */ /* wait blocking for new data */
int poll_ret = poll(fds, 1, 1000); int poll_ret = poll(&fds[0], max_gyros, 1000);
if (poll_ret > 0) { if (poll_ret > 0) {
@ -150,6 +153,11 @@ int do_gyro_calibration(int mavlink_fd)
if (changed) { if (changed) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report); orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report);
if (s == 0) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0);
}
gyro_scale[s].x_offset += gyro_report.x; gyro_scale[s].x_offset += gyro_report.x;
gyro_scale[s].y_offset += gyro_report.y; gyro_scale[s].y_offset += gyro_report.y;
gyro_scale[s].z_offset += gyro_report.z; gyro_scale[s].z_offset += gyro_report.z;
@ -183,8 +191,20 @@ int do_gyro_calibration(int mavlink_fd)
if (res == OK) { if (res == OK) {
/* check offsets */ /* check offsets */
if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || !isfinite(gyro_scale[0].z_offset)) { float xdiff = gyro_report_0.x - gyro_scale[0].x_offset;
mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); float ydiff = gyro_report_0.y - gyro_scale[0].y_offset;
float zdiff = gyro_report_0.z - gyro_scale[0].z_offset;
/* maximum allowable calibration error in radians */
const float maxoff = 0.01f;
if (!isfinite(gyro_scale[0].x_offset) ||
!isfinite(gyro_scale[0].y_offset) ||
!isfinite(gyro_scale[0].z_offset) ||
fabsf(xdiff) > maxoff ||
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
mavlink_log_critical(mavlink_fd, "ERROR: Calibration failed");
res = ERROR; res = ERROR;
} }
} }
@ -214,6 +234,7 @@ int do_gyro_calibration(int mavlink_fd)
int fd = open(str, 0); int fd = open(str, 0);
if (fd < 0) { if (fd < 0) {
failed = true;
continue; continue;
} }
@ -226,7 +247,7 @@ int do_gyro_calibration(int mavlink_fd)
} }
if (failed) { if (failed) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params");
res = ERROR; res = ERROR;
} }
} }