From 703c488dc595fae583475f8d8d2febf23e03653f Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 8 Oct 2019 10:20:31 +0200 Subject: [PATCH 1/5] rename parameters to allow more descriptive names --- .../CollisionPrevention.cpp | 10 +++++----- .../CollisionPrevention.hpp | 8 ++++---- .../CollisionPreventionTest.cpp | 20 +++++++++---------- .../collisionprevention_params.c | 6 +++--- src/lib/parameters/ParameterTest.cpp | 2 +- 5 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index a5028d9e275d..0c5f2ca9d16f 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -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_mpc_cp_dist.get(); + const int guidance_bins = floor(_param_mpc_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG); const int sp_index_original = setpoint_index; float best_cost = 9999.f; @@ -376,8 +376,8 @@ 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_mpc_cp_dist.get(); + const float col_prev_dly = _param_mpc_cp_delay.get(); 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 +400,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_mpc_cp_guide_ang degrees) to help guide through narrow gaps _adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); // limit speed for safe flight diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index af4afe255550..2e42802d709f 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -70,7 +70,7 @@ class CollisionPrevention : public ModuleParams /** * Returs true if Collision Prevention is running */ - bool is_active() { return _param_mpc_col_prev_d.get() > 0; } + bool is_active() { return _param_mpc_cp_dist.get() > 0; } /** * Computes collision free setpoints @@ -138,10 +138,10 @@ class CollisionPrevention : public ModuleParams hrt_abstime _last_collision_warning{0}; DEFINE_PARAMETERS( - (ParamFloat) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */ - (ParamFloat) _param_mpc_col_prev_cng, /**< collision prevention change setpoint angle */ + (ParamFloat) _param_mpc_cp_dist, /**< collision prevention keep minimum distance */ + (ParamFloat) _param_mpc_cp_guide_ang, /**< collision prevention change setpoint angle */ (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ - (ParamFloat) _param_mpc_col_prev_dly, /**< delay of the range measurement data*/ + (ParamFloat) _param_mpc_cp_delay, /**< delay of the range measurement data*/ (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ (ParamFloat) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/ ) diff --git a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp index fef858b775d7..e9b74688348e 100644 --- a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp +++ b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp @@ -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::MPC_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::MPC_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::MPC_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::MPC_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::MPC_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::MPC_CP_DIST); float value = 5; // try to keep 5m distance param_set(param, &value); cp.paramsChanged(); @@ -439,7 +439,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::MPC_CP_DIST); float value = 5; // try to keep 5m distance param_set(param, &value); cp.paramsChanged(); @@ -847,7 +847,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::MPC_CP_DIST); float value = 3; // try to keep 10m away from obstacles param_set(param, &value); cp.paramsChanged(); @@ -896,7 +896,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::MPC_CP_DIST); float value = 3; // try to keep 10m away from obstacles param_set(param, &value); cp.paramsChanged(); @@ -927,7 +927,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::MPC_CP_DIST); float value = 10; // try to keep 10m distance param_set(param, &value); cp.paramsChanged(); diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c index cbc734b6dc49..99823e229ca6 100644 --- a/src/lib/CollisionPrevention/collisionprevention_params.c +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -49,7 +49,7 @@ * @unit meters * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f); +PARAM_DEFINE_FLOAT(MPC_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(MPC_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,4 @@ 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(MPC_CP_GUIDE_ANG, 30.f); diff --git a/src/lib/parameters/ParameterTest.cpp b/src/lib/parameters/ParameterTest.cpp index 1e9bec0336d8..3bd68e593e1f 100644 --- a/src/lib/parameters/ParameterTest.cpp +++ b/src/lib/parameters/ParameterTest.cpp @@ -51,7 +51,7 @@ class ParameterTest : public ::testing::Test 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::MPC_CP_DIST); // WHEN: we get the parameter float value = -999.f; From 0ef5a94f6b872bd1ce6a7d52a7c0e67ab8e52f65 Mon Sep 17 00:00:00 2001 From: baumanta Date: Wed, 9 Oct 2019 09:00:35 +0200 Subject: [PATCH 2/5] add option to enable moving where there is no data --- src/lib/CollisionPrevention/CollisionPrevention.cpp | 3 ++- src/lib/CollisionPrevention/CollisionPrevention.hpp | 3 ++- .../CollisionPrevention/collisionprevention_params.c | 10 ++++++++++ 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 0c5f2ca9d16f..55b28ec016b2 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -378,6 +378,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec // read parameters const float col_prev_d = _param_mpc_cp_dist.get(); const float col_prev_dly = _param_mpc_cp_delay.get(); + bool move_no_data = _param_mpc_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(); @@ -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; } } diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 2e42802d709f..9deef84b2dfe 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -139,9 +139,10 @@ class CollisionPrevention : public ModuleParams DEFINE_PARAMETERS( (ParamFloat) _param_mpc_cp_dist, /**< collision prevention keep minimum distance */ + (ParamFloat) _param_mpc_cp_delay, /**< delay of the range measurement data*/ (ParamFloat) _param_mpc_cp_guide_ang, /**< collision prevention change setpoint angle */ + (ParamFloat) _param_mpc_cp_go_nodata, /**< movement allowed where no data*/ (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ - (ParamFloat) _param_mpc_cp_delay, /**< delay of the range measurement data*/ (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ (ParamFloat) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/ ) diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c index 99823e229ca6..4ac968bb74f5 100644 --- a/src/lib/CollisionPrevention/collisionprevention_params.c +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -74,3 +74,13 @@ PARAM_DEFINE_FLOAT(MPC_CP_DELAY, 0.4f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_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(MPC_CP_GO_NODATA, 0); From c7b385f710d4c1839871b6fa533c5ff9bdfdea3e Mon Sep 17 00:00:00 2001 From: baumanta Date: Thu, 17 Oct 2019 11:22:20 +0200 Subject: [PATCH 3/5] group collision prevention params under CP group --- .../CollisionPrevention.cpp | 12 +++++------ .../CollisionPrevention.hpp | 10 +++++----- .../CollisionPreventionTest.cpp | 20 +++++++++---------- .../collisionprevention_params.c | 8 ++++---- src/lib/parameters/ParameterTest.cpp | 2 +- 5 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 55b28ec016b2..ab8bfe1fc161 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -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_cp_dist.get(); - const int guidance_bins = floor(_param_mpc_cp_guide_ang.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,9 +376,9 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec _updateObstacleMap(); // read parameters - const float col_prev_d = _param_mpc_cp_dist.get(); - const float col_prev_dly = _param_mpc_cp_delay.get(); - bool move_no_data = _param_mpc_cp_go_nodata.get() > 0; + const float col_prev_d = _param_cp_dist.get(); + const float col_prev_dly = _param_cp_delay.get(); + 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(); @@ -401,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_cp_guide_ang 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 diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 9deef84b2dfe..063be9a2323c 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -70,7 +70,7 @@ class CollisionPrevention : public ModuleParams /** * Returs true if Collision Prevention is running */ - bool is_active() { return _param_mpc_cp_dist.get() > 0; } + bool is_active() { return _param_cp_dist.get() > 0; } /** * Computes collision free setpoints @@ -138,10 +138,10 @@ class CollisionPrevention : public ModuleParams hrt_abstime _last_collision_warning{0}; DEFINE_PARAMETERS( - (ParamFloat) _param_mpc_cp_dist, /**< collision prevention keep minimum distance */ - (ParamFloat) _param_mpc_cp_delay, /**< delay of the range measurement data*/ - (ParamFloat) _param_mpc_cp_guide_ang, /**< collision prevention change setpoint angle */ - (ParamFloat) _param_mpc_cp_go_nodata, /**< movement allowed where no data*/ + (ParamFloat) _param_cp_dist, /**< collision prevention keep minimum distance */ + (ParamFloat) _param_cp_delay, /**< delay of the range measurement data*/ + (ParamFloat) _param_cp_guide_ang, /**< collision prevention change setpoint angle */ + (ParamFloat) _param_cp_go_nodata, /**< movement allowed where no data*/ (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ (ParamFloat) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/ diff --git a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp index e9b74688348e..fc0d0f1a8720 100644 --- a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp +++ b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp @@ -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_CP_DIST); + 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_CP_DIST); + 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_CP_DIST); + 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_CP_DIST); + 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_CP_DIST); + 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_CP_DIST); + param_t param = param_handle(px4::params::CP_DIST); float value = 5; // try to keep 5m distance param_set(param, &value); cp.paramsChanged(); @@ -439,7 +439,7 @@ TEST_F(CollisionPreventionTest, jerkLimit) matrix::Vector2f curr_vel(2, 0); // AND: distance set to 5m - param_t param = param_handle(px4::params::MPC_CP_DIST); + 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 +847,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_CP_DIST); + 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 +896,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_CP_DIST); + 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 +927,7 @@ TEST_F(CollisionPreventionTest, overlappingSensors) attitude.q[3] = 0.0f; // AND: a parameter handle - param_t param = param_handle(px4::params::MPC_CP_DIST); + param_t param = param_handle(px4::params::CP_DIST); float value = 10; // try to keep 10m distance param_set(param, &value); cp.paramsChanged(); diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c index 4ac968bb74f5..3f80cf84bfc5 100644 --- a/src/lib/CollisionPrevention/collisionprevention_params.c +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -49,7 +49,7 @@ * @unit meters * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_CP_DIST, -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_CP_DIST, -1.0f); * @unit seconds * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_CP_DELAY, 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,7 +73,7 @@ PARAM_DEFINE_FLOAT(MPC_CP_DELAY, 0.4f); * @unit [deg] * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_CP_GUIDE_ANG, 30.f); +PARAM_DEFINE_FLOAT(CP_GUIDE_ANG, 30.f); /** * Boolean to allow moving into directions where there is no sensor data (outside FOV) @@ -83,4 +83,4 @@ PARAM_DEFINE_FLOAT(MPC_CP_GUIDE_ANG, 30.f); * @boolean * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_CP_GO_NODATA, 0); +PARAM_DEFINE_FLOAT(CP_GO_NO_DATA, 0); diff --git a/src/lib/parameters/ParameterTest.cpp b/src/lib/parameters/ParameterTest.cpp index 3bd68e593e1f..cecb1a70da70 100644 --- a/src/lib/parameters/ParameterTest.cpp +++ b/src/lib/parameters/ParameterTest.cpp @@ -51,7 +51,7 @@ class ParameterTest : public ::testing::Test TEST_F(ParameterTest, testParamReadWrite) { // GIVEN a parameter handle - param_t param = param_handle(px4::params::MPC_CP_DIST); + param_t param = param_handle(px4::params::CP_DIST); // WHEN: we get the parameter float value = -999.f; From 66fa0cefbd905f5e14dc15b47148a82c29bbae42 Mon Sep 17 00:00:00 2001 From: Tanja Baumann Date: Thu, 17 Oct 2019 13:56:20 +0200 Subject: [PATCH 4/5] make bool const Co-Authored-By: Martina Rivizzigno --- src/lib/CollisionPrevention/CollisionPrevention.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index ab8bfe1fc161..f2b4d3750e6f 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -378,7 +378,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec // read parameters const float col_prev_d = _param_cp_dist.get(); const float col_prev_dly = _param_cp_delay.get(); - bool move_no_data = _param_cp_go_nodata.get() > 0; + 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(); From f3450a2066b0327d0cb805d6e04926eff61fc488 Mon Sep 17 00:00:00 2001 From: baumanta Date: Thu, 17 Oct 2019 15:54:58 +0200 Subject: [PATCH 5/5] add test for param CP_GO_NO_DATA --- .../CollisionPreventionTest.cpp | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp index fc0d0f1a8720..3ab0d934873d 100644 --- a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp +++ b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp @@ -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