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

refactor(collision_prevention): Remove loop in _checkSetpointDirectionFeasability() (#26856)

Browse Source 
* Refactor feasibility check for setpoint direction

* Add check for out of bounds index

* Formatting
This commit is contained in:
Matteo Del Seppia 2026-03-24 02:28:57 +01:00 committed by GitHub
parent 8d4a5cc76c
commit 860505fc05
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/lib/collision_prevention/CollisionPrevention.cpp
View File

@ -357,18 +357,18 @@ CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_
bool
CollisionPrevention::_checkSetpointDirectionFeasability()
{
bool setpoint_feasible = true;
for (int i = 0; i < BIN_COUNT; i++) {
// check if our setpoint is either pointing in a direction where data exists, or if not, wether we are allowed to go where there is no data
if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_no_data.get()
|| (_param_cp_go_no_data.get() && _data_fov[i]))) {
setpoint_feasible = false;
}
if (_setpoint_index < 0 || _setpoint_index >= BIN_COUNT) {
return false; // treat out-of-bounds as unsafe
}
return setpoint_feasible;
const bool no_data = (_obstacle_map_body_frame.distances[_setpoint_index] == UINT16_MAX);
const bool allow_movement_towards_no_data = _param_cp_go_no_data.get();
const bool fov_at_setpoint = _data_fov[_setpoint_index];
// The setpoint is feasible if:
// 1. There is actual data at the setpoint (no_data == false), OR
// 2. There is no data, but movement into no-data bins is allowed and the setpoint is outside the sensor FOV.
return !no_data || (allow_movement_towards_no_data && !fov_at_setpoint);
}
void