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

ackermann: add protection against float precision problem in acceptance radius update (#23478)

Browse Source 
* ackermann: add protection against float precision problem in acceptance radius update

* ackermann: protect against divide-by-zero

---------

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
sbtjagu 2024-08-05 13:02:12 +02:00 committed by GitHub
parent 4883f2128a
commit 326e2a9f5c
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp
View File

@ -268,8 +268,9 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned
// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
if (curr_to_next_wp_ned.norm() > FLT_EPSILON && curr_to_prev_wp_ned.norm() > FLT_EPSILON) {
const float theta = acosf((curr_to_prev_wp_ned * curr_to_next_wp_ned) / (curr_to_prev_wp_ned.norm() *
curr_to_next_wp_ned.norm())) / 2.f;
float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero();
cosin = math::constrain<float>(cosin, -1.f, 1.f); // Protect against float precision problem
const float theta = acosf(cosin) / 2.f;
const float min_turning_radius = wheel_base / sinf(max_steer_angle);
const float acceptance_radius_temp = min_turning_radius / tanf(theta);
const float acceptance_radius_temp_scaled = acceptance_radius_gain *