diff --git a/src/api_communication/api_communication/api_listener.py b/src/api_communication/api_communication/api_listener.py
index 4fd92f7b..1c38e398 100644
--- a/src/api_communication/api_communication/api_listener.py
+++ b/src/api_communication/api_communication/api_listener.py
@@ -104,6 +104,7 @@ class ApiListener(Node):
 
         self.event_loop = None
         self.armed = False
+        self.failsafe_enabled = False
     
     def wait_for_service(self,client,service_name):
         """Waits for a client service to be available
@@ -147,6 +148,9 @@ class ApiListener(Node):
         Args:
             msg (FailSAfe): The message that was received
         """
+        if self.failsafe_enabled:
+            return
+        
         self.status_data['failsafe'] = msg.enabled
         self.message_queue.append(json.dumps(
                     {'type': ResponseMessage.FAILSAFE.name, 'message': msg.msg}))
diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp
index 78d9f8b7..2c1324d4 100644
--- a/src/drone_controls/src/PositionChanger.cpp
+++ b/src/drone_controls/src/PositionChanger.cpp
@@ -256,17 +256,16 @@ public:
      */
     void check_lidar_health()
     {
-        if (!this->received_lidar_message && !this->failsafe_enabled && this->lidar_health_checks > 20)
+        if (this->has_received_first_lidar_message)
         {
-            RCLCPP_WARN(this->get_logger(), "Lidar not sending messages, enabling failsafe");
-            enable_failsafe(u"No healthy connection to LIDAR! Check the LIDAR USB cable and restart the drone.");
+            if (!this->received_lidar_message)
+            {
+                RCLCPP_WARN(this->get_logger(), "Lidar not sending messages, enabling failsafe");
+                enable_failsafe(u"No healthy connection to LIDAR! Check the LIDAR USB cable and restart the drone.");
+            }
         }
         this->received_lidar_message = false;
         this->has_received_first_lidar_message = true;
-        if (this->lidar_health_checks <= 20)
-        {
-            this->lidar_health_checks++;
-        }
     }
 
     /**