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

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

@ -1182,19 +1182,21 @@ Navigator::start_rtl()
{ {
_do_takeoff = false; _do_takeoff = false;
/* decide if we need climb */
if (_rtl_state == RTL_STATE_NONE) { if (_rtl_state == RTL_STATE_NONE) {
if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
_rtl_state = RTL_STATE_CLIMB; _rtl_state = RTL_STATE_CLIMB;
} else { } else {
_rtl_state = RTL_STATE_RETURN; _rtl_state = RTL_STATE_RETURN;
}
}
if (_reset_loiter_pos) { /* if switching directly to return state, reset altitude setpoint */
if (_rtl_state == RTL_STATE_RETURN) {
_mission_item.altitude_is_relative = false; _mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt; _mission_item.altitude = _global_pos.alt;
} }
}
}
_reset_loiter_pos = true; _reset_loiter_pos = true;
set_rtl_item(); set_rtl_item();
@ -1278,7 +1280,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false; _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; break;
} }