fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

accelerometer_calibration: simplify & fix if temp compensation is enabled

Browse Source 
if compensation enabled, scale & offsets for the drivers should be reset,
but actually only the params were reset and accel_scale was still applied
to the driver via ioctl.
This commit is contained in:
Beat Küng 2017-01-25 14:55:22 +01:00 committed by Lorenz Meier
parent d0ea4e8876
commit fdb75dbba2
1 changed files with 21 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
src/modules/commander/accelerometer_calibration.cpp
View File

@ -349,40 +349,29 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
}
// Ensure the calibration values used the driver are at default settings
float driver_offset = 0.0f;
float driver_scale = 1.0f;
(void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(driver_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(driver_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(driver_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(driver_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(driver_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(driver_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index])));
} else {
(void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index])));
accel_scale.x_offset = 0.f;
accel_scale.y_offset = 0.f;
accel_scale.z_offset = 0.f;
accel_scale.x_scale = 1.f;
accel_scale.y_scale = 1.f;
accel_scale.z_scale = 1.f;
}
(void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index])));
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, uorb_index);
return PX4_ERROR;