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

Add test for jerk reduction

Browse Source
This commit is contained in:
Julian Kent
2019-09-02 14:51:18 +02:00
parent 865157228f
commit bc58bed960
2 changed files with 52 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.hpp
+2 -2
View File
@@ -97,8 +97,8 @@ private:
(ParamFloat<px4::params::MPC_COL_PREV_ANG>) _param_mpc_col_prev_ang, /**< collision prevention detection angle */
(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,
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor
(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
+50
View File
@@ -233,3 +233,53 @@ TEST_F(CollisionPreventionTest, outsideFOV)
orb_unadvertise(obstacle_distance_pub);
}
TEST_F(CollisionPreventionTest, jerkLimit)
{
// GIVEN: a simple setup condition
TestCollisionPrevention cp;
matrix::Vector2f original_setpoint(10, 0);
float max_speed = 3;
matrix::Vector2f curr_pos(0, 0);
matrix::Vector2f curr_vel(2, 0);
// AND: distance set to 5m
param_t param = param_handle(px4::params::MPC_COL_PREV_D);
float value = 5; // try to keep 5m distance
param_set(param, &value);
cp.paramsChanged();
// AND: an obstacle message
obstacle_distance_s message;
memset(&message, 0xDEAD, sizeof(message));
message.min_distance = 100;
message.max_distance = 2000;
message.timestamp = hrt_absolute_time();
int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
message.increment = 360 / distances_array_size;
for (int i = 0; i < distances_array_size; i++) {
message.distances[i] = 700;
}
// AND: we publish the message and set the parameter and then run the setpoint modification
orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message);
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
matrix::Vector2f modified_setpoint_default_jerk = original_setpoint;
cp.modifySetpoint(modified_setpoint_default_jerk, max_speed, curr_pos, curr_vel);
orb_unadvertise(obstacle_distance_pub);
// AND: we now set max jerk to 0.1
param = param_handle(px4::params::MPC_JERK_MAX);
value = 0.1; // 0.1 maximum jerk
param_set(param, &value);
cp.paramsChanged();
// WHEN: we run the setpoint modification again
matrix::Vector2f modified_setpoint_limited_jerk = original_setpoint;
cp.modifySetpoint(modified_setpoint_limited_jerk, max_speed, curr_pos, curr_vel);
// THEN: the new setpoint should be much slower than the one with default jerk
EXPECT_LT(modified_setpoint_limited_jerk.norm() * 10, modified_setpoint_default_jerk.norm());
}