From 5ff267a44e55ae20b5c4d994b1d49d4d2c06ca33 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Sat, 3 Aug 2024 13:26:30 +0200
Subject: [PATCH] fixing rangefinder

---
 src/uav_system_ros.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/uav_system_ros.cpp b/src/uav_system_ros.cpp
index d362c88..eee2065 100644
--- a/src/uav_system_ros.cpp
+++ b/src/uav_system_ros.cpp
@@ -409,7 +409,7 @@ void UavSystemRos::publishRangefinder(const MultirotorModel::State &state) {
   double range_measurement;
 
   if (body_z(2) > 0) {
-    range_measurement = (state.x(2) - model_params_.ground_z) / cos(tilt);
+    range_measurement = (state.x(2) - model_params_.ground_z) / cos(tilt) + 0.01;
   } else {
     range_measurement = std::numeric_limits<double>::max();
   }