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

collision_prevention: improve behavior in case of range data loss

Browse Source 
* add test for no incoming range data
This commit is contained in:
Tanja Baumann
2019-11-21 11:13:28 -05:00
committed by Daniel Agar
parent 45b32b5eaa
commit 5d3a6d8213
3 changed files with 109 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/collision_prevention/CollisionPreventionTest.cpp
+69 -1
View File
@@ -251,9 +251,9 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage)
TEST_F(CollisionPreventionTest, testPurgeOldData)
{
// GIVEN: a simple setup condition
TestTimingCollisionPrevention cp;
hrt_abstime start_time = hrt_absolute_time();
mocked_time = start_time;
TestTimingCollisionPrevention cp;
matrix::Vector2f original_setpoint(10, 0);
float max_speed = 3;
matrix::Vector2f curr_pos(0, 0);
@@ -325,6 +325,74 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
orb_unadvertise(vehicle_attitude_pub);
}
TEST_F(CollisionPreventionTest, testNoRangeData)
{
// GIVEN: a simple setup condition
TestTimingCollisionPrevention cp;
hrt_abstime start_time = hrt_absolute_time();
mocked_time = start_time;
matrix::Vector2f original_setpoint(10, 0);
float max_speed = 3;
matrix::Vector2f curr_pos(0, 0);
matrix::Vector2f curr_vel(2, 0);
vehicle_attitude_s attitude;
attitude.timestamp = start_time;
attitude.q[0] = 1.0f;
attitude.q[1] = 0.0f;
attitude.q[2] = 0.0f;
attitude.q[3] = 0.0f;
// AND: a parameter handle
param_t param = param_handle(px4::params::CP_DIST);
float value = 10; // try to keep 10m distance
param_set(param, &value);
cp.paramsChanged();
// AND: an obstacle message without any obstacle
obstacle_distance_s message;
memset(&message, 0xDEAD, sizeof(message));
message.frame = message.MAV_FRAME_GLOBAL; //north aligned
message.min_distance = 100;
message.max_distance = 10000;
message.angle_offset = 0;
message.timestamp = start_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] = 9000;
}
// WHEN: 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_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude);
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message);
orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude);
for (int i = 0; i < 10; i++) {
matrix::Vector2f modified_setpoint = original_setpoint;
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
//advance time by 0.1 seconds but no new message comes in
mocked_time = mocked_time + 100000;
if (i < 5) {
// THEN: If the data is new enough, the velocity setpoint should stay the same as the input
// Note: direction will change slightly due to guidance
EXPECT_EQ(original_setpoint.norm(), modified_setpoint.norm());
} else {
// THEN: If the data is expired, the velocity setpoint should be cut down to zero because there is no data
EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm()) << modified_setpoint(0) << "," << modified_setpoint(1);
}
}
orb_unadvertise(obstacle_distance_pub);
orb_unadvertise(vehicle_attitude_pub);
}
TEST_F(CollisionPreventionTest, noBias)
{
// GIVEN: a simple setup condition