CollisionPrevention/SF45: move common projection function to library

Author: MaEtUgR

src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt

@@ -41,6 +41,7 @@ px4_add_module(
 	DEPENDS
 		drivers_rangefinder
 		px4_work_queue
+		CollisionPrevention
 	MODULE_CONFIG
 		module.yaml
 	)

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -33,14 +33,15 @@
 
 #include "lightware_sf45_serial.hpp"
 
-#include <inttypes.h>
 #include <fcntl.h>
+#include <float.h>
+#include <inttypes.h>
 #include <termios.h>
+
 #include <lib/crc/crc.h>
 #include <lib/mathlib/mathlib.h>
 #include <matrix/matrix/math.hpp>
-
-#include <float.h>
+#include <ObstacleMath.hpp>
 
 using namespace time_literals;
 using matrix::Quatf;
@@ -660,7 +661,7 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 					}
 				}
 
-				_project_distance_on_horizontal_plane(*distance_m, scaled_yaw, _vehicle_attitude);
+				ObstacleMath::project_distance_on_horizontal_plane(*distance_m, scaled_yaw, _vehicle_attitude);
 
 				if (_current_bin_dist > _obstacle_distance.max_distance) {
 					_current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition
@@ -741,18 +742,6 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 		}
 	}
 }
-void SF45LaserSerial::_project_distance_on_horizontal_plane(float &distance, const int16_t &yaw,
-		const matrix::Quatf &q_world_vehicle)
-{
-	const Quatf q_vehicle_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f)));
-	const Quatf q_world_sensor = q_world_vehicle * q_vehicle_sensor;
-	const Vector3f forward(1.f, 0.f, 0.f);
-	const Vector3f sensor_direction_in_world = q_world_sensor.rotateVector(forward);
-
-	float horizontal_projection_scale = sensor_direction_in_world.xy().norm();
-	horizontal_projection_scale = math::constrain(horizontal_projection_scale, FLT_EPSILON, 1.0f);
-	distance *= horizontal_projection_scale;
-}
 
 uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
 {

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -114,7 +114,6 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
 	bool				_crc_valid{false};
 
 	void 				_handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now);
-	void _project_distance_on_horizontal_plane(float &distance, const int16_t &yaw, const matrix::Quatf &q_world_vehicle);
 	void 				_publish_obstacle_msg(hrt_abstime now);
 	uORB::Subscription 		_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uint64_t			_data_timestamps[BIN_COUNT];

src/lib/collision_prevention/CMakeLists.txt

@@ -31,7 +31,12 @@
 #
 ############################################################################
 
-px4_add_library(CollisionPrevention CollisionPrevention.cpp)
+px4_add_library(CollisionPrevention
+	CollisionPrevention.cpp
+	ObstacleMath.cpp
+)
 target_compile_options(CollisionPrevention PRIVATE -Wno-cast-align) # TODO: fix and enable
+target_include_directories(CollisionPrevention PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(CollisionPrevention PRIVATE mathlib)
 
 px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention)

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2024 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,6 +38,7 @@
  */
 
 #include "CollisionPrevention.hpp"
+#include "ObstacleMath.hpp"
 #include <px4_platform_common/events.h>
 
 using namespace matrix;
@@ -400,18 +401,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 		int lower_bound = (int)round((sensor_yaw_body_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
 		int upper_bound = (int)round((sensor_yaw_body_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
 
-		const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f)));
-
-		const Vector3f forward_vector(1.0f, 0.0f, 0.0f);
-
-		const Quatf q_sensor_rotation = vehicle_attitude * q_sensor;
-
-		const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector);
-
-		const float sensor_dist_scale = rotated_sensor_vector.xy().norm();
-
 		if (distance_reading < distance_sensor.max_distance) {
-			distance_reading = distance_reading * sensor_dist_scale;
+			ObstacleMath::project_distance_on_horizontal_plane(distance_reading, sensor_yaw_body_rad, vehicle_attitude);
 		}
 
 		uint16_t sensor_range = static_cast<uint16_t>(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2024 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/lib/collision_prevention/ObstacleMath.cpp

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ObstacleMath.hpp"
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+
+namespace ObstacleMath
+{
+
+void project_distance_on_horizontal_plane(float &distance, const int16_t yaw, const matrix::Quatf &q_world_vehicle)
+{
+	const Quatf q_vehicle_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f)));
+	const Quatf q_world_sensor = q_world_vehicle * q_vehicle_sensor;
+	const Vector3f forward(1.f, 0.f, 0.f);
+	const Vector3f sensor_direction_in_world = q_world_sensor.rotateVector(forward);
+
+	float horizontal_projection_scale = sensor_direction_in_world.xy().norm();
+	horizontal_projection_scale = math::constrain(horizontal_projection_scale, FLT_EPSILON, 1.0f);
+	distance *= horizontal_projection_scale;
+}
+
+} // ObstacleMath

src/lib/collision_prevention/ObstacleMath.hpp

@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <matrix/math.hpp>
+
+namespace ObstacleMath
+{
+
+/**
+ * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane
+ * @param distance measurement which is scaled down
+ * @param yaw orientation of the measurement on the body horizontal plane
+ * @param q_world_vehicle vehicle attitude quaternion
+ */
+void project_distance_on_horizontal_plane(float &distance, const int16_t yaw, const matrix::Quatf &q_world_vehicle);
+
+} // ObstacleMath