fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 03:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Test rotation matrix

Browse Source
This commit is contained in:
Anton
2013-03-28 21:35:38 +04:00
parent 83e356fda4
commit b837692d48
1 changed files with 64 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/position_estimator_inav/position_estimator_inav_main.c
+64 -46
View File
@@ -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;