From 7d334ed54f41bd89fcaaddff4091e3eb8901b6b8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 21:52:23 +0100 Subject: navigator: RTL fix, more informative log message --- src/modules/navigator/navigator_main.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) (limited to 'src/modules/navigator/navigator_main.cpp') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2117755ee..8985b28df 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1182,20 +1182,22 @@ Navigator::start_rtl() { _do_takeoff = false; + /* decide if we need climb */ if (_rtl_state == RTL_STATE_NONE) { if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; } else { _rtl_state = RTL_STATE_RETURN; - - if (_reset_loiter_pos) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; - } } } + /* if switching directly to return state, reset altitude setpoint */ + if (_rtl_state == RTL_STATE_RETURN) { + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + } + _reset_loiter_pos = true; set_rtl_item(); } @@ -1278,7 +1280,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", climb_alt - _home_pos.alt); break; } -- cgit v1.2.3