Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Let user choose which EK3_SRC params to change #41

Merged
merged 1 commit into from
Jul 11, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 21 additions & 5 deletions dvl-a50/dvl.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -294,18 +314,14 @@ 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
self.mav.set_param("EK2_ENABLE", "MAV_PARAM_TYPE_UINT8", 0)

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

Expand Down
12 changes: 12 additions & 0 deletions dvl-a50/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand All @@ -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))
Expand Down
10 changes: 10 additions & 0 deletions dvl-a50/static/index.html
Original file line number Diff line number Diff line change
Expand Up @@ -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%;"
Expand Down Expand Up @@ -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();
Expand Down
Loading