SENS: SF45: Scale the measured distance with pitch and roll

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,19 +33,18 @@
 
 #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 <float.h>
+#include <matrix/matrix/math.hpp>
+#include <ObstacleMath.hpp>
 
 using namespace time_literals;
 
-/* Configuration Constants */
-
-
 SF45LaserSerial::SF45LaserSerial(const char *port) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
@@ -136,8 +135,6 @@ int SF45LaserSerial::measure()
 
 int SF45LaserSerial::collect()
 {
-	float distance_m = -1.0f;
-
 	if (_sensor_state == STATE_UNINIT) {
 
 		perf_begin(_sample_perf);
@@ -196,8 +193,7 @@ int SF45LaserSerial::collect()
 		sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM);
 
 		if (_crc_valid) {
-			sf45_process_replies(&distance_m);
-			PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));
+			sf45_process_replies();
 			perf_end(_sample_perf);
 			return PX4_OK;
 		}
@@ -592,7 +588,7 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8
 	}
 }
 
-void SF45LaserSerial::sf45_process_replies(float *distance_m)
+void SF45LaserSerial::sf45_process_replies()
 {
 	switch (rx_field.msg_id) {
 	case SF_DISTANCE_DATA_CM: {
@@ -640,20 +636,34 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 			}
 
 			// Convert to meters for the debug message
-			*distance_m = raw_distance * SF45_SCALE_FACTOR;
+			float distance_m = raw_distance * SF45_SCALE_FACTOR;
 			_current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist;
 
 			uint8_t current_bin = sf45_convert_angle(scaled_yaw);
 
 			if (current_bin != _previous_bin) {
 				PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin,
-					  (double)*distance_m);
+					  (double)distance_m);
+
+				if (_vehicle_attitude_sub.updated()) {
+					vehicle_attitude_s vehicle_attitude;
+
+					if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+						_vehicle_attitude = matrix::Quatf(vehicle_attitude.q);
+					}
+				}
+
+				float current_bin_dist = static_cast<float>(_current_bin_dist);
+				float scaled_yaw_rad = math::radians(static_cast<float>(scaled_yaw));
+				ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_rad, _vehicle_attitude);
+				_current_bin_dist = static_cast<uint16_t>(current_bin_dist);
 
 				if (_current_bin_dist > _obstacle_distance.max_distance) {
 					_current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition
 				}
 
 				hrt_abstime now = hrt_absolute_time();
+
 				_handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now);
 
 				_publish_obstacle_msg(now);
@@ -727,6 +737,7 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 		}
 	}
 }
+
 uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
 {
 	uint8_t mapped_sector = 0;

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -49,7 +49,9 @@
 #include <lib/perf/perf_counter.h>
 
 #include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/vehicle_attitude.h>
 
 #include "sf45_commands.h"
 
@@ -92,7 +94,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
 	void				sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id);
 	void				sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
 	uint16_t			sf45_format_crc(uint16_t crc, uint8_t data_value);
-	void				sf45_process_replies(float *data);
+	void				sf45_process_replies();
 	uint8_t				sf45_convert_angle(const int16_t yaw);
 	float				sf45_wrap_360(float f);
 
@@ -113,6 +115,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
 
 	void 				_handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now);
 	void 				_publish_obstacle_msg(hrt_abstime now);
+	uORB::Subscription 		_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uint64_t			_data_timestamps[BIN_COUNT];
 
 
@@ -141,6 +144,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
 	int32_t				_orient_cfg{0};
 	uint8_t				_previous_bin{0};
 	uint16_t			_current_bin_dist{UINT16_MAX};
+	matrix::Quatf			_vehicle_attitude{};
 
 	// end of SF45/B data members
 

src/lib/collision_prevention/CMakeLists.txt

@@ -31,7 +31,13 @@
 #
 ############################################################################
 
-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)
+px4_add_unit_gtest(SRC ObstacleMathTest.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 float 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 float yaw, const matrix::Quatf &q_world_vehicle);
+
+} // ObstacleMath

src/lib/collision_prevention/ObstacleMathTest.cpp

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+#include <matrix/math.hpp>
+#include "ObstacleMath.hpp"
+
+using namespace matrix;
+
+TEST(ObstacleMathTest, ProjectDistanceOnHorizontalPlane)
+{
+	// standard vehicle orientation inputs
+	Quatf vehicle_pitch_up_45(Eulerf(0.0f, M_PI_4_F, 0.0f));
+	Quatf vehicle_roll_right_45(Eulerf(M_PI_4_F, 0.0f, 0.0f));
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	float distance = 1.0f;
+	float sensor_orientation = 0; // radians (forward facing)
+
+	// WHEN: we project the distance onto the horizontal plane
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45);
+
+	// THEN: the distance should be scaled correctly
+	float expected_scale    = sqrtf(2) / 2;
+	float expected_distance = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	distance = 1.0f;
+
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45);
+
+	// THEN: the distance should be scaled correctly
+	expected_scale     = 1.f;
+	expected_distance  = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	distance = 1.0f;
+	sensor_orientation = M_PI_2_F; // radians (right facing)
+
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45);
+
+	// THEN: the distance should be scaled correctly
+	expected_scale     = sqrtf(2) / 2;
+	expected_distance  = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	distance = 1.0f;
+
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45);
+
+	// THEN: the distance should be scaled correctly
+	expected_scale     = 1.f;
+	expected_distance  = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+
+}