From 7a43d15096867e8e67aed9203e282bae856643f5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 5 Sep 2024 12:23:57 +1000
Subject: [PATCH] SITL: support more rangefinder orientations

allows for quadplane tailsitter rangefinders
---
 libraries/SITL/SIM_Aircraft.cpp | 34 +++++++++++++++++++++++----------
 1 file changed, 24 insertions(+), 10 deletions(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index e3a10152af5d09..80e7c160682bfd 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -33,6 +33,7 @@
 #include <AP_Filesystem/AP_Filesystem.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_HAL_SITL/HAL_SITL_Class.h>
+#include <AP_Vehicle/AP_Vehicle_Type.h>
 
 using namespace SITL;
 
@@ -505,12 +506,21 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
 float Aircraft::perpendicular_distance_to_rangefinder_surface() const
 {
     switch ((Rotation)sitl->sonar_rot.get()) {
-    case Rotation::ROTATION_PITCH_270:
-        return sitl->state.height_agl;
+#if !APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+        /*
+          for copter and rover we treat these yaw rotations as meaning
+          the sensor is being used for object avoidance. This is a
+          hack and should be removed in the future.  For plane we
+          can't do this as it would break the use of a YAW_180 sensor
+          for tailsitter landings
+         */
     case ROTATION_NONE ... ROTATION_YAW_315:
+        // assume these are avoidance sensors
         return sitl->measure_distance_at_angle_bf(location, sitl->sonar_rot.get()*45);
+#endif
     default:
-        AP_BoardConfig::config_error("Bad simulated sonar rotation");
+        // assume all others are ground sensing rangefinders
+        return sitl->state.height_agl;
     }
 }
 
@@ -543,11 +553,6 @@ float Aircraft::rangefinder_range() const
         }
     }
 
-    if (fabs(roll) >= 90.0 || fabs(pitch) >= 90.0) {
-        // not going to hit the ground....
-        return INFINITY;
-    }
-
     float altitude = perpendicular_distance_to_rangefinder_surface();
 
     // sensor position offset in body frame
@@ -565,8 +570,17 @@ float Aircraft::rangefinder_range() const
         altitude -= relPosSensorEF.z;
     }
 
-    // adjust for apparent altitude with roll
-    altitude /= cosf(radians(roll)) * cosf(radians(pitch));
+    // adjust for rotation based on orientation of the sensor
+    Matrix3f rotmat;
+    sitl->state.quaternion.rotation_matrix(rotmat);
+    Vector3f v{1, 0, 0};
+    v.rotate((Rotation)sitl->sonar_rot.get());
+    v = rotmat * v;
+
+    if (!is_positive(v.z)) {
+        return INFINITY;
+    }
+    altitude /= v.z;
 
     // Add some noise on reading
     altitude += sitl->sonar_noise * rand_float();