Accelerometers offsets calibration implemented

3 files changed, 182 insertions, 63 deletions

diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c
index bd933cab4..812920878 100644
--- a/apps/position_estimator_inav/acceleration_transform.c
+++ b/apps/position_estimator_inav/acceleration_transform.c
@@ -17,23 +17,23 @@
  *
  * * * * Calibration * * *
  *
- * Tests
- *
- * accel_corr_x_p = [  g  0  0 ]^T	// positive x
- * accel_corr_x_n = [ -g  0  0 ]^T	// negative x
- * accel_corr_y_p = [  0  g  0 ]^T	// positive y
- * accel_corr_y_n = [  0 -g  0 ]^T	// negative y
- * accel_corr_z_p = [  0  0  g ]^T	// positive z
- * accel_corr_z_n = [  0  0 -g ]^T	// negative z
- *
- * 6 tests * 3 axes = 18 equations
+ * Reference vectors
+ * accel_corr_ref[6][3] = [  g  0  0 ]     // positive x
+ *                        | -g  0  0 |     // negative x
+ *                        |  0  g  0 |     // positive y
+ *                        |  0 -g  0 |     // negative y
+ *                        |  0  0  g |     // positive z
+ *                        [  0  0 -g ]     // negative z
+ * accel_raw_ref[6][3]
+ *
+ * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5
+ *
+ * 6 reference vectors * 3 axes = 18 equations
  * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
  *
  * Find accel_offs
  *
- * accel_offs = (accel_raw_x_p + accel_raw_x_n +
- * 				 accel_raw_y_p + accel_raw_y_n +
- * 				 accel_raw_z_p + accel_raw_z_n) / 6
+ * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
  *
  *
  * Find accel_T
@@ -41,9 +41,8 @@
  * 9 unknown constants
  * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
  *
- * accel_corr[i] = accel_T[i][0] * (accel_raw[0] - accel_offs[0]) +
- * 				   accel_T[i][1] * (accel_raw[1] - accel_offs[1]) +
- * 				   accel_T[i][2] * (accel_raw[2] - accel_offs[2])
+ * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
+ *
  * A x = b
  *
  * x = [ accel_T[0][0] ]
@@ -56,34 +55,35 @@
  *     | accel_T[2][1] |
  *     [ accel_T[2][2] ]
  *
- * b = [ accel_corr_x_p[0] ]	// One measurement per axis is enough
- *     | accel_corr_x_p[1] |
- *     | accel_corr_x_p[2] |
- *     | accel_corr_y_p[0] |
- *     | accel_corr_y_p[1] |
- *     | accel_corr_y_p[2] |
- *     | accel_corr_z_p[0] |
- *     | accel_corr_z_p[1] |
- *     [ accel_corr_z_p[2] ]
- *
- * a_x[i] = accel_raw_x_p[i] - accel_offs[i]	// Measurement for X axis
- * ...
- * A = [ a_x[0]  a_x[1]  a_x[2]       0       0       0       0       0       0 ]
- *     |      0       0       0  a_x[0]  a_x[1]  a_x[2]       0       0       0 |
- *     |      0       0       0       0       0       0  a_x[0]  a_x[1]  a_x[2] |
- *     | a_y[0]  a_y[1]  a_y[2]       0       0       0       0       0       0 |
- *     |      0       0       0  a_y[0]  a_y[1]  a_y[2]       0       0       0 |
- *     |      0       0       0       0       0       0  a_y[0]  a_y[1]  a_y[2] |
- *     | a_z[0]  a_z[1]  a_z[2]       0       0       0       0       0       0 |
- *     |      0       0       0  a_z[0]  a_z[1]  a_z[2]       0       0       0 |
- *     [      0       0       0       0       0       0  a_z[0]  a_z[1]  a_z[2] ]
+ * b = [ accel_corr_ref[0][0] ]	// One measurement per axis is enough
+ *     | accel_corr_ref[0][1] |
+ *     | accel_corr_ref[0][2] |
+ *     | accel_corr_ref[2][0] |
+ *     | accel_corr_ref[2][1] |
+ *     | accel_corr_ref[2][2] |
+ *     | accel_corr_ref[4][0] |
+ *     | accel_corr_ref[4][1] |
+ *     [ accel_corr_ref[4][2] ]
+ *
+ * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0...5, j = 0...2
+ *
+ * A = [ a[0][0]  a[0][1]  a[0][2]        0        0        0        0        0        0 ]
+ *     |       0        0        0  a[0][0]  a[0][1]  a[0][2]        0        0        0 |
+ *     |       0        0        0        0        0        0  a[0][0]  a[0][1]  a[0][2] |
+ *     | a[1][0]  a[1][1]  a[1][2]        0        0        0        0        0        0 |
+ *     |       0        0        0  a[1][0]  a[1][1]  a[1][2]        0        0        0 |
+ *     |       0        0        0        0        0        0  a[1][0]  a[1][1]  a[1][2] |
+ *     | a[2][0]  a[2][1]  a[2][2]        0        0        0        0        0        0 |
+ *     |       0        0        0  a[2][0]  a[2][1]  a[2][2]        0        0        0 |
+ *     [       0        0        0        0        0        0  a[2][0]  a[2][1]  a[2][2] ]
  *
  * x = A^-1 b
  */
 
 #include "acceleration_transform.h"
 
-void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]) {
+void acceleration_correct(float accel_corr[3], int16_t accel_raw[3],
+		float accel_T[3][3], int16_t accel_offs[3]) {
 	for (int i = 0; i < 3; i++) {
 		accel_corr[i] = 0.0f;
 		for (int j = 0; j < 3; j++) {
@@ -91,3 +91,11 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel
 		}
 	}
 }
+
+void calculate_calibration_values(int16_t accel_raw_ref[6][3],
+		float accel_T[3][3], int16_t accel_offs[3], float g) {
+	for (int i = 0; i < 3; i++) {
+		accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i])
+				+ (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2);
+	}
+}
diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h
index 26bf0d68f..e21aebe73 100644
--- a/apps/position_estimator_inav/acceleration_transform.h
+++ b/apps/position_estimator_inav/acceleration_transform.h
@@ -12,4 +12,7 @@
 
 void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]);
 
+void calculate_calibration_values(int16_t accel_raw_ref[6][3],
+		float accel_T[3][3], int16_t accel_offs[3], float g);
+
 #endif /* ACCELERATION_TRANSFORM_H_ */
diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c
index b8ef1f76a..552046568 100644
--- a/apps/position_estimator_inav/position_estimator_inav_main.c
+++ b/apps/position_estimator_inav/position_estimator_inav_main.c
@@ -72,6 +72,7 @@ static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
 static int position_estimator_inav_task; /**< Handle of deamon task / thread */
 static bool verbose_mode = false;
+const static float const_earth_gravity = 9.81f;
 
 __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
 
@@ -84,19 +85,19 @@ static void usage(const char *reason);
 static void usage(const char *reason) {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr,
-			"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
-	exit(1);
-}
+		fprintf(stderr,
+				"usage: position_estimator_inav {start|stop|status|calibrate} [-v]\n\n");
+		exit(1);
+	}
 
-/**
- * The position_estimator_inav_thread only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
+		/**
+		 * The position_estimator_inav_thread only briefly exists to start
+		 * the background job. The stack size assigned in the
+		 * Makefile does only apply to this management task.
+		 *
+		 * The actual stack size should be set in the call
+		 * to task_create().
+		 */
 int position_estimator_inav_main(int argc, char *argv[]) {
 	if (argc < 1)
 		usage("missing command");
@@ -132,10 +133,112 @@ int position_estimator_inav_main(int argc, char *argv[]) {
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "calibrate")) {
+		do_accelerometer_calibration();
+		exit(0);
+	}
+
 	usage("unrecognized command");
 	exit(1);
 }
 
+void wait_for_input() {
+	printf(
+			"press any key to continue, 'Q' to abort\n");
+	while (true ) {
+		int c = getchar();
+		if (c == 'q' || c == 'Q') {
+			exit(0);
+		} else {
+			return;
+		}
+	}
+}
+
+void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3],
+		int samples) {
+	printf("[position_estimator_inav] measuring...\n");
+	struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
+	int count = 0;
+	int32_t accel_sum[3] = { 0, 0, 0 };
+	while (count < samples) {
+		int poll_ret = poll(fds, 1, 1000);
+		if (poll_ret == 1) {
+			struct sensor_combined_s sensor;
+			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+			accel_sum[0] += sensor.accelerometer_raw[0];
+			accel_sum[1] += sensor.accelerometer_raw[1];
+			accel_sum[2] += sensor.accelerometer_raw[2];
+			count++;
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			printf("[position_estimator_inav] no accelerometer data for 1s\n");
+			exit(1);
+		} else {
+			printf("[position_estimator_inav] poll error\n");
+			exit(1);
+		}
+	}
+	for (int i = 0; i < 3; i++)
+		accel_avg[i] = (accel_sum[i] + count / 2) / count;
+	printf("[position_estimator_inav] raw data: [ %i  %i  %i ]\n", accel_avg[0], accel_avg[1], accel_avg[2]);
+}
+
+void do_accelerometer_calibration() {
+	printf("[position_estimator_inav] calibration started\n");
+	const int calibration_samples = 1000;
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int16_t accel_raw_ref[6][3];	// Reference measurements
+	printf("[position_estimator_inav] place vehicle level, ");
+	wait_for_input();
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]),
+			calibration_samples);
+	printf("[position_estimator_inav] place vehicle on it's back, ");
+	wait_for_input();
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]),
+			calibration_samples);
+	printf("[position_estimator_inav] place vehicle on right side, ");
+	wait_for_input();
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]),
+			calibration_samples);
+	printf("[position_estimator_inav] place vehicle on left side, ");
+	wait_for_input();
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]),
+			calibration_samples);
+	printf("[position_estimator_inav] place vehicle nose down, ");
+	wait_for_input();
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]),
+			calibration_samples);
+	printf("[position_estimator_inav] place vehicle nose up, ");
+	wait_for_input();
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]),
+			calibration_samples);
+	close(sensor_combined_sub);
+	printf("[position_estimator_inav] reference data collection done\n");
+
+	int16_t accel_offs[3];
+	float accel_T[3][3];
+	calculate_calibration_values(accel_raw_ref, accel_T, accel_offs,
+			const_earth_gravity);
+	printf("[position_estimator_inav] accelerometers raw offsets: [ %i  %i  %i ]\n",
+			accel_offs[0], accel_offs[1], accel_offs[2]);
+	int32_t accel_offs_int32[3] = { accel_offs[0], accel_offs[1], accel_offs[2] };
+
+	if (param_set(param_find("INAV_ACC_OFFS_0"), &(accel_offs_int32[0]))
+			|| param_set(param_find("INAV_ACC_OFFS_1"), &(accel_offs_int32[1]))
+			|| param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2]))) {
+		printf("[position_estimator_inav] setting parameters failed\n");
+		exit(1);
+	}
+	/* auto-save to EEPROM */
+	int save_ret = param_save_default();
+	if (save_ret != 0) {
+		printf("[position_estimator_inav] auto-save of parameters to storage failed\n");
+		exit(1);
+	}
+	printf("[position_estimator_inav] calibration done\n");
+}
+
 /****************************************************************************
  * main
  ****************************************************************************/
@@ -157,7 +260,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 	int baro_loop_cnt = 0;
 	int baro_loop_end = 70; /* measurement for 1 second */
 	float baro_alt0 = 0.0f; /* to determin while start up */
-	const static float const_earth_gravity = 9.81f;
 
 	static double lat_current = 0.0d; //[°]] --> 47.0
 	static double lon_current = 0.0d; //[°]] -->8.5
@@ -237,7 +339,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 		map_projection_init(lat_current, lon_current);
 		/* publish global position messages only after first GPS message */
 	}
-	printf("[position_estimator_inav] initialized projection with: lat: %.10f,  lon:%.10f\n",
+	printf(
+			"[position_estimator_inav] initialized projection with: lat: %.10f,  lon:%.10f\n",
 			lat_current, lon_current);
 
 	hrt_abstime last_time = 0;
@@ -253,13 +356,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 	uint32_t pub_interval = 5000;	// limit publish rate to 200 Hz
 
 	/* main loop */
-	struct pollfd fds[5] = {
-			{ .fd = parameter_update_sub, .events = POLLIN },
-			{ .fd = vehicle_status_sub, .events = POLLIN },
-			{ .fd = vehicle_attitude_sub, .events = POLLIN },
-			{ .fd = sensor_combined_sub, .events = POLLIN },
-			{ .fd = vehicle_gps_position_sub, .events = POLLIN }
-	};
+	struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, {
+			.fd = vehicle_status_sub, .events = POLLIN }, { .fd =
+			vehicle_attitude_sub, .events = POLLIN }, { .fd =
+			sensor_combined_sub, .events = POLLIN }, { .fd =
+			vehicle_gps_position_sub, .events = POLLIN } };
 	printf("[position_estimator_inav] main loop started\n");
 	while (!thread_should_exit) {
 		bool accelerometer_updated = false;
@@ -311,7 +412,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 						baro_alt0 /= (float) (baro_loop_cnt);
 						local_flag_baroINITdone = true;
 						char str[80];
-						sprintf(str, "[position_estimator_inav] barometer initialized with alt0 = %.2f", baro_alt0);
+						sprintf(str,
+								"[position_estimator_inav] baro_alt0 = %.2f",
+								baro_alt0);
 						printf("%s\n", str);
 						mavlink_log_info(mavlink_fd, str);
 					}
@@ -339,7 +442,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 		last_time = t;
 		/* calculate corrected acceleration vector in UAV frame */
 		float accel_corr[3];
-		acceleration_correct(accel_corr, sensor.accelerometer_raw, pos_inav_params.acc_T, pos_inav_params.acc_offs);
+		acceleration_correct(accel_corr, sensor.accelerometer_raw,
+				pos_inav_params.acc_T, pos_inav_params.acc_offs);
 		/* transform acceleration vector from UAV frame to NED frame */
 		float accel_NED[3];
 		for (int i = 0; i < 3; i++) {
@@ -364,13 +468,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 		}
 		if (use_z[0] || use_z[1]) {
 			/* correction */
-			kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, use_z);
+			kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k,
+					use_z);
 		}
 		if (verbose_mode) {
 			/* print updates rate */
 			if (t - updates_counter_start > updates_counter_len) {
 				float updates_dt = (t - updates_counter_start) * 0.000001f;
-				printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt);
+				printf(
+						"[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n",
+						accelerometer_updates / updates_dt,
+						baro_updates / updates_dt);
 				updates_counter_start = t;
 				accelerometer_updates = 0;
 				baro_updates = 0;