fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 18:10:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

collision-prevention: extract rotation scaling function into new ObstacleMath library.

Browse Source 
New library created for static and/or repeated code across collision prevention and driver files.
This commit is contained in:
mahimayoga
2025-01-20 10:29:30 +01:00
committed by Mathieu Bresciani
parent b1ca0495e2
commit 29f981f14c
5 changed files with 111 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/collision_prevention/CMakeLists.txt
+6 -1
View File
@@ -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
+3 -12
View File
@@ -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 -1
View File
@@ -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
+54
View File
@@ -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
+47
View File
@@ -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