Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

fixed radius enlargement bug

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2026-04-21 15:49:58 +03:00
parent 3e1f18e8de
commit 58a62340d9
1 changed files with 15 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.cpp
+15 -4
View File
@@ -251,13 +251,24 @@ bool GeofenceAvoidancePlanner::calculate_graph_nodes(const matrix::Vector2<doubl
} else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
const matrix::Vector2f center = get_vertex_local_position(poly_index, 0, geofence, start);
const float delta_angle = 2.f * M_PI_F / kCircleApproxVertices;
const float enlarged_radius = info.circle_radius / (2.f * cosf(delta_angle)) + (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION ?
-margin : margin);
float adjusted_radius;
if (info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
adjusted_radius = info.circle_radius / cosf(M_PI_F / kCircleApproxVertices) + margin;
} else {
adjusted_radius = info.circle_radius - margin;
if (adjusted_radius <= 0.f) {
return false;
}
}
for (int i = 0; i < kCircleApproxVertices; i++) {
const float angle = i * (2.f * M_PI_F / kCircleApproxVertices);
const float angle = i * delta_angle;
_graph_nodes[node_index].type = Node::Type::VERTEX;
_graph_nodes[node_index].position = center + matrix::Vector2f{enlarged_radius * cosf(angle), enlarged_radius * sinf(angle)};
_graph_nodes[node_index].position = center + matrix::Vector2f{adjusted_radius * cosf(angle), adjusted_radius * sinf(angle)};
_graph_nodes[node_index].best_distance = FLT_MAX;
_graph_nodes[node_index].previous_index = -1;
_graph_nodes[node_index].visited = false;