fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add safety measure and test for missing sensor data

Browse Source
This commit is contained in:
baumanta 2020-01-23 13:56:18 +01:00 committed by Julian Kent
parent 67d0f5c5d1
commit 7801ed129f
3 changed files with 28 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/lib/collision_prevention/CollisionPrevention.cpp
View File

@ -83,6 +83,7 @@ CollisionPrevention::CollisionPrevention(ModuleParams *parent) :
for (uint32_t i = 0 ; i < internal_bins; i++) {
_data_timestamps[i] = current_time;
_data_maxranges[i] = 0;
_data_fov[i] = 0;
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
}
}
@ -129,6 +130,7 @@ CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle,
_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
_data_maxranges[i] = obstacle.max_distance;
_data_fov[i] = 1;
}
}
}
@ -148,6 +150,7 @@ CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle,
_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
_data_maxranges[i] = obstacle.max_distance;
_data_fov[i] = 1;
}
}
}
@ -281,6 +284,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
_obstacle_map_body_frame.distances[wrapped_bin] = static_cast<uint16_t>(100.0f * distance_reading + 0.5f);
_data_timestamps[wrapped_bin] = _obstacle_map_body_frame.timestamp;
_data_maxranges[wrapped_bin] = sensor_range;
_data_fov[wrapped_bin] = 1;
}
}
}
@ -468,8 +472,10 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
}
}
} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index && (!move_no_data)) {
vel_max = 0.f;
} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) {
if (!move_no_data || (move_no_data && _data_fov[i])) {
vel_max = 0.f;
}
}
}

1
src/lib/collision_prevention/CollisionPrevention.hpp
View File

@ -85,6 +85,7 @@ public:
protected:
obstacle_distance_s _obstacle_map_body_frame {};
bool _data_fov[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])];
uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])];
uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof(
_obstacle_map_body_frame.distances[0])]; /**< in cm */

26
src/lib/collision_prevention/CollisionPreventionTest.cpp
View File

@ -272,7 +272,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
cp.paramsChanged();
// AND: an obstacle message
obstacle_distance_s message, message_empty;
obstacle_distance_s message, message_lost_data;
memset(&message, 0xDEAD, sizeof(message));
message.frame = message.MAV_FRAME_GLOBAL; //north aligned
message.min_distance = 100;
@ -281,20 +281,23 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
message.timestamp = start_time;
int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]);
message.increment = 360 / distances_array_size;
message_empty = message;
message_lost_data = message;
for (int i = 0; i < distances_array_size; i++) {
if (i < 10) {
message.distances[i] = 10001;
message_lost_data.distances[i] = UINT16_MAX;
} else if (i > 15 && i < 18) {
message.distances[i] = 10001;
message_lost_data.distances[i] = 10001;
} else {
message.distances[i] = UINT16_MAX;
message_lost_data.distances[i] = UINT16_MAX;
}
message_empty.distances[i] = UINT16_MAX;
}
// 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);
@ -307,8 +310,16 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel);
mocked_time = mocked_time + 100000; //advance time by 0.1 seconds
message_empty.timestamp = mocked_time;
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message_empty);
message_lost_data.timestamp = mocked_time;
orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message_lost_data);
//at iteration 8 change the CP_GO_NO_DATA to True
if (i == 8) {
param_t param_allow = param_handle(px4::params::CP_GO_NO_DATA);
float value_allow = 1;
param_set(param_allow, &value_allow);
cp.paramsChanged();
}
if (i < 6) {
// THEN: If the data is new enough, the velocity setpoint should stay the same as the input
@ -317,6 +328,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
} else {
// THEN: If the data is expired, the velocity setpoint should be cut down to zero because there is no data
//(even if CP_GO_NO_DATA is set to true, because we once had data in those bins and now lost the sensor)
EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm()) << modified_setpoint(0) << "," << modified_setpoint(1);
}
}