diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-05 16:42:52 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-05 16:44:02 +0200 |
| commit | 6e0690fde1073649a4ef201671ca947cb83edc37 (patch) | |
| tree | 75e1a077601d2f76545574bfb464fd8f9d82193f | |
| parent | 67e3c808d24773c7c4bd4608866de1edb617cb04 (diff) | |
| download | px4-firmware-6e0690fde1073649a4ef201671ca947cb83edc37.tar.gz px4-firmware-6e0690fde1073649a4ef201671ca947cb83edc37.tar.bz2 px4-firmware-6e0690fde1073649a4ef201671ca947cb83edc37.zip | |
init global map projection when gps is valid
| -rw-r--r-- | src/lib/geo/geo.c | 2 | ||||
| -rw-r--r-- | src/modules/commander/commander.cpp | 18 |
2 files changed, 10 insertions, 10 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 438e354df..10a518908 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -79,7 +79,7 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference __EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - if (strcmp("navigator", getprogname() == 0)) { + if (strcmp("commander", getprogname() == 0)) { return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); } else { diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bc3bdb782..f8f215cd7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -570,9 +570,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); - /* set reference for map _projection */ - map_projection_global_init(home->lat, home->lon, hrt_absolute_time()); - /* announce new home position */ if (*home_pub > 0) { orb_publish(ORB_ID(home_position), *home_pub, home); @@ -959,9 +956,6 @@ int commander_thread_main(int argc, char *argv[]) warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - /* set reference for map _projection */ - map_projection_global_init(home.lat, home.lon, hrt_absolute_time()); - /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); @@ -1130,6 +1124,15 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); } + /* Initialize map projection if gps is valid */ + if (!map_projection_global_initialized() + && (gps_position.eph < eph_epv_threshold) + && (gps_position.epv < eph_epv_threshold)) { + /* set reference for map _projection */ + map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time()); + + } + /* start RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ @@ -1348,9 +1351,6 @@ int commander_thread_main(int argc, char *argv[]) warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - /* set reference for map _projection */ - map_projection_global_init(home.lat, home.lon, hrt_absolute_time()); - /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); |