commander: Improve calibration routines for mag

5 files changed, 87 insertions, 15 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 409c2ea00..68c566c07 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -354,7 +354,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 	}
 
 	if (result == OK) {
-		result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker, &worker_data);
+		result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker,
+				&worker_data, 1.6f /* still time */, 0.25f /* still threshold in m/s^2 */);
 	}
 
 	/* close all subscriptions */
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 192b8c727..1b7e6df17 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -230,7 +230,7 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 	return 0;
 }
 
-enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
+enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub, float hold_still_time = 2.0f, float still_threshold = 0.25f)
 {
 	const unsigned ndim = 3;
 	
@@ -242,11 +242,11 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
 	/* EMA time constant in seconds*/
 	float ema_len = 0.5f;
 	/* set "still" threshold to 0.25 m/s^2 */
-	float still_thr2 = powf(0.25f, 2);
+	float still_thr2 = still_threshold * still_threshold;
 	/* set accel error threshold to 5m/s^2 */
 	float accel_err_thr = 5.0f;
 	/* still time required in us */
-	hrt_abstime still_time = 2000000;
+	hrt_abstime still_time = hold_still_time * 1000000;
 	struct pollfd fds[1];
 	fds[0].fd = accel_sub;
 	fds[0].events = POLLIN;
@@ -404,7 +404,9 @@ const char* detect_orientation_str(enum detect_orientation_return orientation)
 int calibrate_from_orientation(int	mavlink_fd,
 			       bool	side_data_collected[detect_orientation_side_count],
 			       calibration_from_orientation_worker_t calibration_worker,
-			       void*	worker_data)
+			       void*	worker_data,
+			       float still_time,
+			       float still_threshold)
 {
 	int result = OK;
 	
@@ -453,7 +455,7 @@ int calibrate_from_orientation(int	mavlink_fd,
 		mavlink_and_console_log_info(mavlink_fd, "pending:%s", pendingStr);
 		
 		mavlink_and_console_log_info(mavlink_fd, "hold the vehicle still on one of the pending sides");
-		enum detect_orientation_return orient = detect_orientation(mavlink_fd, sub_accel);
+		enum detect_orientation_return orient = detect_orientation(mavlink_fd, sub_accel, still_time, still_threshold);
 		
 		if (orient == DETECT_ORIENTATION_ERROR) {
 			orientation_failures++;
@@ -473,7 +475,10 @@ int calibrate_from_orientation(int	mavlink_fd,
 		orientation_failures = 0;
 		
 		// Call worker routine
-		calibration_worker(orient, worker_data);
+		if (OK != calibration_worker(orient, worker_data)) {
+			result = ERROR;
+			break;
+		}
 		
 		mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(orient));
 		
diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h
index 8f34f0204..eb4552d8e 100644
--- a/src/modules/commander/calibration_routines.h
+++ b/src/modules/commander/calibration_routines.h
@@ -80,11 +80,14 @@ static const unsigned detect_orientation_side_count = 6;
  *
  * @param mavlink_fd the MAVLink file descriptor to print output to
  * @param accel_sub Subscription to onboard accel
+ * @param still_time Time interval where the user needs to keep the vehicle still
+ * @param still_threshold Threshold in m/s^2 which defines the nominal still state
  *
  * @return detect_orientation)_return according to orientation when vehicle is still and ready for measurements,
  * DETECT_ORIENTATION_ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
  */
-enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub);
+enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub,
+	float hold_still_time, float still_threshold);
 
 
 /**
@@ -101,4 +104,4 @@ typedef int (*calibration_from_orientation_worker_t)(detect_orientation_return o
 int calibrate_from_orientation(int mavlink_fd,
 			       bool side_data_collected[detect_orientation_side_count],
 			       calibration_from_orientation_worker_t calibration_worker,
-			       void* worker_data);
+			       void* worker_data, float hold_still_time, float still_threshold);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index d67f184ce..27f84116c 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2748,6 +2748,11 @@ void *commander_low_prio_loop(void *arg)
 
 					arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
+					/* instruct user to power-cycle */
+					if (status.condition_system_sensors_initialized && status.vehicle_status == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
+						mavlink_and_console_log_critical("Calibration successful. Power-cycle the system to complete.");
+					}
+
 					break;
 				}
 
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 198acb027..2eaac8d4e 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -50,6 +50,7 @@
 #include <fcntl.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_mag.h>
 #include <mavlink/mavlink_log.h>
@@ -181,14 +182,65 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
 	
 	mavlink_and_console_log_info(worker_data->mavlink_fd, "Rotate vehicle around the detected orientation");
 	mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
-	sleep(2);
 	
-	uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
 	unsigned poll_errcount = 0;
 	
 	calibration_counter_side = 0;
+
+	hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
+	hrt_abstime last_gyro = 0;
+	float gyro_x_integral = 0.0f;
+	float gyro_y_integral = 0.0f;
+	float gyro_z_integral = 0.0f;
+
+	const float gyro_int_thresh_rad = 0.2f;
+
+	int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+
+	while (fabsf(gyro_x_integral) < gyro_int_thresh_rad &&
+		fabsf(gyro_y_integral) < gyro_int_thresh_rad &&
+		fabsf(gyro_z_integral) < gyro_int_thresh_rad) {
+
+		/* abort with timeout */
+		if (hrt_absolute_time() > detection_deadline) {
+			result = ERROR;
+			warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral);
+			mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation.");
+			break;
+		}
+		
+		/* Wait clocking for new data on all mags */
+		struct pollfd fds[1];
+		fds[0].fd = sub_gyro;
+		fds[0].events = POLLIN;
+		size_t fd_count = 1;
+
+		int poll_ret = poll(fds, fd_count, 1000);
+
+		if (poll_ret > 0) {
+			struct gyro_report gyro;
+			orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro);
+
+			/* ensure we have a valid first timestamp */
+			if (last_gyro > 0) {
+
+				/* integrate */
+				float delta_t = (gyro.timestamp - last_gyro) / 1e6f;
+				gyro_x_integral += gyro.x * delta_t;
+				gyro_y_integral += gyro.y * delta_t;
+				gyro_z_integral += gyro.z * delta_t;
+			}
+
+			last_gyro = gyro.timestamp;
+		}
+	}
+
+	close(sub_gyro);
+
+	uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
 	
-	while (hrt_absolute_time() < calibration_deadline &&
+	/* perform the calibration */
+	while (result == OK && hrt_absolute_time() < calibration_deadline &&
 	       calibration_counter_side < worker_data->calibration_points_perside) {
 		
 		// Wait clocking for new data on all mags
@@ -232,11 +284,16 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
 		
 		if (poll_errcount > worker_data->calibration_points_perside * 3) {
 			result = ERROR;
-			mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG);
+			mavlink_and_console_log_critical(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG);
 			break;
 		}
 	}
 	
+	// Abort early
+	if (result != OK) {
+		return result;
+	}
+
 	// Mark the opposite side as collected as well. No need to collect opposite side since it
 	// would generate similar points.
 	detect_orientation_return alternateOrientation = orientation;
@@ -268,7 +325,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
 	
 	worker_data->done_count++;
 	mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count);
-	
+
 	return result;
 }
 
@@ -345,7 +402,8 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 		
 	}
 
-	result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected, mag_calibration_worker, &worker_data);
+	result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected,
+			mag_calibration_worker, &worker_data, 1.0f /* still time */, 1.5f /* still threshold in m/s^2 */);
 	
 	// Close subscriptions
 	for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {