From bdbc4f4d6506c8067eb01f4fa86c7e07407a547a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 28 Jan 2017 12:18:56 +1100 Subject: [PATCH] commander: fix bug in gyro calibration If the same gyro data was contained in two uORB instances, the thermal offset coefficient was being corrected twice. TODO should fix what was causing data from the same sensor to appear on two uORB topics. --- src/modules/commander/gyro_calibration.cpp | 51 ++++++++++++++-------- 1 file changed, 32 insertions(+), 19 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 44bac9521a..784d176507 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -364,6 +364,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary))); + bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator + for (unsigned uorb_index = 0; uorb_index < max_gyros; uorb_index++) { if (worker_data.device_id[uorb_index] != 0) { char str[30]; @@ -377,27 +379,38 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) memset(&sensor_correction, 0, sizeof(sensor_correction)); orb_copy(ORB_ID(sensor_correction), worker_data.sensor_correction_sub, &sensor_correction); - /* update the _X0_ terms to include the additional offset */ - int32_t handle; - float val; - for (unsigned axis_index = 0; axis_index < 3; axis_index++) { - val = 0.0f; - (void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index); - handle = param_find(str); - param_get(handle, &val); - if (axis_index == 0) { - val += worker_data.gyro_scale[uorb_index].x_offset; - } else if (axis_index == 1) { - val += worker_data.gyro_scale[uorb_index].y_offset; - } else if (axis_index == 2) { - val += worker_data.gyro_scale[uorb_index].z_offset; - } - if (axis_index == 2) { //notify the system about the change, but only once, for the last one - failed |= (PX4_OK != param_set(handle, &val)); - } else { - failed |= (PX4_OK != param_set_no_notification(handle, &val)); + /* don't allow a parameter instance to be calibrated again by another uORB instance */ + if (!tc_locked[sensor_correction.gyro_mapping[uorb_index]]) { + tc_locked[sensor_correction.gyro_mapping[uorb_index]] = true; + + /* update the _X0_ terms to include the additional offset */ + int32_t handle; + float val; + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + val = 0.0f; + (void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index); + handle = param_find(str); + param_get(handle, &val); + if (axis_index == 0) { + val += worker_data.gyro_scale[uorb_index].x_offset; + + } else if (axis_index == 1) { + val += worker_data.gyro_scale[uorb_index].y_offset; + + } else if (axis_index == 2) { + val += worker_data.gyro_scale[uorb_index].z_offset; + + } + if (axis_index == 2) { //notify the system about the change, but only once, for the last one + failed |= (PX4_OK != param_set(handle, &val)); + + } else { + failed |= (PX4_OK != param_set_no_notification(handle, &val)); + + } } } + // Ensure the calibration values used the driver are at default settings worker_data.gyro_scale[uorb_index].x_offset = 0.f; worker_data.gyro_scale[uorb_index].y_offset = 0.f;