From 489a27ca198267abacb05425e67369e22942d302 Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Sun, 10 Jul 2022 12:45:54 +0530 Subject: [PATCH] SITL: fix sim_precland orientation bug --- libraries/SITL/SIM_Precland.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_Precland.cpp b/libraries/SITL/SIM_Precland.cpp index 6998334052..648228f73c 100644 --- a/libraries/SITL/SIM_Precland.cpp +++ b/libraries/SITL/SIM_Precland.cpp @@ -140,15 +140,17 @@ void SIM_Precland::update(const Location &loc, const Vector3d &position) // longitudinal distance of vehicle from the precland device // this is the distance of vehicle from the plane which is passing through precland device origin and perpendicular to axis of cone/cylinder // this plane is the ground plane when the axis has PITCH_90 rotation - const float longitudinal_dist = position_wrt_origin.projected(axis).length(); + Vector3d projection_on_axis = position_wrt_origin.projected(axis); + const float longitudinal_dist = projection_on_axis.length(); // lateral distance of vehicle from the precland device // this is the perpendicular distance of vehicle from the axis of cone/cylinder const float lateral_distance = safe_sqrt(MAX(0, position_wrt_origin.length_squared() - longitudinal_dist*longitudinal_dist)); + // sign of projection's dot product with axis tells if vehicle is in front of beacon // return false if vehicle if vehicle is longitudinally too far away from precland device // for PITCH_90 orientation, longitudinal distance = alt of vehicle - origin_height (in m) - if (longitudinal_dist > _alt_limit) { + if (projection_on_axis.dot(axis) <= 0 || longitudinal_dist > _alt_limit) { _healthy = false; return; }