diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index b5a49192e9..8b1b39f013 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -275,6 +275,8 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) get_rot_matrix(board_rotation_id, &board_rotation); math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); + 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 < active_sensors; uorb_index++) { /* handle individual sensors, one by one */ @@ -295,11 +297,11 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); - PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, + PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, (double)accel_scale.x_offset, (double)accel_scale.y_offset, (double)accel_scale.z_offset); - PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, + PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, (double)accel_scale.x_scale, (double)accel_scale.y_scale, (double)accel_scale.z_scale); @@ -315,44 +317,53 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction); orb_unsubscribe(sensor_correction_sub); - /* update the _X0_ terms to include the additional offset */ - /* update the _SCL_ terms to include the additonal scale factor */ - int32_t handle; - float val; - for (unsigned axis_index = 0; axis_index < 3; axis_index++) { - val = 0.0f; - (void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index); - handle = param_find(str); - param_get(handle, &val); - if (axis_index == 0) { - val += accel_scale.x_offset; - } else if (axis_index == 1) { - val += accel_scale.y_offset; - } else if (axis_index == 2) { - val += accel_scale.z_offset; + /* don't allow a parameter instance to be calibrated more than once by another uORB instance */ + if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) { + tc_locked[sensor_correction.accel_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_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index); + handle = param_find(str); + param_get(handle, &val); + if (axis_index == 0) { + val += accel_scale.x_offset; + } else if (axis_index == 1) { + val += accel_scale.y_offset; + } else if (axis_index == 2) { + val += accel_scale.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)); + } } - failed |= (PX4_OK != param_set_no_notification(handle, &val)); - } - for (unsigned axis_index = 0; axis_index < 3; axis_index++) { - val = 1.0f; - (void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index); - handle = param_find(str); - param_get(handle, &val); - if (axis_index == 0) { - val *= accel_scale.x_scale; - } else if (axis_index == 1) { - val *= accel_scale.y_scale; - } else if (axis_index == 2) { - val *= accel_scale.z_scale; - } - 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)); + + /* update the _SCL_ terms to include the scale factor */ + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + val = 1.0f; + (void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index); + handle = param_find(str); + if (axis_index == 0) { + val = accel_scale.x_scale; + } else if (axis_index == 1) { + val = accel_scale.y_scale; + } else if (axis_index == 2) { + val = accel_scale.z_scale; + } + 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 + // Ensure the calibration values used by the driver are at default settings when we are using thermal calibration data accel_scale.x_offset = 0.f; accel_scale.y_offset = 0.f; accel_scale.z_offset = 0.f; @@ -361,6 +372,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) accel_scale.z_scale = 1.f; } + // save the driver level calibration data (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); @@ -606,19 +618,19 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m struct accel_report arp; orb_copy(ORB_ID(sensor_accel), subs[s], &arp); - // Apply thermal corrections + // Apply thermal offset corrections in sensor/board frame if (s == 0) { - accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]) * sensor_correction.accel_scale_0[0]; - accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]) * sensor_correction.accel_scale_0[1]; - accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]) * sensor_correction.accel_scale_0[2]; + accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]); + accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]); + accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]); } else if (s == 1) { - accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]) * sensor_correction.accel_scale_1[0]; - accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]) * sensor_correction.accel_scale_1[1]; - accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]) * sensor_correction.accel_scale_1[2]; + accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]); + accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]); + accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]); } else if (s == 2) { - accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]) * sensor_correction.accel_scale_2[0]; - accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]) * sensor_correction.accel_scale_2[1]; - accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]) * sensor_correction.accel_scale_2[2]; + accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]); + accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]); + accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]); } else { accel_sum[s][0] += arp.x; accel_sum[s][1] += arp.y;