Acceleration vector transform implemented. Accelerometer calibration procedure defined but not implemented yet.

1 files changed, 132 insertions, 82 deletions

diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c
index 4277fe19b..1c6d21ded 100644
--- a/apps/position_estimator_inav/position_estimator_inav_main.c
+++ b/apps/position_estimator_inav/position_estimator_inav_main.c
@@ -71,6 +71,7 @@
 #include "sounds.h"
 #include <drivers/drv_tone_alarm.h>
 #include "kalman_filter_inertial.h"
+#include "acceleration_transform.h"
 
 static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
@@ -147,9 +148,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	mavlink_log_info(mavlink_fd, "[position_estimator_inav] started");
 
+	/* kalman filter K for simulation parameters:
+	 * rate_accel = 200 Hz
+	 * rate_baro = 100 Hz
+	 * err_accel = 1.0 m/s^2
+	 * err_baro = 0.2 m
+	 */
+	static float k[3][2] = {
+			{ 0.0262f, 0.00001f },
+			{ 0.0349f, 0.00191f },
+			{ 0.000259f, 0.618f }
+	};
 	/* initialize values */
-	static float k[3][2] = { { 0.002f, 0.0f }, { 0.001f, 0.00002f }, { 0.0f,
-			0.99f } };
 	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 };
@@ -160,7 +170,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 */
-	float rho0 = 1.293f; /* standard pressure */
 	const static float const_earth_gravity = 9.81f;
 
 	static double lat_current = 0.0d; //[°]] --> 47.0
@@ -203,6 +212,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 	/* FIRST PARAMETER UPDATE */
 	parameters_update(&pos_inav_param_handles, &pos_inav_params);
 	/* END FIRST PARAMETER UPDATE */
+
+	// TODO implement calibration procedure, now put dummy values
+	int16_t accel_offs[3] = { 0, 0, 0 };
+	float accel_T[3][3] = {
+			{ 1.0f, 0.0f, 0.0f },
+			{ 0.0f, 1.0f, 0.0f },
+			{ 0.0f, 0.0f, 1.0f }
+	};
+
 	/* wait until gps signal turns valid, only then can we initialize the projection */
 	if (use_gps) {
 		struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub,
@@ -226,12 +244,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 			static int printcounter = 0;
 			if (printcounter == 100) {
 				printcounter = 0;
-				printf("[pos_est1D] wait for GPS fix type 3\n");
+				printf("[position_estimator_inav] wait for GPS fix type 3\n");
 			}
 			printcounter++;
 		}
 
-		/* get gps value for first initialization */
+		/* get GPS position for first initialization */
 		orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 		lat_current = ((double) (gps.lat)) * 1e-7;
 		lon_current = ((double) (gps.lon)) * 1e-7;
@@ -240,20 +258,38 @@ 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("[pos_est1D] 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;
 	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;
+	hrt_abstime updates_counter_start = hrt_absolute_time();
+	uint32_t updates_counter_len = 1000000;
+	hrt_abstime pub_last = 0;
+	uint32_t pub_interval = 5000;	// limit publish rate to 200 Hz
 
-	/** main loop */
-	struct pollfd fds[3] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN } };
+	/* main loop */
+	struct pollfd fds[3] = {
+			{ .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");
-	static int printatt_counter = 0;
 	while (!thread_should_exit) {
-		int ret = poll(fds, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate
+		bool accelerometer_updated = false;
+		bool baro_updated = false;
+		int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate
 		if (ret < 0) {
 			/* poll error */
-		} else {
+			printf("[position_estimator_inav] subscriptions poll error\n", ret);
+		} else if (ret > 0) {
 			/* parameter update */
 			if (fds[0].revents & POLLIN) {
 				/* read from param to clear updated flag */
@@ -262,7 +298,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 						&update);
 				/* update parameters */
 				parameters_update(&pos_inav_param_handles, &pos_inav_params);
-				//r = pos_inav_params.r;
 			}
 			/* vehicle status */
 			if (fds[1].revents & POLLIN) {
@@ -270,16 +305,41 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 						&vehicle_status);
 			}
 			/* vehicle attitude */
-			//if (fds[2].revents & POLLIN) {
-			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-			//}
-			/* sensor combined */
 			if (fds[2].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+			}
+			/* sensor combined */
+			if (fds[3].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+				if (sensor.accelerometer_counter > accelerometer_counter) {
+					accelerometer_updated = true;
+					accelerometer_counter = sensor.accelerometer_counter;
+					accelerometer_updates++;
+				}
+				if (sensor.baro_counter > baro_counter) {
+					baro_updated = true;
+					baro_counter = sensor.baro_counter;
+					baro_updates++;
+				}
+				// barometric pressure estimation at start up
+				if (!local_flag_baroINITdone && baro_updated) {
+					// mean calculation over several measurements
+					if (baro_loop_cnt < baro_loop_end) {
+						baro_alt0 += sensor.baro_alt_meter;
+						baro_loop_cnt++;
+					} else {
+						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);
+						printf("%s\n", str);
+						mavlink_log_info(mavlink_fd, str);
+					}
+				}
 			}
 			if (use_gps) {
 				/* vehicle GPS position */
-				//if (fds[4].revents & POLLIN) {
+				if (fds[4].revents & POLLIN) {
 					/* new GPS value */
 					orb_copy(ORB_ID(vehicle_gps_position),
 							vehicle_gps_position_sub, &gps);
@@ -289,75 +349,65 @@ 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);
-				//}
-			}
-			// barometric pressure estimation at start up
-			if (!local_flag_baroINITdone) {
-				// mean calculation over several measurements
-				if (baro_loop_cnt < baro_loop_end) {
-					baro_alt0 += sensor.baro_alt_meter;
-					baro_loop_cnt++;
-				} else {
-					baro_alt0 /= (float) (baro_loop_cnt);
-					local_flag_baroINITdone = true;
-					char *baro_m_start = "barometer initialized with alt0 = ";
-					char p0_char[15];
-					sprintf(p0_char, "%8.2f", baro_alt0 / 100);
-					char *baro_m_end = " m";
-					char str[80];
-					strcpy(str, baro_m_start);
-					strcat(str, p0_char);
-					strcat(str, baro_m_end);
-					mavlink_log_info(mavlink_fd, str);
 				}
 			}
 
-			hrt_abstime t = hrt_absolute_time();
-			float dt = (t - last_time) / 1000000.0;
-			last_time = t;
-			/* convert accelerations from UAV frame to NED frame */
-			float accel_NED[3] = { 0.0f, 0.0f, 0.0f };
-			for (int i = 0; i < 3; i++) {
-				for (int j = 0; j < 3; j++) {
-					accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
-				}
-			}
-			accel_NED[2] += const_earth_gravity;
-			/* prediction */
-			kalman_filter_inertial_predict(dt, z_est);
-			/* prepare vectors for kalman filter correction */
-			float z_meas[2];	// pos, accel
-			bool use_z[2] = { false, true };
-			if (local_flag_baroINITdone) {
-				z_meas[0] = baro_alt0 - sensor.baro_alt_meter;	// Z = -alt
-				use_z[0] = true;
+		} /* end of poll return value check */
+
+		hrt_abstime t = hrt_absolute_time();
+		float dt = (t - last_time) / 1000000.0;
+		last_time = t;
+		/* calculate corrected acceleration vector in UAV frame */
+		float accel_corr[3];
+		acceleration_correct(accel_corr, sensor.accelerometer_raw, accel_T, accel_offs);
+		/* transform acceleration vector from UAV frame to NED frame */
+		float accel_NED[3];
+		for (int i = 0; i < 3; i++) {
+			accel_NED[i] = 0.0f;
+			for (int j = 0; j < 3; j++) {
+				accel_NED[i] += att.R[i][j] * accel_corr[j];
 			}
+		}
+		accel_NED[2] += const_earth_gravity;
+		/* kalman filter prediction */
+		kalman_filter_inertial_predict(dt, z_est);
+		/* prepare vectors for kalman filter correction */
+		float z_meas[2];	// position, acceleration
+		bool use_z[2] = { false, false };
+		if (local_flag_baroINITdone && baro_updated) {
+			z_meas[0] = baro_alt0 - sensor.baro_alt_meter;	// Z = -alt
+			use_z[0] = true;
+		}
+		if (accelerometer_updated) {
 			z_meas[1] = accel_NED[2];
+			use_z[1] = true;
+		}
+		if (use_z[0] || use_z[1]) {
 			/* correction */
 			kalman_filter_inertial_update(z_est, z_meas, k, use_z);
-			if (printatt_counter == 100) {
-				printatt_counter = 0;
-				printf("[pos_est_inav] dt = %.6f\n", dt);
-				printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n",
-						att.pitch, att.roll, att.yaw);
-				printf("[pos_est_inav] accel_UAV = %.3f, %.3f, %.3f\n",
-						sensor.accelerometer_m_s2[0],
-						sensor.accelerometer_m_s2[1],
-						sensor.accelerometer_m_s2[2]);
-				printf("[pos_est_inav] accel_NED = %.3f, %.3f, %.3f\n",
-						accel_NED[0], accel_NED[1], accel_NED[2]);
-				/*
-				printf("[pos_est_inav] Rot_matrix: %.3f  %.3f  %.3f\n",
-						att.R[0][0], att.R[0][1], att.R[0][2]);
-				printf("[pos_est_inav] Rot_matrix: %.3f  %.3f  %.3f\n",
-						att.R[1][0], att.R[1][1], att.R[1][2]);
-				printf("[pos_est_inav] Rot_matrix: %.3f  %.3f  %.3f\n",
-						att.R[2][0], att.R[2][1], att.R[2][2]);
-				*/
-				printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n",
-						z_est[0], z_est[1], z_est[2]);
-			}
-			printatt_counter++;
+		}
+		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);
+			updates_counter_start = t;
+		}
+		if (printatt_counter == 100) {
+			printatt_counter = 0;
+			printf("[position_estimator_inav] dt = %.6f\n", dt);
+			printf("[position_estimator_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n",
+					att.pitch, att.roll, att.yaw);
+			printf("[position_estimator_inav] accel_UAV = %.3f, %.3f, %.3f\n",
+					sensor.accelerometer_m_s2[0],
+					sensor.accelerometer_m_s2[1],
+					sensor.accelerometer_m_s2[2]);
+			printf("[position_estimator_inav] accel_NED = %.3f, %.3f, %.3f\n",
+					accel_NED[0], accel_NED[1], accel_NED[2]);
+			printf("[position_estimator_inav] z = %.2f, vz = %.2f, az = %.2f\n",
+					z_est[0], z_est[1], z_est[2]);
+		}
+		printatt_counter++;
+		if (t - pub_last > pub_interval) {
+			pub_last = t;
 			local_pos_est.x = 0.0;
 			local_pos_est.vx = 0.0;
 			local_pos_est.y = 0.0;
@@ -374,11 +424,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 						vehicle_local_position), local_pos_est_pub,
 						&local_pos_est);
 			}
-		} /* end of poll return value check */
+		}
 	}
 
-	printf("[pos_est_inav] exiting.\n");
-	mavlink_log_info(mavlink_fd, "[pos_est_inav] exiting");
+	printf("[position_estimator_inav] exiting.\n");
+	mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting");
 	thread_running = false;
 	return 0;
 }