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

CollisionPrevention: use the maximum timestamp between offboard and

Browse Source 
distance sensor such that if one of the two fails the vehicle goes into
failsafe, do not switch off CollisionPrevention if it fails
This commit is contained in:
Martina Rivizzigno
2019-07-15 10:58:00 +02:00
committed by Beat Küng
parent 8aad105265
commit 560c9f972a
1 changed files with 2 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+2 -6
View File
@@ -113,7 +113,7 @@ void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle)
if (obstacle.increment > 0) {
// data from companion
obstacle.timestamp = math::min(obstacle.timestamp, distance_sensor.timestamp);
obstacle.timestamp = math::max(obstacle.timestamp, distance_sensor.timestamp);
obstacle.max_distance = math::max((int)obstacle.max_distance,
(int)distance_sensor.max_distance * 100);
obstacle.min_distance = math::min((int)obstacle.min_distance,
@@ -264,13 +264,9 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
}
} else {
// if distance data are stale, switch to Loiter and disable Collision Prevention
// such that it is still possible to fly in Position Control Mode
// if distance data are stale, switch to Loiter
_publishVehicleCmdDoLoiter();
mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering.");
float col_prev_d = -1.f;
param_set(param_find("MPC_COL_PREV_D"), &col_prev_d);
mavlink_log_critical(&_mavlink_log_pub, "Collision Prevention disabled.");
}
}