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

Account for the time since range data arrived

Browse Source
This commit is contained in:
Julian Kent
2019-09-11 12:34:59 -04:00
committed by Daniel Agar
parent 94ef585f76
commit b27d6a958a
1 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+4 -2
View File
@@ -267,7 +267,9 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { //disregard unused bins at the end of the message
//delete stale values
if (getElapsedTime(&_data_timestamps[i]) > RANGE_STREAM_TIMEOUT_US) {
auto data_age = getElapsedTime(&_data_timestamps[i]);
if (data_age > RANGE_STREAM_TIMEOUT_US) {
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
}
@@ -287,7 +289,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
&& setpoint_dir.dot(bin_direction) > cosf(col_prev_ang_rad)) {
//calculate max allowed velocity with a P-controller (same gain as in the position controller)
float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
float delay_distance = curr_vel_parallel * col_prev_dly;
float delay_distance = curr_vel_parallel * (col_prev_dly + data_age * 1e-6f);
float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
float vel_max_posctrl = xy_p * stop_distance;
float vel_max_smooth = trajmath::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance);