Test rotation matrix

1 files changed, 64 insertions, 46 deletions

diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c
index e28be5b51..774125e83 100644
--- a/apps/position_estimator_inav/position_estimator_inav_main.c
+++ b/apps/position_estimator_inav/position_estimator_inav_main.c
@@ -154,6 +154,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 	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;
 	int baro_loop_end = 70; /* measurement for 1 second */
 	float baro_alt0 = 0.0f; /* to determin while start up */
@@ -200,40 +201,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 	parameters_update(&pos_inav_param_handles, &pos_inav_params);
 	/* END FIRST PARAMETER UPDATE */
 	/* wait until gps signal turns valid, only then can we initialize the projection */
-	struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, .events =
-			POLLIN } };
+	if (use_gps) {
+		struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub,
+				.events = POLLIN } };
 
-	while (gps.fix_type < 3) {
+		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) */
-				usleep(5000);
-				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub,
-						&gps);
+			/* 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) */
+					usleep(5000);
+					orb_copy(ORB_ID(vehicle_gps_position),
+							vehicle_gps_position_sub, &gps);
+				}
 			}
+			static int printcounter = 0;
+			if (printcounter == 100) {
+				printcounter = 0;
+				printf("[pos_est1D] wait for GPS fix type 3\n");
+			}
+			printcounter++;
 		}
-		static int printcounter = 0;
-		if (printcounter == 100) {
-			printcounter = 0;
-			printf("[pos_est1D] wait for GPS fix type 3\n");
-		}
-		printcounter++;
-	}
 
-	/* get gps value 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;
-	alt_current = gps.alt * 1e-3;
-	/* initialize coordinates */
-	map_projection_init(lat_current, lon_current);
-	/* publish global position messages only after first GPS message */
+		/* get gps value 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;
+		alt_current = gps.alt * 1e-3;
+		/* initialize coordinates */
+		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",
 			lat_current, lon_current);
 	uint64_t last_time = 0;
@@ -245,6 +248,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 			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, 5, 20); //wait maximal this 20 ms = 50 Hz minimum rate
 		if (ret < 0) {
@@ -262,7 +267,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 			}
 			/* vehicle status */
 			if (fds[1].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
+				orb_copy(ORB_ID(vehicle_status), vehicle_status_sub,
+						&vehicle_status);
 			}
 			/* vehicle attitude */
 			if (fds[2].revents & POLLIN) {
@@ -270,20 +276,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 			}
 			/* sensor combined */
 			if (fds[3].revents & POLLIN) {
-				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub,
-						&sensor);
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 			}
-			/* vehicle GPS position */
-			if (fds[4].revents & POLLIN) {
-				/* new GPS value */
-				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub,
-						&gps);
-				static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */
-				/* Project gps lat lon (Geographic coordinate system) to plane */
-				map_projection_project(((double) (gps.lat)) * 1e-7,
-						((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]),
-						&(local_pos_gps[1]));
-				local_pos_gps[2] = (float) (gps.alt * 1e-3);
+			if (use_gps) {
+				/* vehicle GPS position */
+				if (fds[4].revents & POLLIN) {
+					/* new GPS value */
+					orb_copy(ORB_ID(vehicle_gps_position),
+							vehicle_gps_position_sub, &gps);
+					static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */
+					/* Project gps lat lon (Geographic coordinate system) to plane */
+					map_projection_project(((double) (gps.lat)) * 1e-7,
+							((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) {
@@ -320,9 +327,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 			z_meas[1] = accel_NED[2];
 			/* correction */
 			kalman_filter_inertial_update(z_est, z_meas, k, use_z);
-			printf("[pos_est_inav] att = %.3f, %.3f, %.3f, %.3f\n", att.q[0], att.q[1], att.q[2], att.q[3]);
-			printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", z_est[0], z_est[1], z_est[2]);
-
+			if (printatt_counter == 100) {
+				printatt_counter = 0;
+				printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n",
+						att.pitch, att.roll, att.yaw);
+				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++;
 			local_pos_est.x = 0.0;
 			local_pos_est.vx = 0.0;
 			local_pos_est.y = 0.0;