diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt index 561b7755c76d..6d561f72db08 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt +++ b/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 ) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index a8ed20383d11..a7a94c67915e 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -33,19 +33,18 @@ #include "lightware_sf45_serial.hpp" -#include #include +#include +#include #include + #include #include - -#include +#include +#include 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(_current_bin_dist); + float scaled_yaw_rad = math::radians(static_cast(scaled_yaw)); + ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_rad, _vehicle_attitude); + _current_bin_dist = static_cast(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; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index c5cae5e049df..bfe44f0da2a5 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -49,7 +49,9 @@ #include #include +#include #include +#include #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 diff --git a/src/lib/collision_prevention/CMakeLists.txt b/src/lib/collision_prevention/CMakeLists.txt index 663c9e9fee2f..ae42b65454cb 100644 --- a/src/lib/collision_prevention/CMakeLists.txt +++ b/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) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index b2c62d4906c6..3f1344ffdd1b 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/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 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(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 8fcc7c28f3ec..7d9d16cd7fca 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/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 diff --git a/src/lib/collision_prevention/ObstacleMath.cpp b/src/lib/collision_prevention/ObstacleMath.cpp new file mode 100644 index 000000000000..22d67c64ae61 --- /dev/null +++ b/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 + +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 diff --git a/src/lib/collision_prevention/ObstacleMath.hpp b/src/lib/collision_prevention/ObstacleMath.hpp new file mode 100644 index 000000000000..3c19eb911062 --- /dev/null +++ b/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 + +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 diff --git a/src/lib/collision_prevention/ObstacleMathTest.cpp b/src/lib/collision_prevention/ObstacleMathTest.cpp new file mode 100644 index 000000000000..e24b95134c96 --- /dev/null +++ b/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 +#include +#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); + +}