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

commander gyro cal: Optimize parameter set calls and allow up to 0.0055 rad/s deviation - tuned to allow in-field calibration, but fail anyone really rotating during the step

This commit is contained in:
Lorenz Meier 2015-04-19 16:06:52 +02:00
parent 1685f77031
commit 1da048549a
1 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -86,13 +86,6 @@ int do_gyro_calibration(int mavlink_fd)
int res = OK;
/* store board ID */
uint32_t mcu_id[3];
mcu_unique_id(&mcu_id[0]);
/* store last 32bit number - not unique, but unique in a given set */
(void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
char str[30];
for (unsigned s = 0; s < max_gyros; s++) {
@ -196,7 +189,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.004f;
const float maxoff = 0.0055f;
if (!isfinite(gyro_scale[0].x_offset) ||
!isfinite(gyro_scale[0].y_offset) ||
@ -204,7 +197,7 @@ int do_gyro_calibration(int mavlink_fd)
fabsf(xdiff) > maxoff ||
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
mavlink_log_critical(mavlink_fd, "ERROR: Motion during calibration");
mavlink_and_console_log_critical(mavlink_fd, "ERROR: Motion during calibration");
res = ERROR;
}
}
@ -221,13 +214,13 @@ int do_gyro_calibration(int mavlink_fd)
}
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
failed |= (OK != param_set(param_find(str), &(gyro_scale[s].x_offset)));
failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].x_offset)));
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
failed |= (OK != param_set(param_find(str), &(gyro_scale[s].y_offset)));
failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].y_offset)));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
failed |= (OK != param_set(param_find(str), &(gyro_scale[s].z_offset)));
failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].z_offset)));
(void)sprintf(str, "CAL_GYRO%u_ID", s);
failed |= (OK != param_set(param_find(str), &(device_id[s])));
failed |= (OK != param_set_no_notification(param_find(str), &(device_id[s])));
/* apply new scaling and offsets */
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
@ -252,6 +245,13 @@ int do_gyro_calibration(int mavlink_fd)
}
}
/* store board ID */
uint32_t mcu_id[3];
mcu_unique_id(&mcu_id[0]);
/* store last 32bit number - not unique, but unique in a given set */
(void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();