diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-28 16:44:26 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-28 16:44:26 -0700 |
commit | 17ee20a338a05a9d46c4aa8a4c193c0b0810b3b1 (patch) | |
tree | 01d92024fcab9c850996101485c70d0c2202bc9f | |
parent | cb99467cdeaffc783c9df8fd286a6a6ca2b2df34 (diff) | |
download | px4-firmware-17ee20a338a05a9d46c4aa8a4c193c0b0810b3b1.tar.gz px4-firmware-17ee20a338a05a9d46c4aa8a4c193c0b0810b3b1.tar.bz2 px4-firmware-17ee20a338a05a9d46c4aa8a4c193c0b0810b3b1.zip |
commander: Improve gyro cal
-rw-r--r-- | src/modules/commander/gyro_calibration.cpp | 132 |
1 files changed, 3 insertions, 129 deletions
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 47ee9a5e0..e2a7ef743 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -82,7 +82,7 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, }; - struct gyro_scale gyro_scale[max_gyros]; + struct gyro_scale gyro_scale[max_gyros] = {}; int res = OK; @@ -196,7 +196,7 @@ int do_gyro_calibration(int mavlink_fd) float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; /* maximum allowable calibration error in radians */ - const float maxoff = 0.01f; + const float maxoff = 0.002f; if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || @@ -204,7 +204,7 @@ int do_gyro_calibration(int mavlink_fd) fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) { - mavlink_log_critical(mavlink_fd, "ERROR: Calibration failed"); + mavlink_log_critical(mavlink_fd, "ERROR: Motion during calibration"); res = ERROR; } } @@ -252,132 +252,6 @@ int do_gyro_calibration(int mavlink_fd) } } -#if 0 - /* beep on offset calibration end */ - mavlink_log_info(mavlink_fd, "gyro offset calibration done"); - tune_neutral(); - - /* scale calibration */ - /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ - - mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); - warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); - - /* apply new offsets */ - fd = open(GYRO_DEVICE_PATH, 0); - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) { - warn("WARNING: failed to apply new offsets for gyro"); - } - - close(fd); - - - unsigned rotations_count = 30; - float gyro_integral = 0.0f; - float baseline_integral = 0.0f; - - // XXX change to mag topic - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - - float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - - if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; } - - if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; } - - - uint64_t last_time = hrt_absolute_time(); - uint64_t start_time = hrt_absolute_time(); - - while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { - - /* abort this loop if not rotated more than 180 degrees within 5 seconds */ - if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) { - mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); - close(sub_sensor_combined); - return OK; - } - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_sensor_combined; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - - float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; - last_time = hrt_absolute_time(); - - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - - // XXX this is just a proof of concept and needs world / body - // transformation and more - - //math::Vector2f magNav(raw.magnetometer_ga); - - // calculate error between estimate and measurement - // apply declination correction for true heading as well. - //float mag = -atan2f(magNav(1),magNav(0)); - float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - - if (mag > M_PI_F) { mag -= 2 * M_PI_F; } - - if (mag < -M_PI_F) { mag += 2 * M_PI_F; } - - float diff = mag - mag_last; - - if (diff > M_PI_F) { diff -= 2 * M_PI_F; } - - if (diff < -M_PI_F) { diff += 2 * M_PI_F; } - - baseline_integral += diff; - mag_last = mag; - // Jump through some timing scale hoops to avoid - // operating near the 1e6/1e8 max sane resolution of float. - gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; - -// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); - - // } else if (poll_ret == 0) { - // /* any poll failure for 1s is a reason to abort */ - // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - // return; - } - } - - float gyro_scale = baseline_integral / gyro_integral; - - warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); - mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); - - - if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { - mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); - close(sub_sensor_gyro); - mavlink_log_critical(mavlink_fd, "gyro calibration failed"); - return ERROR; - } - - /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "gyro scale calibration done"); - tune_neutral(); - - if (res == OK) { - /* set scale parameters to new values */ - if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale)) - || param_set(param_find("CAL_GYRO0_YSCALE"), &(gyro_scale.y_scale)) - || param_set(param_find("CAL_GYRO0_ZSCALE"), &(gyro_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); - res = ERROR; - } - } - -#endif - if (res == OK) { /* auto-save to EEPROM */ res = param_save_default(); |