From 7031bb5a6d9f869238bb8ffef6e7d8957d16d903 Mon Sep 17 00:00:00 2001
From: Martina <martina@rivizzigno.it>
Date: Tue, 7 Aug 2018 11:31:18 +0200
Subject: [PATCH] navigator: add yaw_acceptance getter to incorporate feedback
from position controller. The yaw acceptance is defined by the mission item.
If the pos control sets it to NAN, then the yaw at a waypoint is ignored.
---
src/modules/navigator/mission_block.cpp | 2 +-
src/modules/navigator/navigator.h | 10 ++++++++++
src/modules/navigator/navigator_main.cpp | 14 ++++++++++++++
3 files changed, 25 insertions(+), 1 deletion(-)
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 5a0d2fe16c..c144224fe6 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -322,7 +322,7 @@ MissionBlock::is_mission_item_reached()
if ((_navigator->get_vstatus()->is_rotary_wing
|| (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading))
- && PX4_ISFINITE(_mission_item.yaw)) {
+ && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) {
/* check course if defined only for rotary wing except takeoff */
float cog = _navigator->get_vstatus()->is_rotary_wing ? _navigator->get_global_position()->yaw : atan2f(
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 2633522a94..99e70b9e52 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -248,6 +248,16 @@ public:
*/
float get_acceptance_radius(float mission_item_radius);
+ /**
+ * Get the yaw acceptance given the current mission item
+ *
+ * @param mission_item_yaw the yaw to use in case the controller-derived radius is finite
+ *
+ * @return the yaw at which the next waypoint should be used or if the yaw at a waypoint
+ * should be ignored
+ */
+ float get_yaw_acceptance(float mission_item_yaw);
+
orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }
void increment_mission_instance_count() { _mission_result.instance_count++; }
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c55dade689..e3808d7a5a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -946,6 +946,20 @@ Navigator::get_acceptance_radius(float mission_item_radius)
return radius;
}
+float
+Navigator::get_yaw_acceptance(float mission_item_yaw)
+{
+ float yaw = mission_item_yaw;
+
+ const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
+
+ if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && !PX4_ISFINITE(pos_ctrl_status.yaw_acceptance)) {
+ yaw = pos_ctrl_status.yaw_acceptance;
+ }
+
+ return yaw;
+}
+
void
Navigator::load_fence_from_file(const char *filename)
{
--
GitLab