Skip to content

Commit

Permalink
dvl.py: fix incorrect attitude handling from DVL
Browse files Browse the repository at this point in the history
  • Loading branch information
ES-Alexander authored Dec 11, 2023
1 parent 7bb1bb5 commit b2667b2
Showing 1 changed file with 2 additions and 1 deletion.
3 changes: 2 additions & 1 deletion dvl-a50/dvl.py
Original file line number Diff line number Diff line change
Expand Up @@ -384,7 +384,8 @@ def handle_velocity(self, data: Dict[str, Any]) -> None:
self.last_attitude = self.current_attitude

def handle_position_local(self, data):
self.current_attitude = data["roll"], data["pitch"], data["yaw"]
# DVL provides rotation angles in degrees, but MAVLink expects them in radians
self.current_attitude = [math.radians(data[axis]) for axis in ("roll", "pitch", "yaw")]
if self.should_send == MessageType.POSITION_ESTIMATE:
x, y, z = data["x"], data["y"], data["z"]
self.timestamp = data["ts"]
Expand Down

0 comments on commit b2667b2

Please sign in to comment.