New worker routines for orientation detection

2 files changed, 316 insertions, 1 deletions

diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 290e83a0b..192b8c727 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -38,11 +38,22 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include <stdio.h>
 #include <math.h>
 #include <float.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <mavlink/mavlink_log.h>
+#include <geo/geo.h>
+#include <string.h>
 
 #include "calibration_routines.h"
+#include "calibration_messages.h"
+#include "commander_helper.h"
 
+// FIXME: Fix return codes
+static const int ERROR = -1;
 
 int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
@@ -219,3 +230,264 @@ 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)
+{
+	const unsigned ndim = 3;
+	
+	struct accel_report sensor;
+	/* exponential moving average of accel */
+	float accel_ema[ndim] = { 0.0f };
+	/* max-hold dispersion of accel */
+	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
+	/* 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);
+	/* set accel error threshold to 5m/s^2 */
+	float accel_err_thr = 5.0f;
+	/* still time required in us */
+	hrt_abstime still_time = 2000000;
+	struct pollfd fds[1];
+	fds[0].fd = accel_sub;
+	fds[0].events = POLLIN;
+	
+	hrt_abstime t_start = hrt_absolute_time();
+	/* set timeout to 30s */
+	hrt_abstime timeout = 30000000;
+	hrt_abstime t_timeout = t_start + timeout;
+	hrt_abstime t = t_start;
+	hrt_abstime t_prev = t_start;
+	hrt_abstime t_still = 0;
+	
+	unsigned poll_errcount = 0;
+	
+	while (true) {
+		/* wait blocking for new data */
+		int poll_ret = poll(fds, 1, 1000);
+		
+		if (poll_ret) {
+			orb_copy(ORB_ID(sensor_accel), accel_sub, &sensor);
+			t = hrt_absolute_time();
+			float dt = (t - t_prev) / 1000000.0f;
+			t_prev = t;
+			float w = dt / ema_len;
+			
+			for (unsigned i = 0; i < ndim; i++) {
+				
+				float di = 0.0f;
+				switch (i) {
+					case 0:
+						di = sensor.x;
+						break;
+					case 1:
+						di = sensor.y;
+						break;
+					case 2:
+						di = sensor.z;
+						break;
+				}
+				
+				float d = di - accel_ema[i];
+				accel_ema[i] += d * w;
+				d = d * d;
+				accel_disp[i] = accel_disp[i] * (1.0f - w);
+				
+				if (d > still_thr2 * 8.0f) {
+					d = still_thr2 * 8.0f;
+				}
+				
+				if (d > accel_disp[i]) {
+					accel_disp[i] = d;
+				}
+			}
+			
+			/* still detector with hysteresis */
+			if (accel_disp[0] < still_thr2 &&
+			    accel_disp[1] < still_thr2 &&
+			    accel_disp[2] < still_thr2) {
+				/* is still now */
+				if (t_still == 0) {
+					/* first time */
+					mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still...");
+					t_still = t;
+					t_timeout = t + timeout;
+					
+				} else {
+					/* still since t_still */
+					if (t > t_still + still_time) {
+						/* vehicle is still, exit from the loop to detection of its orientation */
+						break;
+					}
+				}
+				
+			} else if (accel_disp[0] > still_thr2 * 4.0f ||
+				   accel_disp[1] > still_thr2 * 4.0f ||
+				   accel_disp[2] > still_thr2 * 4.0f) {
+				/* not still, reset still start time */
+				if (t_still != 0) {
+					mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
+					sleep(3);
+					t_still = 0;
+				}
+			}
+			
+		} else if (poll_ret == 0) {
+			poll_errcount++;
+		}
+		
+		if (t > t_timeout) {
+			poll_errcount++;
+		}
+		
+		if (poll_errcount > 1000) {
+			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+			return DETECT_ORIENTATION_ERROR;
+		}
+	}
+
+	if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return DETECT_ORIENTATION_TAIL_DOWN;        // [ g, 0, 0 ]
+	}
+	
+	if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return DETECT_ORIENTATION_NOSE_DOWN;        // [ -g, 0, 0 ]
+	}
+	
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return DETECT_ORIENTATION_LEFT;        // [ 0, g, 0 ]
+	}
+	
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return DETECT_ORIENTATION_RIGHT;        // [ 0, -g, 0 ]
+	}
+	
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
+		return DETECT_ORIENTATION_UPSIDE_DOWN;        // [ 0, 0, g ]
+	}
+	
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
+		return DETECT_ORIENTATION_RIGHTSIDE_UP;        // [ 0, 0, -g ]
+	}
+	
+	mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation");
+	
+	return DETECT_ORIENTATION_ERROR;	// Can't detect orientation
+}
+
+const char* detect_orientation_str(enum detect_orientation_return orientation)
+{
+	static const char* rgOrientationStrs[] = {
+		"back",		// tail down
+		"front",	// nose down
+		"left",
+		"right",
+		"up",		// upside-down
+		"down",		// right-side up
+		"error"
+	};
+	
+	return rgOrientationStrs[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)
+{
+	int result = OK;
+	
+	// Setup subscriptions to onboard accel sensor
+	
+	int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
+	if (sub_accel < 0) {
+		mavlink_and_console_log_critical(mavlink_fd, "No onboard accel found, abort");
+		return ERROR;
+	}
+	
+	unsigned orientation_failures = 0;
+	
+	// Rotate through all three main positions
+	while (true) {
+		if (orientation_failures > 10) {
+			result = ERROR;
+			mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_ORIENTATION_TIMEOUT);
+			break;
+		}
+		
+		unsigned int side_complete_count = 0;
+		
+		// Update the number of completed sides
+		for (unsigned i = 0; i < detect_orientation_side_count; i++) {
+			if (side_data_collected[i]) {
+				side_complete_count++;
+			}
+		}
+		
+		if (side_complete_count == detect_orientation_side_count) {
+			// We have completed all sides, move on
+			break;
+		}
+		
+		/* inform user which orientations are still needed */
+		char pendingStr[256];
+		pendingStr[0] = 0;
+		
+		for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) {
+			if (!side_data_collected[cur_orientation]) {
+				strcat(pendingStr, " ");
+				strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
+			}
+		}
+		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);
+		
+		if (orient == DETECT_ORIENTATION_ERROR) {
+			orientation_failures++;
+			mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
+			continue;
+		}
+		
+		/* inform user about already handled side */
+		if (side_data_collected[orient]) {
+			orientation_failures++;
+			mavlink_and_console_log_info(mavlink_fd, "%s side already completed or not needed", detect_orientation_str(orient));
+			mavlink_and_console_log_info(mavlink_fd, "rotate to a pending side");
+			continue;
+		}
+		
+		mavlink_and_console_log_info(mavlink_fd, "%s orientation detected", detect_orientation_str(orient));
+		orientation_failures = 0;
+		
+		// Call worker routine
+		calibration_worker(orient, worker_data);
+		
+		mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(orient));
+		
+		// Note that this side is complete
+		side_data_collected[orient] = true;
+		tune_neutral(true);
+		sleep(1);
+	}
+	
+	if (sub_accel >= 0) {
+		close(sub_accel);
+	}
+	
+	// FIXME: Do we need an orientation complete routine?
+	
+	return result;
+}
diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h
index ba4ca07cc..8f34f0204 100644
--- a/src/modules/commander/calibration_routines.h
+++ b/src/modules/commander/calibration_routines.h
@@ -58,4 +58,47 @@
  */
 int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
-			     float *sphere_radius);
\ No newline at end of file
+			     float *sphere_radius);
+
+// FIXME: Change the name
+static const unsigned max_accel_sens = 3;
+
+// The order of these cannot change since the calibration calculations depend on them in this order
+enum detect_orientation_return {
+	DETECT_ORIENTATION_TAIL_DOWN,
+	DETECT_ORIENTATION_NOSE_DOWN,
+	DETECT_ORIENTATION_LEFT,
+	DETECT_ORIENTATION_RIGHT,
+	DETECT_ORIENTATION_UPSIDE_DOWN,
+	DETECT_ORIENTATION_RIGHTSIDE_UP,
+	DETECT_ORIENTATION_ERROR
+};
+static const unsigned detect_orientation_side_count = 6;
+
+/**
+ * Wait for vehicle to become still and detect it's orientation.
+ *
+ * @param mavlink_fd the MAVLink file descriptor to print output to
+ * @param accel_sub Subscription to onboard accel
+ *
+ * @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);
+
+
+/**
+ * Returns the human readable string representation of the orientation
+ *
+ * @param orientation Orientation to return string for, "error" if buffer is too small
+ *
+ * @return str Returned orientation string
+ */
+const char* detect_orientation_str(enum detect_orientation_return orientation);
+
+typedef int (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, void* worker_data);
+
+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);