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.

Sign up for GitHub

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

Allow using SPEED_ESTIMATE messages #42

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

Allow using SPEED_ESTIMATE messages #42

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
c29ceff
Improve WL/MAVLink protocol support
ES-Alexander Jul 4, 2024
ea7a93d
frontend: allow testing alternate MAVLink message options
ES-Alexander Jul 4, 2024
8a401e7
index.html: fix potentially unreachable code
ES-Alexander Jul 4, 2024
4d51a48
fix incorrect rangefinder type
ES-Alexander Jul 5, 2024
9ec204a
add forward DVL orientation support
ES-Alexander Jul 5, 2024
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
23 changes: 21 additions & 2 deletions dvl-a50/dvl.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -366,7 +366,8 @@ def handle_velocity(self, data: Dict[str, Any]) -> None:
return

if self.rangefinder and alt > 0.05:
self.mav.send_rangefinder(alt)
orientation = "MAV_SENSOR_ROTATION_PITCH_270" if self.current_orientation == DVL_DOWN else "MAV_SENSOR_ROTATION_NONE"
self.mav.send_rangefinder(alt, orientation)

if self.should_send == MessageType.POSITION_DELTA:
dRoll, dPitch, dYaw = [
Expand All @@ -382,7 +383,25 @@ def handle_velocity(self, data: Dict[str, Any]) -> None:
self.mav.send_vision(position_delta, attitude_delta, dt=data["time"] * 1e3, confidence=confidence)
elif self.should_send == MessageType.SPEED_ESTIMATE:
velocity = [vx, vy, vz] if self.current_orientation == DVL_DOWN else [vz, vy, -vx] # DVL_FORWARD
ES-Alexander marked this conversation as resolved.
Show resolved Hide resolved
self.mav.send_vision_speed_estimate(velocity)
covariance = data.get("covariance") # covariance was not provided in old protocol versions
if covariance is not None:
((vxx, vxy, vxz), (vyx, vyy, vyz), (vzx, vzy, vzz)) = covariance # Extract values
# Flatten 3x3 into 9 element list, with order determined by orientation.
# Shown in matrix form to help with visualisation.
if self.current_orientation == DVL_DOWN:
covariance = [
vxx, vxy, vxz,
vyx, vyy, vyz,
vzx, vzy, vzz
]
else: # DVL_FORWARD
covariance = [
vzz, vzy, -vzx,
vyz, vyy, -vyx,
-vxz, -vxy, vxx
]

self.mav.send_vision_speed_estimate(velocity, covariance, reset_counter=self.reset_counter)

self.last_attitude = self.current_attitude

Expand Down
69 changes: 23 additions & 46 deletions dvl-a50/mavlink2resthelper.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,18 +65,8 @@ def __init__(self, vehicle: int = 1, component: int = 1):
"x": {vx},
"y": {vy},
"z": {vz},
"covariance": [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
"reset_counter": 0
"covariance": {covariance},
"reset_counter": {reset_counter}
}}
}}"""

Expand All @@ -96,29 +86,7 @@ def __init__(self, vehicle: int = 1, component: int = 1):
"roll": {roll},
"pitch": {pitch},
"yaw": {yaw},
"covariance": [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
],
"covariance": {covariance},
"reset_counter": {reset_counter}
}}
}}"""
Expand Down Expand Up @@ -153,13 +121,13 @@ def __init__(self, vehicle: int = 1, component: int = 1):
"time_boot_ms": 0,
"min_distance": 0,
"max_distance": 5000,
"current_distance": {0},
"current_distance": {distance},
"mavtype": {{
"type": "MAV_DISTANCE_SENSOR_LASER"
"type": "MAV_DISTANCE_SENSOR_ULTRASOUND"
}},
"id": 0,
"orientation": {{
"type": "MAV_SENSOR_ROTATION_PITCH_270"
"type": "{orientation}"
}},
"covariance": 0,
"horizontal_fov": 0.0,
Expand Down Expand Up @@ -311,7 +279,7 @@ def send_statustext(self, text: str, severity: str = "MAV_SEVERITY_EMERGENCY"):
return False

def send_vision(self, position_deltas, rotation_deltas=(0, 0, 0), confidence=100, dt=125000):
"Sends message VISION_POSITION_DELTA to flight controller"
"Sends message ardupilotmega.VISION_POSITION_DELTA to flight controller"
data = self.vision_template.format(
dt=int(dt),
dRoll=radians(rotation_deltas[0]),
Expand All @@ -325,21 +293,29 @@ def send_vision(self, position_deltas, rotation_deltas=(0, 0, 0), confidence=100

post(MAVLINK2REST_URL + "/mavlink", data=data)

def send_vision_speed_estimate(self, speed_estimates):
"Sends message VISION_SPEED_ESTIMATE to flight controller"
def send_vision_speed_estimate(self, speed_estimates, covariance=None, reset_counter=0):
"Sends message common.VISION_SPEED_ESTIMATE to flight controller"
if covariance is None:
covariance = [float("NaN"), *[0.0] * 8] # NaN first element means unknown

data = self.vision_speed_estimate_template.format(
us=int((time.time() - self.start_time) * 1e6),
vx=speed_estimates[0],
vy=speed_estimates[1],
vz=speed_estimates[2],
covariance=covariance,
reset_counter=reset_counter,
)

post(MAVLINK2REST_URL + "/mavlink", data=data)

def send_vision_position_estimate(
self, timestamp, position_estimates, attitude_estimates=(0.0, 0.0, 0.0), reset_counter=0
self, timestamp, position_estimates, attitude_estimates=(0.0, 0.0, 0.0), covariance=None, reset_counter=0
):
"Sends message GLOBAL_VISION_POSITION_ESTIMATE to flight controller"
"Sends message common.GLOBAL_VISION_POSITION_ESTIMATE to flight controller"
if covariance is None:
covariance = [float("NaN"), *[0.0] * 20] # NaN first element means unknown

data = self.global_vision_position_estimate_template.format(
us=int(timestamp * 1e3),
roll=radians(attitude_estimates[0]),
Expand All @@ -348,15 +324,16 @@ def send_vision_position_estimate(
x=position_estimates[0],
y=position_estimates[1],
z=position_estimates[2],
covariance=covariance,
reset_counter=reset_counter,
)
logger.info(post(MAVLINK2REST_URL + "/mavlink", data=data))

def send_rangefinder(self, distance: float):
"Sends message DISTANCE_SENSOR to flight controller"
def send_rangefinder(self, distance: float, orientation: str):
"Sends message common.DISTANCE_SENSOR to flight controller"
if distance == -1:
return
data = self.rangefinder_template.format(int(distance * 100))
data = self.rangefinder_template.format(distance=int(distance * 100), orientation=orientation)

post(MAVLINK2REST_URL + "/mavlink", data=data)

Expand Down
7 changes: 2 additions & 5 deletions dvl-a50/static/index.html
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,6 @@ <h2>Status</h2>
</div>
</v-card>
</v-col>
<!-- uncoomment this to have control of what kind of message is bein sent and to allow pointing the dvl forward-->
<!--
<v-col>
<v-card height="200">
<h2>Message type:</h2>
Expand All @@ -69,7 +67,6 @@ <h2>Orientation:</h2>
</v-card>
</v-col>
<v-col>
-->
<v-col
class="col-12 col-sm-6 col-md-8">
<v-card height="650">
Expand Down Expand Up @@ -126,7 +123,7 @@ <h2>Water Linked DVL Configuration Page</h2>
rovMarker: undefined,
newHostname: null,
rovPosition: [],
messageOptions: ["POSITION_DELTA", "POSITION_ESTIMATE", "SPEED_ESTIMATE"],
messageOptions: ["POSITION_DELTA", "SPEED_ESTIMATE", "POSITION_ESTIMATE"],
orientationOptions: {
"Downwards (LED pointing forward)": 1,
"Forward (Experimental)": 2,
Expand All @@ -143,7 +140,7 @@ <h2>Water Linked DVL Configuration Page</h2>
this.enabled = data.enabled
this.orientation = data.orientation
this.origin = data.origin
if (this.newOrigin === ['0','0']) {
if (this.newOrigin == ['0','0']) {
this.newOrigin = data.origin
}
this.rangefinder = data.rangefinder
Expand Down
Loading