From 017567792bef361c946bbc1010b7655370e3dc9d Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Fri, 8 Jan 2021 16:55:02 +0100
Subject: [PATCH] Navigator: for loiter position acceptance, use L1 distance
and loiter radius
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
src/modules/navigator/mission_block.cpp | 18 +++++++--------
src/modules/navigator/navigator.h | 9 --------
src/modules/navigator/navigator_main.cpp | 29 +++++++-----------------
3 files changed, 16 insertions(+), 40 deletions(-)
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 97d8f6ef88..5bc5ca9146 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -197,12 +197,13 @@ MissionBlock::is_mission_item_reached()
* coordinates with a radius equal to the loiter_radius field. It is not flying
* through the waypoint center.
* 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 + L1 distance. 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();
- if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(radius) * 1.2f)
+ // check if within loiter radius around wp, if yes then set altitude sp to mission item
+ if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
@@ -228,7 +229,8 @@ MissionBlock::is_mission_item_reached()
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
- if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
+ // check if within loiter radius around wp, if yes then set altitude sp to mission item
+ if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
&& dist_z <= _navigator->get_default_altitude_acceptance_radius()) {
// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
@@ -236,7 +238,7 @@ MissionBlock::is_mission_item_reached()
_navigator->set_position_setpoint_triplet_updated();
}
- } else if (dist >= 0.f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
+ } else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
@@ -298,13 +300,9 @@ MissionBlock::is_mission_item_reached()
_time_wp_reached = now;
} else {
- /* for normal mission items used their acceptance radius */
- float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
+ /*normal mission items */
- /* if set to zero use the default instead */
- if (mission_acceptance_radius < NAV_EPSILON_POSITION) {
- mission_acceptance_radius = _navigator->get_acceptance_radius();
- }
+ float mission_acceptance_radius = _navigator->get_acceptance_radius();
/* for vtol back transition calculate acceptance radius based on time and ground speed */
if (_mission_item.vtol_back_transition
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 4aa4f16da0..af96b4ce45 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -246,15 +246,6 @@ public:
*/
void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; }
- /**
- * Get the acceptance radius given the mission item preset radius
- *
- * @param mission_item_radius the radius to use in case the controller-derived radius is smaller
- *
- * @return the distance at which the next waypoint should be used
- */
- float get_acceptance_radius(float mission_item_radius);
-
/**
* Get the yaw acceptance given the current mission item
*
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index cb48a9d99b..90c962335b 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -873,12 +873,6 @@ Navigator::get_default_acceptance_radius()
return _param_nav_acc_rad.get();
}
-float
-Navigator::get_acceptance_radius()
-{
- return get_acceptance_radius(_param_nav_acc_rad.get());
-}
-
float
Navigator::get_default_altitude_acceptance_radius()
{
@@ -996,24 +990,17 @@ Navigator::get_cruising_throttle()
}
float
-Navigator::get_acceptance_radius(float mission_item_radius)
+Navigator::get_acceptance_radius()
{
- float radius = mission_item_radius;
-
- // XXX only use navigation capabilities for now
- // when in fixed wing mode
- // this might need locking against a commanded transition
- // so that a stale _vstatus doesn't trigger an accepted mission item.
-
- const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
+ if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+ // return the value specified in the parameter NAV_ACC_RAD
+ return get_default_acceptance_radius();
- if (_vstatus.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
- && (pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp)
- && pos_ctrl_status.acceptance_radius > radius) {
- radius = pos_ctrl_status.acceptance_radius;
+ } else {
+ // return the max of NAV_ACC_RAD and the controller acceptance radius (e.g. L1 distance)
+ const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
+ return math::max(pos_ctrl_status.acceptance_radius, get_default_acceptance_radius());
}
-
- return radius;
}
float