From 560c9f972a04a4d5e7e2f51fb514e9607275bde9 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Mon, 1 Jul 2019 18:46:01 +0200 Subject: [PATCH] CollisionPrevention: use the maximum timestamp between offboard and distance sensor such that if one of the two fails the vehicle goes into failsafe, do not switch off CollisionPrevention if it fails --- src/lib/CollisionPrevention/CollisionPrevention.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 13d990b14d..e2cfc26226 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -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."); } }