Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

use NAV_ACC_RAD for vertical waypoint acceptance (#4978)

Browse Source
This commit is contained in:
Daniel Agar
2016-07-04 15:41:16 -04:00
committed by Lorenz Meier
parent a9a3050682
commit 7f89994785
3 changed files with 10 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.fw_defaults
+3 -1
View File
@@ -11,7 +11,9 @@ then
param set RTL_DESCEND_ALT 100 param set RTL_DESCEND_ALT 100
param set RTL_LAND_DELAY -1 param set RTL_LAND_DELAY -1
param set NAV_ACC_RAD 50 # FW uses L1 distance for acceptance radius
# set a smaller NAV_ACC_RAD for vertical acceptance distance
param set NAV_ACC_RAD 10
param set PE_VELNE_NOISE 0.3 param set PE_VELNE_NOISE 0.3
param set PE_VELD_NOISE 0.35 param set PE_VELD_NOISE 0.35
src/modules/navigator/mission_block.cpp
+6 -3
View File
@@ -170,7 +170,8 @@ MissionBlock::is_mission_item_reached()
} }
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* for takeoff mission items use the parameter for the takeoff acceptance radius */ /* for takeoff mission items use the parameter for the takeoff acceptance radius */
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()
&& dist_z <= _navigator->get_default_acceptance_radius()) {
_waypoint_position_reached = true; _waypoint_position_reached = true;
} }
} else if (!_navigator->get_vstatus()->is_rotary_wing && } else if (!_navigator->get_vstatus()->is_rotary_wing &&
@@ -183,7 +184,8 @@ MissionBlock::is_mission_item_reached()
* Therefore the item is marked as reached once the system reaches the loiter * Therefore the item is marked as reached once the system reaches the loiter
* radius (+ some margin). Time inside and turn count is handled elsewhere. * radius (+ some margin). Time inside and turn count is handled elsewhere.
*/ */
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) { if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
&& dist_z <= _navigator->get_default_acceptance_radius()) {
_waypoint_position_reached = true; _waypoint_position_reached = true;
} }
} else { } else {
@@ -195,7 +197,8 @@ MissionBlock::is_mission_item_reached()
mission_acceptance_radius = _navigator->get_acceptance_radius(); mission_acceptance_radius = _navigator->get_acceptance_radius();
} }
if (dist >= 0.0f && dist <= mission_acceptance_radius) { if (dist >= 0.0f && dist <= mission_acceptance_radius
&& dist_z <= _navigator->get_default_acceptance_radius()) {
_waypoint_position_reached = true; _waypoint_position_reached = true;
} }
} }
src/modules/navigator/navigator_params.c
+1
View File
@@ -58,6 +58,7 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
* Acceptance Radius * Acceptance Radius
* *
* Default acceptance radius, overridden by acceptance radius of waypoint if set. * Default acceptance radius, overridden by acceptance radius of waypoint if set.
* For fixed wing NAV_ACC_RAD is the vertical acceptance, as the L1 turning distance is used for horizontal acceptance.
* *
* @unit m * @unit m
* @min 0.05 * @min 0.05