diff options
author | Anton <rk3dov@gmail.com> | 2013-03-28 21:35:38 +0400 |
---|---|---|
committer | Anton <rk3dov@gmail.com> | 2013-03-28 21:35:38 +0400 |
commit | b837692d48e6dcc568bdf7009f515504c0772cf8 (patch) | |
tree | c032355b56067a0dd19b0c906e42ad417259d11e /apps/position_estimator_inav/position_estimator_inav_main.c | |
parent | 83e356fda41b0936d924e4e74673789bfdd0b29c (diff) | |
download | px4-firmware-b837692d48e6dcc568bdf7009f515504c0772cf8.tar.gz px4-firmware-b837692d48e6dcc568bdf7009f515504c0772cf8.tar.bz2 px4-firmware-b837692d48e6dcc568bdf7009f515504c0772cf8.zip |
Test rotation matrix
Diffstat (limited to 'apps/position_estimator_inav/position_estimator_inav_main.c')
-rw-r--r-- | apps/position_estimator_inav/position_estimator_inav_main.c | 110 |
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; |