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

pos estimator inav: check if map projection is initialized

Browse Source
This commit is contained in:
Thomas Gubler
2014-04-24 17:06:10 +02:00
parent 9f2da53f08
commit 618ac319e6
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+4 -4
View File
@@ -549,7 +549,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) { if (updated) {
orb_copy(ORB_ID(home_position), home_position_sub, &home); orb_copy(ORB_ID(home_position), home_position_sub, &home);
if (home.timestamp != home_timestamp) { if (home.timestamp != home_timestamp && map_projection_inited()) {
home_timestamp = home.timestamp; home_timestamp = home.timestamp;
double est_lat, est_lon; double est_lat, est_lon;
@@ -569,7 +569,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.ref_alt = home.alt; local_pos.ref_alt = home.alt;
local_pos.ref_timestamp = home.timestamp; local_pos.ref_timestamp = home.timestamp;
if (ref_inited) { //XXX fix reference update if (ref_inited) {
/* reproject position estimate with new reference */ /* reproject position estimate with new reference */
map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]); map_projection_project(est_lat, est_lon, &x_est[0], &y_est[0]);
z_est[0] = -(est_alt - local_pos.ref_alt); z_est[0] = -(est_alt - local_pos.ref_alt);
@@ -602,7 +602,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
} }
if (gps_valid) { if (gps_valid && map_projection_inited()) {
double lat = gps.lat * 1e-7; double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7; double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3; float alt = gps.alt * 1e-3;
@@ -633,7 +633,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
} }
if (ref_inited) { //XXX fix reference update if (ref_inited) {
/* project GPS lat lon to plane */ /* project GPS lat lon to plane */
float gps_proj[2]; float gps_proj[2];
map_projection_project(lat, lon, &(gps_proj[0]), &(gps_proj[1])); map_projection_project(lat, lon, &(gps_proj[0]), &(gps_proj[1]));