From 4847023cadbb1fd21cc4056ce41e9c43ee4ea8c0 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 14 Mar 2016 22:38:27 +0100 Subject: [PATCH] reset current setpoint when entering RTL --- src/modules/navigator/mission_block.cpp | 18 ++++++++++++++++++ src/modules/navigator/mission_block.h | 2 ++ src/modules/navigator/rtl.cpp | 8 ++++++-- 3 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index d18f395488..fbdd684ec6 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -503,6 +503,24 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio item->origin = ORIGIN_ONBOARD; } +void +MissionBlock::set_current_position_item(struct mission_item_s *item) +{ + item->nav_cmd = NAV_CMD_WAYPOINT; + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + item->altitude_is_relative = false; + item->altitude = _navigator->get_global_position()->alt; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; +} + void MissionBlock::set_idle_item(struct mission_item_s *item) { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 2f69294d1f..e151994525 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -108,6 +108,8 @@ protected: */ void set_land_item(struct mission_item_s *item, bool at_current_location); + void set_current_position_item(struct mission_item_s *item); + /** * Set idle mission item */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 0fc377874d..78062cd88b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -85,6 +85,12 @@ RTL::on_inactive() void RTL::on_activation() { + /* reset starting point so we override what the triplet contained from the previous navigation state */ + _rtl_start_lock = false; + set_current_position_item(&_mission_item); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + /* decide where to enter the RTL procedure when we switch into it */ if (_rtl_state == RTL_STATE_NONE) { /* for safety reasons don't go into RTL if landed */ @@ -96,7 +102,6 @@ RTL::on_activation() } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) { _rtl_state = RTL_STATE_CLIMB; - _rtl_start_lock = false; /* otherwise go straight to return */ } else { @@ -104,7 +109,6 @@ RTL::on_activation() _rtl_state = RTL_STATE_RETURN; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_global_position()->alt; - _rtl_start_lock = false; } }