diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-15 18:03:49 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-15 18:03:49 +0200 |
| commit | e14d3b654b55d81ca1190050311575e98ec931df (patch) | |
| tree | 0d0c07f4e096136216cb916b8c40517254b9f7d7 /src/modules/commander | |
| parent | b71778d7e1d03adf8b1366982f0e86753ac072be (diff) | |
| parent | e9b0ee75015cf3544a3dab2015d7af77f53bd23e (diff) | |
| download | px4-firmware-e14d3b654b55d81ca1190050311575e98ec931df.tar.gz px4-firmware-e14d3b654b55d81ca1190050311575e98ec931df.tar.bz2 px4-firmware-e14d3b654b55d81ca1190050311575e98ec931df.zip | |
Merge remote-tracking branch 'upstream/master' into obcfailsafe
Diffstat (limited to 'src/modules/commander')
| -rw-r--r-- | src/modules/commander/commander.cpp | 14 |
1 files changed, 14 insertions, 0 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4f996b526..28aba759f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -59,6 +59,7 @@ #include <string.h> #include <math.h> #include <poll.h> +#include <float.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> @@ -92,6 +93,7 @@ #include <systemlib/err.h> #include <systemlib/cpuload.h> #include <systemlib/rc_check.h> +#include <geo/geo.h> #include <systemlib/state_table.h> #include <dataman/dataman.h> @@ -903,6 +905,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 = FLT_MAX; + gps_position.epv = FLT_MAX; /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -1391,6 +1395,16 @@ 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_threshold) + && (gps_position.epv < epv_threshold) + && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { + /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ + globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); + } + + /* start mission result check */ orb_check(mission_result_sub, &updated); if (updated) { |