diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-27 21:52:23 +0100 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-27 21:52:23 +0100 |
| commit | 7d334ed54f41bd89fcaaddff4091e3eb8901b6b8 (patch) | |
| tree | a426168bc4e0eb8f1a3b89b1eadca125439c716f /src/modules/navigator/navigator_main.cpp | |
| parent | 99bb606a88177ff7ef3478b17c8f3652e6f4cb3b (diff) | |
| download | px4-firmware-7d334ed54f41bd89fcaaddff4091e3eb8901b6b8.tar.gz px4-firmware-7d334ed54f41bd89fcaaddff4091e3eb8901b6b8.tar.bz2 px4-firmware-7d334ed54f41bd89fcaaddff4091e3eb8901b6b8.zip | |
navigator: RTL fix, more informative log message
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 14 |
1 files changed, 8 insertions, 6 deletions
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; } |