dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ColPrev: No direction change if no obstacle (#13398) 

* only change direction if in other bin
This commit is contained in:
Tanja Baumann 2019-11-07 14:21:12 +01:00 committed by Julian Kent
parent a5af1c8afc
commit b60a955501
2 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/lib/CollisionPrevention/CollisionPrevention.cpp
View File

@ -289,6 +289,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG);
const int sp_index_original = setpoint_index;
float best_cost = 9999.f;
int new_sp_index = setpoint_index;
for (int i = sp_index_original - guidance_bins; i <= sp_index_original + guidance_bins; i++) {
@ -314,13 +315,17 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
if (bin_cost < best_cost && _obstacle_map_body_frame.distances[bin] != UINT16_MAX) {
best_cost = bin_cost;
float angle = math::radians((float)bin * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
setpoint_dir = {cosf(angle), sinf(angle)};
setpoint_index = bin;
new_sp_index = bin;
}
}
//only change setpoint direction if it was moved to a different bin
if (new_sp_index != setpoint_index) {
float angle = math::radians((float)new_sp_index * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
setpoint_dir = {cosf(angle), sinf(angle)};
setpoint_index = new_sp_index;
}
}
float

4
src/lib/CollisionPrevention/CollisionPreventionTest.cpp
View File

@ -421,8 +421,8 @@ TEST_F(CollisionPreventionTest, outsideFOV)
matrix::Vector2f setpoint_dir = modified_setpoint / setpoint_length;
float sp_angle_body_frame = atan2(setpoint_dir(1), setpoint_dir(0));
float sp_angle_deg = math::degrees(matrix::wrap_2pi(sp_angle_body_frame));
EXPECT_GT(sp_angle_deg, 45.f);
EXPECT_LT(sp_angle_deg, 225.f);
EXPECT_GE(sp_angle_deg, 45.f);
EXPECT_LE(sp_angle_deg, 225.f);
}
}