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

init global map projection when gps is valid

Browse Source
This commit is contained in:
Thomas Gubler
2014-05-05 16:42:52 +02:00
parent 67e3c808d2
commit 6e0690fde1
2 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/geo/geo.c
+1 -1
View File
@@ -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 {
src/modules/commander/commander.cpp
+9 -9
View File
@@ -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);