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

navigator: takeoff fixes

This commit is contained in:
Anton Babushkin 2014-01-02 16:44:31 +04:00
parent 6a5d5f7136
commit 911c2bdeee
1 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1053,13 +1053,19 @@ Navigator::advance_mission()
/* do special TAKEOFF handling for VTOL */ /* do special TAKEOFF handling for VTOL */
_need_takeoff = false; _need_takeoff = false;
/* check if we really need takeoff */ /* calculate desired takeoff altitude AMSL */
float wp_alt_amsl = _mission_item_triplet.current.altitude; float takeoff_alt_amsl = _mission_item_triplet.current.altitude;
if (_mission_item_triplet.current.altitude_is_relative) if (_mission_item_triplet.current.altitude_is_relative)
wp_alt_amsl += _home_pos.altitude; takeoff_alt_amsl += _home_pos.altitude;
if (_vstatus.condition_landed || _global_pos.alt - _home_pos.altitude < _parameters.takeoff_alt ||
(_mission_item_triplet.next.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && _global_pos.alt > wp_alt_amsl)) { if (_vstatus.condition_landed) {
/* force TAKEOFF if (landed or near ground) and waypoint altitude is more than current */ /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */
takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt);
}
/* check if we really need takeoff */
if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item_triplet.current.acceptance_radius) {
/* force TAKEOFF if landed or waypoint altitude is more than current */
_do_takeoff = true; _do_takeoff = true;
/* move current mission item to next */ /* move current mission item to next */
@ -1071,6 +1077,8 @@ Navigator::advance_mission()
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_mission_item_triplet.current.altitude = takeoff_alt_amsl;
_mission_item_triplet.current.altitude_is_relative = false;
} }
} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
/* will need takeoff after landing */ /* will need takeoff after landing */