From ae7d67685c4939762a72e5f7e2c4421bdcbca0c1 Mon Sep 17 00:00:00 2001
From: Clyde McQueen <clyde@mcqueen.net>
Date: Sat, 29 Jun 2024 15:34:31 -0700
Subject: [PATCH] Let user choose which EK3_SRC params to change

---
 dvl-a50/dvl.py            | 26 +++++++++++++++++++++-----
 dvl-a50/main.py           | 12 ++++++++++++
 dvl-a50/static/index.html | 10 ++++++++++
 3 files changed, 43 insertions(+), 5 deletions(-)

diff --git a/dvl-a50/dvl.py b/dvl-a50/dvl.py
index 9b54e51..5a6bb3c 100644
--- a/dvl-a50/dvl.py
+++ b/dvl-a50/dvl.py
@@ -283,6 +283,26 @@ def set_use_as_rangefinder(self, enable: bool) -> bool:
             self.mav.set_param("RNGFND1_TYPE", "MAV_PARAM_TYPE_UINT8", 10)  # MAVLINK
         return True
 
+    def load_params(self, selector: str) -> bool:
+        """
+        Load EK3_SRC1 parameters to match the use case:
+        "dvl"       The DVL will be used for horizontal position and velocity
+        "dvl_gps"   The GPS will be used for horizontal position, and the DVL will be used for horizontal velocity
+        """
+        if selector == "dvl":
+            self.mav.set_param("EK3_GPS_TYPE", "MAV_PARAM_TYPE_UINT8", 3)  # Disable
+            self.mav.set_param("EK3_SRC1_POSXY", "MAV_PARAM_TYPE_UINT8", 6)  # EXTNAV
+            self.mav.set_param("EK3_SRC1_VELXY", "MAV_PARAM_TYPE_UINT8", 6)  # EXTNAV
+            self.mav.set_param("EK3_SRC1_POSZ", "MAV_PARAM_TYPE_UINT8", 1)  # BARO
+            return True
+        if selector == "dvl_gps":
+            self.mav.set_param("EK3_GPS_TYPE", "MAV_PARAM_TYPE_UINT8", 0)  # Enable
+            self.mav.set_param("EK3_SRC1_POSXY", "MAV_PARAM_TYPE_UINT8", 3)  # GPS
+            self.mav.set_param("EK3_SRC1_VELXY", "MAV_PARAM_TYPE_UINT8", 6)  # EXTNAV
+            self.mav.set_param("EK3_SRC1_POSZ", "MAV_PARAM_TYPE_UINT8", 1)  # BARO
+            return True
+        return False
+
     def setup_mavlink(self) -> None:
         """
         Sets up mavlink streamrates so we have the needed messages at the
@@ -294,7 +314,7 @@ def setup_mavlink(self) -> None:
 
     def setup_params(self) -> None:
         """
-        Sets up the required params for DVL integration
+        Sets up the required params for DVL integration -- but leave the EK3_SRC1 params alone
         """
         self.mav.set_param("AHRS_EKF_TYPE", "MAV_PARAM_TYPE_UINT8", 3)
         # TODO: Check if really required. It doesn't look like the ekf2 stops at all
@@ -302,10 +322,6 @@ def setup_params(self) -> None:
 
         self.mav.set_param("EK3_ENABLE", "MAV_PARAM_TYPE_UINT8", 1)
         self.mav.set_param("VISO_TYPE", "MAV_PARAM_TYPE_UINT8", 1)
-        self.mav.set_param("EK3_GPS_TYPE", "MAV_PARAM_TYPE_UINT8", 3)
-        self.mav.set_param("EK3_SRC1_POSXY", "MAV_PARAM_TYPE_UINT8", 6)  # EXTNAV
-        self.mav.set_param("EK3_SRC1_VELXY", "MAV_PARAM_TYPE_UINT8", 6)  # EXTNAV
-        self.mav.set_param("EK3_SRC1_POSZ", "MAV_PARAM_TYPE_UINT8", 1)  # BARO
         if self.rangefinder:
             self.mav.set_param("RNGFND1_TYPE", "MAV_PARAM_TYPE_UINT8", 10)  # MAVLINK
 
diff --git a/dvl-a50/main.py b/dvl-a50/main.py
index 8255a24..f4774af 100755
--- a/dvl-a50/main.py
+++ b/dvl-a50/main.py
@@ -63,6 +63,14 @@ def set_use_as_rangefinder(self, enabled: str) -> bool:
             return self.dvl.set_use_as_rangefinder(enabled == "true")
         return False
 
+    def load_params(self, selector: str) -> bool:
+        """
+        Load parameters
+        """
+        if selector in ["dvl", "dvl_gps"]:
+            return self.dvl.load_params(selector)
+        return False
+
     def set_message_type(self, messagetype: str):
         self.dvl.set_should_send(messagetype)
 
@@ -83,6 +91,10 @@ def set_enabled(enable: str):
     def set_use_rangefinder(enable: str):
         return str(api.set_use_as_rangefinder(enable))
 
+    @app.route("/load_params/<selector>")
+    def load_params(selector: str):
+        return str(api.load_params(selector))
+
     @app.route("/orientation/<int:orientation>")
     def set_orientation(orientation: int):
         return str(api.set_orientation(orientation))
diff --git a/dvl-a50/static/index.html b/dvl-a50/static/index.html
index 39002d5..4809982 100644
--- a/dvl-a50/static/index.html
+++ b/dvl-a50/static/index.html
@@ -42,6 +42,8 @@ <h1>DVL Configuration</h1>
 							<h2>Settings</h2>
 							<v-switch v-model="this.enabled" inset :label="`Enable DVL driver`" @change="setDvlEnabled($event)"></v-switch>
 							<v-switch v-model="this.rangefinder" inset :label="`Send range data through MAVLink`" @change="setDvlAsRangefinder($event)"></v-switch>
+							<v-btn id="btn-load-dvl" type="button" class="btn btn-primary" @click="loadParams('dvl');">Load parameters for DVL</v-btn>
+							<v-btn id="btn-load-dvl-gps" type="button" class="btn btn-primary" @click="loadParams('dvl_gps');">Load parameters for DVL+GPS</v-btn>
 							<h2>Status</h2>
 							<div>
 								<textarea readonly style="width:100%;"
@@ -228,6 +230,14 @@ <h2>Water Linked DVL Configuration Page</h2>
 				request.send();
 			},
 
+			/* Load parameters */
+			loadParams(value) {
+				const request = new XMLHttpRequest();
+				request.timeout = 800;
+				request.open('GET', 'load_params/' + value, true);
+				request.send();
+			},
+
 			/* Sets dvl orientation (Down/Forward) */
 			setDvlOrientation(orientation) {
 				const request = new XMLHttpRequest();