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

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/navigator/mission_block.cpp
View File

@ -199,10 +199,11 @@ 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.
*/
float radius = (fabsf(_mission_item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(_mission_item.loiter_radius) :
_navigator->get_loiter_radius();
const float radius = (fabsf(_mission_item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(_mission_item.loiter_radius) :
_navigator->get_loiter_radius();
if (dist >= 0.0f && dist_xy <= _navigator->get_acceptance_radius(fabsf(radius) * 1.2f)
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_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;
}
// fall through
case NAV_CMD_LOITER_TIME_LIMIT:
// FALLTHROUGH
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
break;