fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 04:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: fix takeoff jump edge case

Browse Source 
In the normal sitl `commander takeoff` case, the takeoff jump was never
actually carried out because the default altitude radius is set to 3m
and the takeoff altitude to ~2m which means that the takeoff waypoint is
"reached" immediately.

This works around this edge case by using the altitude between the home
altitude and takeoff altitude divided by 2 as a acceptance radius.
This commit is contained in:
Julian Oes
2016-10-23 16:25:15 +02:00
committed by Lorenz Meier
parent 72e46e7ed0
commit 4f5fa50efb
1 changed files with 17 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+17 -1
View File
@@ -171,9 +171,25 @@ MissionBlock::is_mission_item_reached()
if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
&& _navigator->get_vstatus()->is_rotary_wing) { && _navigator->get_vstatus()->is_rotary_wing) {
/* We want to avoid the edge case where the acceptance radius is bigger or equal than
* the altitude of the takeoff waypoint above home. Otherwise, we do not really follow
* take-off procedures like leaving the landing gear down. */
float takeoff_alt = _mission_item.altitude_is_relative ?
_mission_item.altitude :
(_mission_item.altitude - _navigator->get_home_position()->alt);
float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius();
/* It should be safe to juse use half of the takoeff_alt as an acceptance radius. */
if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) {
altitude_acceptance_radius = takeoff_alt / 2.0f;
}
/* require only altitude for takeoff for multicopter */ /* require only altitude for takeoff for multicopter */
if (_navigator->get_global_position()->alt > if (_navigator->get_global_position()->alt >
altitude_amsl - _navigator->get_altitude_acceptance_radius()) { altitude_amsl - altitude_acceptance_radius) {
_waypoint_position_reached = true; _waypoint_position_reached = true;
} }
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {