commander: fix bugs in handling of thermal compensation during access cal

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,8 +317,11 @@ 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);
 
+			/* 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 */
-			/* 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++) {
@@ -331,19 +336,24 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 					} 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));
 					}
+				}
+
+				/* 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);
-				param_get(handle, &val);
 					if (axis_index == 0) {
-					val *= accel_scale.x_scale;
+						val = accel_scale.x_scale;
 					} else if (axis_index == 1) {
-					val *= accel_scale.y_scale;
+						val = accel_scale.y_scale;
 					} else if (axis_index == 2) {
-					val *= accel_scale.z_scale;
+						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));
@@ -351,8 +361,9 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 						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;