From 63651da3097441d675b72478b7a7bd4ebb77c31c Mon Sep 17 00:00:00 2001
From: Alvar Martti <alvar.martti@gmail.com>
Date: Fri, 21 Dec 2018 20:03:31 +0200
Subject: [PATCH] FW bug in checking if landing point has been passed
* the bearing difference between the waypoints and aircraft to landing point should be wrap_pi'ed
---
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp | 2 +-
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp | 1 +
2 files changed, 2 insertions(+), 1 deletion(-)
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index 23a0ee172f..724ae6e74b 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -1463,7 +1463,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
float wp_distance_save = wp_distance;
- if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= radians(90.0f)) {
+ if (fabsf(wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) >= radians(90.0f)) {
wp_distance_save = 0.0f;
}
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
index bd52f62e7b..bb53148a72 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -97,6 +97,7 @@ using matrix::Eulerf;
using matrix::Quatf;
using matrix::Vector2f;
using matrix::Vector3f;
+using matrix::wrap_pi;
using uORB::Subscription;
--
GitLab