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

Collision prevention: Option to enable flying outside FOV and rename parameters (new CP group)

Browse Source 
* rename parameters to allow more descriptive names under CP group
* add option to enable moving where there is no data
* add test for param CP_GO_NO_DATA

Co-Authored-By: Martina Rivizzigno <martina@rivizzigno.it>
This commit is contained in:
Tanja Baumann
2019-10-17 11:23:50 -04:00
committed by Daniel Agar
co-authored by Martina Rivizzigno
parent 2fcddd9b8d
commit be1f966e5f
5 changed files with 69 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+7 -6
View File
@@ -285,8 +285,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
void
CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad)
{
const float col_prev_d = _param_mpc_col_prev_d.get();
const int guidance_bins = floor(_param_mpc_col_prev_cng.get() / INTERNAL_MAP_INCREMENT_DEG);
const float col_prev_d = _param_cp_dist.get();
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;
@@ -376,8 +376,9 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
_updateObstacleMap();
// read parameters
const float col_prev_d = _param_mpc_col_prev_d.get();
const float col_prev_dly = _param_mpc_col_prev_dly.get();
const float col_prev_d = _param_cp_dist.get();
const float col_prev_dly = _param_cp_delay.get();
const bool move_no_data = _param_cp_go_nodata.get() > 0;
const float xy_p = _param_mpc_xy_p.get();
const float max_jerk = _param_mpc_jerk_max.get();
const float max_accel = _param_mpc_acc_hor.get();
@@ -400,7 +401,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
_obstacle_map_body_frame.angle_offset);
int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
// change setpoint direction slightly (max by _param_mpc_col_prev_cng degrees) to help guide through narrow gaps
// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
// limit speed for safe flight
@@ -452,7 +453,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
}
}
} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) {
} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index && (!move_no_data)) {
vel_max = 0.f;
}
}