sensor mag add minimal queue and calibration check all

- mag calibration acceptance check sphere/ellipsoid fit status

msg/sensor_mag.msg

@@ -12,3 +12,5 @@ float32 temperature       # temperature in degrees celsius
 uint32 error_count
 
 bool is_external	# if true the mag is external (i.e. not built into the board)
+
+uint8 ORB_QUEUE_LENGTH = 3

src/lib/drivers/magnetometer/PX4Magnetometer.hpp

@@ -61,7 +61,7 @@ public:
 	int get_class_instance() { return _class_device_instance; };
 
 private:
-	uORB::PublicationMulti<sensor_mag_s> _sensor_pub;
+	uORB::PublicationQueuedMulti<sensor_mag_s> _sensor_pub;
 
 	uint32_t		_device_id{0};
 	const enum Rotation	_rotation;

src/modules/commander/mag_calibration.cpp

@@ -312,11 +312,11 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 			Vector3f new_samples[MAX_MAGS] {};
 
 			for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
-				sensor_mag_s mag;
-
 				if (worker_data->calibration[cur_mag].device_id() != 0) {
-					if (mag_sub[cur_mag].update(&mag)) {
+					bool updated = false;
+					sensor_mag_s mag;
 
+					while (mag_sub[cur_mag].update(&mag)) {
 						if (worker_data->append_to_existing_calibration) {
 							// keep and update the existing calibration when we are not doing a full 6-axis calibration
 							const Matrix3f &scale = worker_data->calibration[cur_mag].scale();
@@ -335,14 +335,15 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 									    worker_data->calibration_counter_total[cur_mag],
 									    worker_data->calibration_sides * worker_data->calibration_points_perside);
 
-						if (reject) {
-							rejected = true;
-
-						} else {
+						if (!reject) {
 							new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z};
+							updated = true;
+							break;
 						}
+					}
 
-					} else {
+					// require an update for all valid mags
+					if (!updated) {
 						rejected = true;
 					}
 				}
@@ -522,6 +523,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 				// enough to reliably estimate both scales and offsets with 2 sides only (even if the existing calibration
 				// is already close)
 				bool sphere_fit_only = worker_data.calibration_sides <= 2;
+				bool sphere_fit_success = false;
 				{
 					float fitness = 1.0e30f;
 					float sphere_lambda = 1.0f;
@@ -535,6 +537,16 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 									    &diag[cur_mag](0), &diag[cur_mag](1), &diag[cur_mag](2),
 									    &offdiag[cur_mag](0), &offdiag[cur_mag](1), &offdiag[cur_mag](2));
 
+						if (ret == PX4_OK) {
+							sphere_fit_success = true;
+
+						} else {
+							// stop here if fit has previously succeeded
+							if (sphere_fit_success) {
+								break;
+							}
+						}
+
 						PX4_DEBUG("Mag: %d (%d/%d) sphere fit ret=%d, fitness: %.5f, lambda: %.5f, radius: %.3f, offset: [%.3f, %.3f %.3f]",
 							  cur_mag, i, max_iterations, ret, (double)fitness, (double)sphere_lambda, (double)sphere_radius[cur_mag],
 							  (double)sphere[cur_mag](0), (double)sphere[cur_mag](1), (double)sphere[cur_mag](2));
@@ -543,6 +555,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 					PX4_INFO("Mag: %d sphere fitness: %.5f radius: %.4f", cur_mag, (double)fitness, (double)sphere_radius[cur_mag]);
 				}
 
+				bool ellipsoid_fit_success = false;
+
 				if (!sphere_fit_only) {
 					float fitness = 1.0e30f;
 					float ellipsoid_lambda = 1.0f;
@@ -556,6 +570,16 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 									       &diag[cur_mag](0), &diag[cur_mag](1), &diag[cur_mag](2),
 									       &offdiag[cur_mag](0), &offdiag[cur_mag](1), &offdiag[cur_mag](2));
 
+						if (ret == PX4_OK) {
+							ellipsoid_fit_success = true;
+
+						} else {
+							// stop here if fit has previously succeeded
+							if (ellipsoid_fit_success) {
+								break;
+							}
+						}
+
 						PX4_DEBUG("Mag: %d (%d/%d) ellipsoid fit ret=%d, fitness: %.5f, lambda: %.5f, radius: %.3f, offset: [%.3f, %.3f %.3f]",
 							  cur_mag, i, max_iterations, ret, (double)fitness, (double)ellipsoid_lambda, (double)sphere_radius[cur_mag],
 							  (double)sphere[cur_mag](0), (double)sphere[cur_mag](1), (double)sphere[cur_mag](2));
@@ -564,6 +588,12 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 					PX4_INFO("Mag: %d ellipsoid fitness: %.5f", cur_mag, (double)fitness);
 				}
 
+				if (!sphere_fit_success && !ellipsoid_fit_success) {
+					calibration_log_emergency(mavlink_log_pub, "Retry calibration (unable to fit mag #%u)", cur_mag);
+					result = calibrate_return_error;
+					break;
+				}
+
 				result = check_calibration_result(sphere[cur_mag](0), sphere[cur_mag](1), sphere[cur_mag](2),
 								  sphere_radius[cur_mag],
 								  diag[cur_mag](0), diag[cur_mag](1), diag[cur_mag](2),
@@ -577,54 +607,47 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 		}
 	}
 
-	// Print uncalibrated data points
-	if (result == calibrate_return_ok) {
 
-		// DO NOT REMOVE! Critical validation data!
 #if 0
-		printf("RAW DATA:\n--------------------\n");
 
+	// DO NOT REMOVE! Critical validation data!
+	if (result == calibrate_return_ok) {
+		// Print uncalibrated data points
 		for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
-
 			if (worker_data.calibration_counter_total[cur_mag] == 0) {
 				continue;
 			}
 
-			printf("RAW: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
+			printf("MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
+			printf("RAW -> CALIBRATED\n");
+
+			float scale_data[9] {
+				diag[cur_mag](0),    offdiag[cur_mag](0), offdiag[cur_mag](1),
+				offdiag[cur_mag](0),    diag[cur_mag](1), offdiag[cur_mag](2),
+				offdiag[cur_mag](1), offdiag[cur_mag](2),    diag[cur_mag](2)
+			};
+
+			const Matrix3f scale{scale_data};
+			const Vector3f &offset = sphere[cur_mag];
 
 			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
 				float x = worker_data.x[cur_mag][i];
 				float y = worker_data.y[cur_mag][i];
 				float z = worker_data.z[cur_mag][i];
-				printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
-			}
-
-			printf(">>>>>>>\n");
-		}
-
-		printf("CALIBRATED DATA:\n--------------------\n");
 
-		for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
+				// apply calibration
+				const Vector3f cal{scale *(Vector3f{x, y, z} - offset)};
 
-			if (worker_data.calibration_counter_total[cur_mag] == 0) {
-				continue;
-			}
-
-			printf("Calibrated: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
-
-			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
-				float x = worker_data.x[cur_mag][i] - sphere[cur_mag](0);
-				float y = worker_data.y[cur_mag][i] - sphere[cur_mag](1);
-				float z = worker_data.z[cur_mag][i] - sphere[cur_mag](2);
-				printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
+				printf("[%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]\n",
+				       (double)x, (double)y, (double)z,
+				       (double)cal(0), (double)cal(1), (double)cal(2));
 			}
 
 			printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
-			printf(">>>>>>>\n");
 		}
+	}
 
 #endif // DO NOT REMOVE! Critical validation data!
-	}
 
 
 	// Attempt to automatically determine external mag rotations

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -219,9 +219,10 @@ void VehicleMagnetometer::Run()
 
 		} else {
 			sensor_mag_s report;
-			updated[uorb_index] = _sensor_sub[uorb_index].update(&report);
 
-			if (updated[uorb_index]) {
+			while (_sensor_sub[uorb_index].update(&report)) {
+				updated[uorb_index] = true;
+
 				if (_calibration[uorb_index].device_id() != report.device_id) {
 					_calibration[uorb_index].set_external(report.is_external);
 					_calibration[uorb_index].set_device_id(report.device_id);