diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-06 11:22:18 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-06 11:24:43 +0200 |
commit | 596b06ff2e13b170545b4f13da1c64e66088aedc (patch) | |
tree | 9e0bbdae328101868b63a25651a80af652ac3aa8 /src/modules | |
parent | f24a6187b60f5b9890d9645881b3642be420f5d6 (diff) | |
download | px4-firmware-596b06ff2e13b170545b4f13da1c64e66088aedc.tar.gz px4-firmware-596b06ff2e13b170545b4f13da1c64e66088aedc.tar.bz2 px4-firmware-596b06ff2e13b170545b4f13da1c64e66088aedc.zip |
commander: init gps eph and epv to large values, safer map projection initialization
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/commander.cpp | 6 |
1 files changed, 5 insertions, 1 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c15651ed5..71de33bcc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -58,6 +58,7 @@ #include <string.h> #include <math.h> #include <poll.h> +#include <float.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> @@ -788,6 +789,8 @@ int commander_thread_main(int argc, char *argv[]) int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); + gps_position.eph_m = FLT_MAX; + gps_position.epv_m = FLT_MAX; /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -1127,7 +1130,8 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() && (gps_position.eph_m < eph_epv_threshold) - && (gps_position.epv_m < eph_epv_threshold)) { + && (gps_position.epv_m < eph_epv_threshold) + && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { /* 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()); |