mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 22:32:11 +08:00
commander: Improve gyro cal
This commit is contained in:
Lorenz Meier
@@ -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();
|
||||
|
||||
Reference in New Issue
Block a user