From fdb75dbba29cc2b95e9a34ee04e64a6dc265a30d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 25 Jan 2017 14:55:22 +0100 Subject: [PATCH] accelerometer_calibration: simplify & fix if temp compensation is enabled 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. --- .../commander/accelerometer_calibration.cpp | 53 ++++++++----------- 1 file changed, 21 insertions(+), 32 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 977282dc5e..4df15b1cb3 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -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;