dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

navigator: RTL fix, more informative log message

This commit is contained in:
Anton Babushkin 2014-01-27 21:52:23 +01:00
parent 99bb606a88
commit 7d334ed54f
1 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/navigator/navigator_main.cpp
View File

@ -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;
}