diff options
Diffstat (limited to 'src/modules/commander/gyro_calibration.cpp')
| -rw-r--r-- | src/modules/commander/gyro_calibration.cpp | 37 |
1 files changed, 29 insertions, 8 deletions
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index dfd1657c5..8a70cf66e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -73,16 +73,17 @@ int do_gyro_calibration(int mavlink_fd) /* wait for the user to respond */ sleep(2); - struct gyro_scale gyro_scale[max_gyros] = { { + struct gyro_scale gyro_scale_zero = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, - } }; + struct gyro_scale gyro_scale[max_gyros]; + int res = OK; /* store board ID */ @@ -97,7 +98,7 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { /* 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); /* 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); - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); close(fd); if (res != OK) { @@ -120,6 +121,8 @@ int do_gyro_calibration(int mavlink_fd) unsigned calibration_counter[max_gyros] = { 0 }; const unsigned calibration_count = 5000; + struct gyro_report gyro_report_0 = {}; + if (res == OK) { /* determine gyro mean values */ unsigned poll_errcount = 0; @@ -140,7 +143,7 @@ int do_gyro_calibration(int mavlink_fd) while (calibration_counter[0] < calibration_count) { /* 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) { @@ -150,6 +153,11 @@ int do_gyro_calibration(int mavlink_fd) if (changed) { 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].y_offset += gyro_report.y; gyro_scale[s].z_offset += gyro_report.z; @@ -183,8 +191,20 @@ int do_gyro_calibration(int mavlink_fd) if (res == OK) { /* check offsets */ - if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || !isfinite(gyro_scale[0].z_offset)) { - mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); + float xdiff = gyro_report_0.x - gyro_scale[0].x_offset; + 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; } } @@ -214,6 +234,7 @@ int do_gyro_calibration(int mavlink_fd) int fd = open(str, 0); if (fd < 0) { + failed = true; continue; } @@ -226,7 +247,7 @@ int do_gyro_calibration(int mavlink_fd) } 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; } } |