fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 12:38:54 +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;
}
}
src/lib/CollisionPrevention/CollisionPrevention.hpp
+5 -4
View File
@@ -70,7 +70,7 @@ public:
/**
* Returs true if Collision Prevention is running
*/
bool is_active() { return _param_mpc_col_prev_d.get() > 0; }
bool is_active() { return _param_cp_dist.get() > 0; }
/**
* Computes collision free setpoints
@@ -138,10 +138,11 @@ private:
hrt_abstime _last_collision_warning{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_COL_PREV_D>) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */
(ParamFloat<px4::params::MPC_COL_PREV_CNG>) _param_mpc_col_prev_cng, /**< collision prevention change setpoint angle */
(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */
(ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, /**< delay of the range measurement data*/
(ParamFloat<px4::params::CP_GUIDE_ANG>) _param_cp_guide_ang, /**< collision prevention change setpoint angle */
(ParamFloat<px4::params::CP_GO_NO_DATA>) _param_cp_go_nodata, /**< movement allowed where no data*/
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
(ParamFloat<px4::params::MPC_COL_PREV_DLY>) _param_mpc_col_prev_dly, /**< delay of the range measurement data*/
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/
)
src/lib/CollisionPrevention/CollisionPreventionTest.cpp
+43 -10
View File
@@ -108,7 +108,7 @@ TEST_F(CollisionPreventionTest, noSensorData)
matrix::Vector2f curr_vel(2, 0);
// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
// WHEN: we set the parameter check then apply the setpoint modification
float value = 10; // try to keep 10m away from obstacles
@@ -140,7 +140,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage)
attitude.q[3] = 0.0f;
// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 10; // try to keep 10m distance
param_set(param, &value);
cp.paramsChanged();
@@ -205,7 +205,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
attitude.q[3] = 0.0f;
// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 10; // try to keep 10m distance
param_set(param, &value);
cp.paramsChanged();
@@ -266,7 +266,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
attitude.q[3] = 0.0f;
// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 10; // try to keep 10m distance
param_set(param, &value);
cp.paramsChanged();
@@ -335,7 +335,7 @@ TEST_F(CollisionPreventionTest, noBias)
matrix::Vector2f curr_vel(2, 0);
// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 5; // try to keep 5m distance
param_set(param, &value);
cp.paramsChanged();
@@ -375,7 +375,7 @@ TEST_F(CollisionPreventionTest, outsideFOV)
matrix::Vector2f curr_vel(2, 0);
// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 5; // try to keep 5m distance
param_set(param, &value);
cp.paramsChanged();
@@ -429,6 +429,39 @@ TEST_F(CollisionPreventionTest, outsideFOV)
orb_unadvertise(obstacle_distance_pub);
}
TEST_F(CollisionPreventionTest, goNoData)
{
// GIVEN: a simple setup condition with the initial state (no distance data)
TestCollisionPrevention cp;
float max_speed = 3;
matrix::Vector2f curr_pos(0, 0);
matrix::Vector2f curr_vel(2, 0);
// AND: a parameter handle
param_t param = param_handle(px4::params::CP_DIST);
float value = 5; // try to keep 5m distance
param_set(param, &value);
cp.paramsChanged();
matrix::Vector2f original_setpoint = {-5, 0};
matrix::Vector2f modified_setpoint = original_setpoint;
//THEN: the modified setpoint should be zero velocity
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f);
//WHEN: we change the parameter CP_GO_NO_DATA to allow flying ouside the FOV
param_t param_allow = param_handle(px4::params::CP_GO_NO_DATA);
float value_allow = 1;
param_set(param_allow, &value_allow);
cp.paramsChanged();
//THEN: the modified setpoint should stay the same as the input
modified_setpoint = original_setpoint;
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
EXPECT_FLOAT_EQ(modified_setpoint.norm(), original_setpoint.norm());
}
TEST_F(CollisionPreventionTest, jerkLimit)
{
// GIVEN: a simple setup condition
@@ -439,7 +472,7 @@ TEST_F(CollisionPreventionTest, jerkLimit)
matrix::Vector2f curr_vel(2, 0);
// AND: distance set to 5m
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 5; // try to keep 5m distance
param_set(param, &value);
cp.paramsChanged();
@@ -847,7 +880,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum)
int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment);
//set parameter
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 3; // try to keep 10m away from obstacles
param_set(param, &value);
cp.paramsChanged();
@@ -896,7 +929,7 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum)
int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment);
//set parameter
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 3; // try to keep 10m away from obstacles
param_set(param, &value);
cp.paramsChanged();
@@ -927,7 +960,7 @@ TEST_F(CollisionPreventionTest, overlappingSensors)
attitude.q[3] = 0.0f;
// AND: a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
float value = 10; // try to keep 10m distance
param_set(param, &value);
cp.paramsChanged();
src/lib/CollisionPrevention/collisionprevention_params.c
+13 -3
View File
@@ -49,7 +49,7 @@
* @unit meters
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f);
PARAM_DEFINE_FLOAT(CP_DIST, -1.0f);
/**
* Average delay of the range sensor message plus the tracking delay of the position controller in seconds
@@ -61,7 +61,7 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f);
* @unit seconds
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.4f);
PARAM_DEFINE_FLOAT(CP_DELAY, 0.4f);
/**
* Angle left/right from the commanded setpoint by which the collision prevention algorithm can choose to change the setpoint direction
@@ -73,4 +73,14 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.4f);
* @unit [deg]
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_COL_PREV_CNG, 30.f);
PARAM_DEFINE_FLOAT(CP_GUIDE_ANG, 30.f);
/**
* Boolean to allow moving into directions where there is no sensor data (outside FOV)
*
* Only used in Position mode.
*
* @boolean
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(CP_GO_NO_DATA, 0);
src/lib/parameters/ParameterTest.cpp
+1 -1
View File
@@ -51,7 +51,7 @@ public:
TEST_F(ParameterTest, testParamReadWrite)
{
// GIVEN a parameter handle
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
param_t param = param_handle(px4::params::CP_DIST);
// WHEN: we get the parameter
float value = -999.f;