From 61d962c31c188e679b481393ce6e74711b771735 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Tue, 13 Feb 2024 21:01:15 -0300
Subject: [PATCH] expose confidence cutoff api

---
 dvl-a50/dvl.py  | 15 ++++++++++++++-
 dvl-a50/main.py | 10 ++++++++++
 2 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/dvl-a50/dvl.py b/dvl-a50/dvl.py
index 9db58fc..1f12d8e 100644
--- a/dvl-a50/dvl.py
+++ b/dvl-a50/dvl.py
@@ -63,6 +63,7 @@ class DvlDriver(threading.Thread):
     last_temperature_check_time = 0
     temperature_check_interval_s = 30
     temperature_too_hot = 45
+    confidence_cutoff = 0
 
     def __init__(self, orientation=DVL_DOWN) -> None:
         threading.Thread.__init__(self)
@@ -88,6 +89,7 @@ def load_settings(self) -> None:
                 self.origin = data["origin"]
                 self.rangefinder = data["rangefinder"]
                 self.should_send = data["should_send"]
+                self.confidence_cutoff = data.get("confidence_cutoff", 80)
                 logger.debug("Loaded settings: ", data)
         except FileNotFoundError:
             logger.warning("Settings file not found, using default.")
@@ -120,6 +122,7 @@ def ensure_dir(file_path) -> None:
                         "hostname": self.hostname,
                         "rangefinder": self.rangefinder,
                         "should_send": self.should_send,
+                        "confidence_cutoff": self.confidence_cutoff,
                     }
                 )
             )
@@ -136,6 +139,7 @@ def get_status(self) -> dict:
             "origin": self.origin,
             "rangefinder": self.rangefinder,
             "should_send": self.should_send,
+            "confidence_cutoff": self.confidence_cutoff,
         }
 
     @property
@@ -273,6 +277,14 @@ def set_enabled(self, enable: bool) -> bool:
         self.save_settings()
         return True
 
+    def set_confidence_cutoff(self, cutoff: int) -> bool:
+        """
+        Enables/disables the driver
+        """
+        self.confidence_cutoff = max(5, min(100, cutoff))
+        self.save_settings()
+        return True
+
     def set_use_as_rangefinder(self, enable: bool) -> bool:
         """
         Enables/disables DISTANCE_SENSOR messages
@@ -382,7 +394,8 @@ def handle_velocity(self, data: Dict[str, Any]) -> None:
             return
 
         if self.rangefinder and alt > 0.05:
-            self.mav.send_rangefinder(alt, confidence)
+            if confidence >= self.confidence_cutoff:
+                self.mav.send_rangefinder(alt, confidence)
 
         position_delta = [0, 0, 0]
         attitude_delta = [0, 0, 0]
diff --git a/dvl-a50/main.py b/dvl-a50/main.py
index f4774af..c80a526 100755
--- a/dvl-a50/main.py
+++ b/dvl-a50/main.py
@@ -35,6 +35,12 @@ def set_enabled(self, enabled: str) -> bool:
             return self.dvl.set_enabled(enabled == "true")
         return False
 
+    def set_cutoff(self, cutoff: int) -> bool:
+        """
+        Enables/Disables the DVL driver
+        """
+        return self.dvl.set_confidence_cutoff(int(cutoff))
+
     def set_orientation(self, orientation: int) -> bool:
         """
         Sets the DVL mounting orientation:
@@ -87,6 +93,10 @@ def get_status():
     def set_enabled(enable: str):
         return str(api.set_enabled(enable))
 
+    @app.route("/cutoff/<cutoff>")
+    def set_cutoff(cutoff: int):
+        return str(api.set_cutoff(cutoff))
+
     @app.route("/use_as_rangefinder/<enable>")
     def set_use_rangefinder(enable: str):
         return str(api.set_use_as_rangefinder(enable))