commander: Reject mag samples which are on top of each other

src/modules/commander/mag_calibration.cpp

@@ -65,6 +65,8 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "mag";
 static const unsigned max_mags = 3;
+static constexpr float mag_sphere_radius = 0.2f;
+static const unsigned int calibration_sides = 3;
 
 calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
 
@@ -76,7 +78,7 @@ typedef struct  {
 	unsigned int	calibration_points_perside;
 	unsigned int	calibration_interval_perside_seconds;
 	uint64_t	calibration_interval_perside_useconds;
-	unsigned int	calibration_counter_total;
+	unsigned int	calibration_counter_total[max_mags];
 	bool		side_data_collected[detect_orientation_side_count];
 	float*		x[max_mags];
 	float*		y[max_mags];
@@ -184,6 +186,25 @@ int do_mag_calibration(int mavlink_fd)
 	return result;
 }
 
+static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count)
+{
+	float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
+	//float min_sample_dist = (2.0f * M_PI_F * mag_sphere_radius / max_count) / 2.0f;
+
+	for (size_t i = 0; i < count; i++) {
+		float dx = sx - x[i];
+		float dy = sy - y[i];
+		float dz = sz - z[i];
+		float dist = sqrtf(dx * dx + dy * dy + dz * dz);
+
+		if (dist < min_sample_dist) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
 static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
 {
 	calibrate_return result = calibrate_return_ok;
@@ -286,27 +307,47 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 		int poll_ret = poll(fds, fd_count, 1000);
 		
 		if (poll_ret > 0) {
+
+			int prev_count[max_mags];
+			bool rejected = false;
+
 			for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+
+				prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag];
+
 				if (worker_data->sub_mag[cur_mag] >= 0) {
 					struct mag_report mag;
 
 					orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);
+
+					// Check if this measurement is good to go in
+					rejected = rejected || reject_sample(mag.x, mag.y, mag.z,
+						worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
+						worker_data->calibration_counter_total[cur_mag],
+						calibration_sides * worker_data->calibration_points_perside);
 					
-					worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x;
-					worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y;
-					worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z;
-					
+					worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x;
+					worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y;
+					worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z;
+					worker_data->calibration_counter_total[cur_mag]++;
+				}
+			}
+
+			// Keep calibration of all mags in lockstep
+			if (rejected) {
+				// Reset counts, since one of the mags rejected the measurement
+				for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+					worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
 				}
+			} else {
+				calibration_counter_side++;
+
+				// Progress indicator for side
+				mavlink_and_console_log_info(worker_data->mavlink_fd,
+							     "[cal] %s side calibration: progress <%u>",
+							     detect_orientation_str(orientation),
+							     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
 			}
-			
-			worker_data->calibration_counter_total++;
-			calibration_counter_side++;
-			
-			// Progress indicator for side
-			mavlink_and_console_log_info(worker_data->mavlink_fd,
-						     "[cal] %s side calibration: progress <%u>",
-						     detect_orientation_str(orientation),
-						     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
 		} else {
 			poll_errcount++;
 		}
@@ -336,7 +377,6 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	
 	worker_data.mavlink_fd = mavlink_fd;
 	worker_data.done_count = 0;
-	worker_data.calibration_counter_total = 0;
 	worker_data.calibration_points_perside = 80;
 	worker_data.calibration_interval_perside_seconds = 20;
 	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
@@ -357,9 +397,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 		worker_data.x[cur_mag] = NULL;
 		worker_data.y[cur_mag] = NULL;
 		worker_data.z[cur_mag] = NULL;
+		worker_data.calibration_counter_total[cur_mag] = 0;
 	}
 
-	const unsigned int calibration_sides = 3;
 	const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
 	
 	char str[30];
@@ -438,7 +478,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 				// Mag in this slot is available and we should have values for it to calibrate
 				
 				sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
-							 worker_data.calibration_counter_total,
+							 worker_data.calibration_counter_total[cur_mag],
 							 100, 0.0f,
 							 &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
 							 &sphere_radius[cur_mag]);
@@ -450,6 +490,41 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 			}
 		}
 	}
+
+	// Print uncalibrated data points
+	if (result == calibrate_return_ok) {
+
+		printf("RAW DATA:\n--------------------\n");
+		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+
+			printf("RAW: 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];
+				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 (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+
+			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_x[cur_mag];
+				float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
+				float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
+				printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
+			}
+
+			printf("SPHERE RADIUS: %8.4f", (double)sphere_radius[cur_mag]);
+			printf(">>>>>>>\n");
+		}
+	}
 	
 	// Data points are no longer needed
 	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {