diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/commander.cpp | 14 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 5 |
2 files changed, 12 insertions, 7 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2db30b066..54834cf9b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -87,6 +87,7 @@ #include <systemlib/err.h> #include <systemlib/cpuload.h> #include <systemlib/rc_check.h> +#include <geo/geo.h> #include "px4_custom_mode.h" #include "commander_helper.h" @@ -366,7 +367,7 @@ static orb_advert_t status_pub; transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; - + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); @@ -375,7 +376,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armed } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); } - + return arming_res; } @@ -571,6 +572,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->valid = true; + /* 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); @@ -958,6 +962,9 @@ 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); @@ -1345,6 +1352,9 @@ 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); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 14c568c46..12fd35a0a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -822,11 +822,6 @@ Navigator::task_main() _global_pos_valid = _vstatus.condition_global_position_valid; - /* set reference for map _projection if global pos is valid and home position is valid and we have not done so already */ - if (!map_projection_global_initialized() && _global_pos_valid && _home_pos.valid) { - map_projection_global_init(_home_pos.lat, _home_pos.lon, _home_pos.timestamp); - } - /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; |