Complete calibration implemented

3 files changed, 122 insertions, 65 deletions

diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c
index 812920878..3aba9b403 100644
--- a/apps/position_estimator_inav/acceleration_transform.c
+++ b/apps/position_estimator_inav/acceleration_transform.c
@@ -43,41 +43,31 @@
  *
  * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
  *
+ * Solve separate system for each row of accel_T:
+ *
+ * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
+ *
  * A x = b
  *
- * x = [ accel_T[0][0] ]
- *     | accel_T[0][1] |
- *     | accel_T[0][2] |
- *     | accel_T[1][0] |
- *     | accel_T[1][1] |
- *     | accel_T[1][2] |
- *     | accel_T[2][0] |
- *     | accel_T[2][1] |
- *     [ accel_T[2][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 = [ accel_T[0][i] ]
+ *     | accel_T[1][i] |
+ *     [ accel_T[2][i] ]
+ *
+ * b = [ accel_corr_ref[0][i] ]	// One measurement per axis is enough
+ *     | accel_corr_ref[2][i] |
+ *     [ accel_corr_ref[4][i] ]
+ *
+ * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
+ *
+ * Matrix A is common for all three systems:
+ * A = [ a[0][0]  a[0][1]  a[0][2] ]
+ *     | a[2][0]  a[2][1]  a[2][2] |
+ *     [ a[4][0]  a[4][1]  a[4][2] ]
  *
  * x = A^-1 b
+ *
+ * accel_T = A^-1 * g
+ * g = 9.81
  */
 
 #include "acceleration_transform.h"
@@ -92,10 +82,49 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3],
 	}
 }
 
-void calculate_calibration_values(int16_t accel_raw_ref[6][3],
+int mat_invert3(float src[3][3], float dst[3][3]) {
+	float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
+			src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
+			src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
+	if (det == 0.0)
+		return -1;	// Singular matrix
+	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
+	dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
+	dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
+	dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
+	dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
+	dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
+	dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
+	dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
+	dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
+	return 0;
+}
+
+int calculate_calibration_values(int16_t accel_raw_ref[6][3],
 		float accel_T[3][3], int16_t accel_offs[3], float g) {
+	/* calculate raw offsets */
 	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);
 	}
+	/* fill matrix A for linear equations system*/
+	float mat_A[3][3];
+	memset(mat_A, 0, sizeof(mat_A));
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			float a = (accel_raw_ref[i * 2][j] - accel_offs[j]);
+			mat_A[i][j] = a;
+		}
+	}
+	/* calculate inverse matrix for A */
+	float mat_A_inv[3][3];
+	mat_invert3(mat_A, mat_A_inv);
+	for (int i = 0; i < 3; i++) {
+		/* copy results to accel_T */
+		for (int j = 0; j < 3; j++) {
+			/* simplify matrices mult because b has only one non-zero element == g at index i */
+			accel_T[j][i] = mat_A_inv[j][i] * g;
+		}
+	}
+	return 0;
 }
diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h
index e21aebe73..4d1299db5 100644
--- a/apps/position_estimator_inav/acceleration_transform.h
+++ b/apps/position_estimator_inav/acceleration_transform.h
@@ -10,9 +10,10 @@
 
 #include <unistd.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]);
 
-void calculate_calibration_values(int16_t accel_raw_ref[6][3],
+int 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 552046568..292cf7f21 100644
--- a/apps/position_estimator_inav/position_estimator_inav_main.c
+++ b/apps/position_estimator_inav/position_estimator_inav_main.c
@@ -76,12 +76,15 @@ const static float const_earth_gravity = 9.81f;
 
 __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
 
+void do_accelerometer_calibration();
+
 int position_estimator_inav_thread_main(int argc, char *argv[]);
+
+static void usage(const char *reason);
+
 /**
  * Print the correct usage.
  */
-static void usage(const char *reason);
-
 static void usage(const char *reason) {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
@@ -143,8 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[]) {
 }
 
 void wait_for_input() {
-	printf(
-			"press any key to continue, 'Q' to abort\n");
+	printf("press any key to continue, 'Q' to abort\n");
 	while (true ) {
 		int c = getchar();
 		if (c == 'q' || c == 'Q') {
@@ -179,9 +181,11 @@ void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3],
 			exit(1);
 		}
 	}
-	for (int i = 0; i < 3; i++)
+	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]);
+	}
+	printf("[position_estimator_inav] raw data: [ %i  %i  %i ]\n", accel_avg[0],
+			accel_avg[1], accel_avg[2]);
 }
 
 void do_accelerometer_calibration() {
@@ -191,49 +195,72 @@ void do_accelerometer_calibration() {
 	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]),
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][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]),
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][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]),
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][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]),
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][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]),
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][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]),
+	read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][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,
+	int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs,
 			const_earth_gravity);
-	printf("[position_estimator_inav] accelerometers raw offsets: [ %i  %i  %i ]\n",
+	if (res != 0) {
+		printf(
+				"[position_estimator_inav] calibration parameters calculation error\n");
+		exit(1);
+
+	}
+	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] };
+	printf(
+			"[position_estimator_inav] accelerometers transform matrix:\n  [ %0.6f  %0.6f  %0.6f ]\n  | %0.6f  %0.6f  %0.6f |\n  [ %0.6f  %0.6f  %0.6f ]\n",
+			accel_T[0][0], accel_T[0][1], accel_T[0][2], accel_T[1][0],
+			accel_T[1][1], accel_T[1][2], accel_T[2][0], accel_T[2][1],
+			accel_T[2][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]))) {
+			|| param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2]))
+			|| param_set(param_find("INAV_ACC_T_00"), &(accel_T[0][0]))
+			|| param_set(param_find("INAV_ACC_T_01"), &(accel_T[0][1]))
+			|| param_set(param_find("INAV_ACC_T_02"), &(accel_T[0][2]))
+			|| param_set(param_find("INAV_ACC_T_10"), &(accel_T[1][0]))
+			|| param_set(param_find("INAV_ACC_T_11"), &(accel_T[1][1]))
+			|| param_set(param_find("INAV_ACC_T_12"), &(accel_T[1][2]))
+			|| param_set(param_find("INAV_ACC_T_20"), &(accel_T[2][0]))
+			|| param_set(param_find("INAV_ACC_T_21"), &(accel_T[2][1]))
+			|| param_set(param_find("INAV_ACC_T_22"), &(accel_T[2][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");
+		printf(
+				"[position_estimator_inav] auto-save of parameters to storage failed\n");
 		exit(1);
 	}
 	printf("[position_estimator_inav] calibration done\n");
@@ -253,8 +280,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 	static float x_est[3] = { 0.0f, 0.0f, 0.0f };
 	static float y_est[3] = { 0.0f, 0.0f, 0.0f };
 	static float z_est[3] = { 0.0f, 0.0f, 0.0f };
-	const static float dT_const_120 = 1.0f / 120.0f;
-	const static float dT2_const_120 = 1.0f / 120.0f / 120.0f / 2.0f;
 
 	bool use_gps = false;
 	int baro_loop_cnt = 0;
@@ -308,12 +333,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 				.events = POLLIN } };
 
 		while (gps.fix_type < 3) {
-
-			/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
-			 * this choice is critical, since the vehicle status might not
-			 * actually change, if this app is started after GPS lock was
-			 * aquired.
-			 */
 			if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */
 				if (fds_init[0].revents & POLLIN) {
 					/* Wait for the GPS update to propagate (we have some time) */
@@ -345,15 +364,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 
 	hrt_abstime last_time = 0;
 	thread_running = true;
-	static int printatt_counter = 0;
 	uint32_t accelerometer_counter = 0;
 	uint32_t baro_counter = 0;
 	uint16_t accelerometer_updates = 0;
 	uint16_t baro_updates = 0;
+	uint16_t gps_updates = 0;
+	uint16_t attitude_updates = 0;
 	hrt_abstime updates_counter_start = hrt_absolute_time();
 	uint32_t updates_counter_len = 1000000;
 	hrt_abstime pub_last = hrt_absolute_time();
-	uint32_t pub_interval = 5000;	// limit publish rate to 200 Hz
+	uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
 
 	/* main loop */
 	struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, {
@@ -368,8 +388,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 		int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate
 		if (ret < 0) {
 			/* poll error */
-			if (verbose_mode)
-				printf("[position_estimator_inav] subscriptions poll error\n");
+			printf("[position_estimator_inav] subscriptions poll error\n");
+			thread_should_exit = true;
+			continue;
 		} else if (ret > 0) {
 			/* parameter update */
 			if (fds[0].revents & POLLIN) {
@@ -388,6 +409,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 			/* vehicle attitude */
 			if (fds[2].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+				attitude_updates++;
 			}
 			/* sensor combined */
 			if (fds[3].revents & POLLIN) {
@@ -432,6 +454,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 							((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]),
 							&(local_pos_gps[1]));
 					local_pos_gps[2] = (float) (gps.alt * 1e-3);
+					gps_updates++;
 				}
 			}
 
@@ -476,12 +499,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 			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",
+						"[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n",
 						accelerometer_updates / updates_dt,
-						baro_updates / updates_dt);
+						baro_updates / updates_dt,
+						gps_updates / updates_dt,
+						attitude_updates / updates_dt);
 				updates_counter_start = t;
 				accelerometer_updates = 0;
 				baro_updates = 0;
+				gps_updates = 0;
+				attitude_updates = 0;
 			}
 		}
 		if (t - pub_last > pub_interval) {