Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

mission block: use dist_xy instead of dist for horizontal pos error check

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2020-12-22 17:37:54 +01:00
parent b3f60b5900
commit a8529672f1
1 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+4 -5
View File
@@ -199,10 +199,11 @@ 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.
*/ */
float radius = (fabsf(_mission_item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(_mission_item.loiter_radius) : const float radius = (fabsf(_mission_item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(_mission_item.loiter_radius) :
_navigator->get_loiter_radius(); _navigator->get_loiter_radius();
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(radius) * 1.2f) if (dist >= 0.0f && dist_xy <= _navigator->get_acceptance_radius(fabsf(radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) { && dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true; _waypoint_position_reached = true;
@@ -598,10 +599,8 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
sp->alt = _navigator->get_global_position()->alt; sp->alt = _navigator->get_global_position()->alt;
} }
// fall through
case NAV_CMD_LOITER_TIME_LIMIT:
// FALLTHROUGH // FALLTHROUGH
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_UNLIMITED: case NAV_CMD_LOITER_UNLIMITED:
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
break; break;