Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 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_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_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) {
/* 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;
}
} 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
* 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;
}
} else {
@@ -195,7 +197,8 @@ MissionBlock::is_mission_item_reached()
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;
}
}
src/modules/navigator/navigator_params.c
+1
View File
@@ -58,6 +58,7 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
* Acceptance Radius
*
* 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
* @min 0.05