fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Collision Prevention: don't allow moving outside sensor FOV (#12741)

Browse Source 
* don't allow moving outside FOV

* Update parameters in tests
This commit is contained in:
Tanja Baumann
2019-08-20 17:01:13 +02:00
committed by Julian Kent
parent d38dfcfcd3
commit be233f6bc7
2 changed files with 148 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+33 -15
View File
@@ -205,6 +205,10 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
_updateDistanceSensor(obstacle);
float setpoint_length = setpoint.norm();
float col_prev_d = _param_mpc_col_prev_d.get();
float col_prev_dly = _param_mpc_col_prev_dly.get();
float col_prev_ang_rad = math::radians(_param_mpc_col_prev_ang.get());
float xy_p = _param_mpc_xy_p.get();
if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) {
if (setpoint_length > 0.001f) {
@@ -212,11 +216,12 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
Vector2f setpoint_dir = setpoint / setpoint_length;
float vel_max = setpoint_length;
int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);
float min_dist_to_keep = math::max(obstacle.min_distance / 100.0f, col_prev_d);
for (int i = 0; i < distances_array_size; i++) {
if (obstacle.distances[i] < obstacle.max_distance &&
obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) {
if ((float)i * obstacle.increment < 360.f) { //disregard unused bins at the end of the message
float distance = obstacle.distances[i] / 100.0f; //convert to meters
float angle = math::radians((float)i * obstacle.increment);
@@ -224,23 +229,36 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
angle += math::radians(obstacle.angle_offset);
}
//check if the bin must be considered regarding the given stick input
//get direction of current bin
Vector2f bin_direction = {cos(angle), sin(angle)};
if (setpoint_dir.dot(bin_direction) > 0
&& setpoint_dir.dot(bin_direction) > cosf(math::radians(_param_mpc_col_prev_ang.get()))) {
//calculate max allowed velocity with a P-controller (same gain as in the position controller)
float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
float delay_distance = curr_vel_parallel * _param_mpc_col_prev_dly.get();
float vel_max_posctrl = math::max(0.f,
_param_mpc_xy_p.get() * (distance - _param_mpc_col_prev_d.get() - delay_distance));
Vector2f vel_max_vec = bin_direction * vel_max_posctrl;
float vel_max_bin = vel_max_vec.dot(setpoint_dir);
if (obstacle.distances[i] < obstacle.max_distance &&
obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) {
//constrain the velocity
if (vel_max_bin >= 0) {
vel_max = math::min(vel_max, vel_max_bin);
if (setpoint_dir.dot(bin_direction) > 0
&& setpoint_dir.dot(bin_direction) > cosf(col_prev_ang_rad)) {
//calculate max allowed velocity with a P-controller (same gain as in the position controller)
float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
float delay_distance = curr_vel_parallel * col_prev_dly;
float vel_max_posctrl = math::max(0.f, xy_p * (distance - min_dist_to_keep - delay_distance));
Vector2f vel_max_vec = bin_direction * vel_max_posctrl;
float vel_max_bin = vel_max_vec.dot(setpoint_dir);
//constrain the velocity
if (vel_max_bin >= 0) {
vel_max = math::min(vel_max, vel_max_bin);
}
}
} else if (obstacle.distances[i] == UINT16_MAX) {
float sp_bin = setpoint_dir.dot(bin_direction);
float ang_half_bin = cosf(math::radians(obstacle.increment) / 2.f);
//if the setpoint lies outside the FOV set velocity to zero
if (sp_bin > ang_half_bin) {
vel_max = 0.f;
}
}
}
}