WaterLinked DVL rotational velocity

General Discussion
1

I am working with a waterlinked DVL integrated into my blueROV and I am wondering if the rotational velocity of the vehicle is incorporated in any way when reporting vehicle velocity to the EKF onboard?

For example, if the vehicle is spinning, the translational velocity should ideally be zero if the coordinate system origin is the center of the vehicle. My DVL is offset by 6 inches or so, so rotational velocity produces significant velocity reading which is not technically correct to report to EKF

Has anyone thought about adding this offset to the Waterlinked DVL code, or is it already incorporated somewhere?

1 Like
2

After looking into this a bit deeper, it seems that even though waterlinked DVL obviously has a gyro, they do not expose that in their api. Would it be possible to add parameters for DVL offset that can factor into the EKF calculation?

3

How do we feel about this change? I don’t have access to my vehicle today, but I can try it tomorrow. My goal is to be able to rotate the vehicle in yaw and have the position_delta be reported at zero. As the code was, rotating the vehicle would induce a surge/sway velocity, but this should account for that. I would like to remove hard-coded position offsets and add as params, but that’s not my concern at the moment

def handle_velocity(self, data: Dict[str, Any]) -> None:
        # extract velocity data from the DVL JSON
        vx, vy, vz, alt, valid, fom = (
            data["vx"],
            data["vy"],
            data["vz"],
            data["altitude"],
            data["velocity_valid"],
            data["fom"],
        )
        dt = data["time"] / 1000
        dx = dt * vx
        dy = dt * vy
        dz = dt * vz

        # fom is the standard deviation. scaling it to a confidence from 0-100%
        # 0 is a very good measurement, 0.4 is considered a inaccurate measurement
        _fom_max = 0.4
        confidence = 100 * (1 - min(_fom_max, fom) / _fom_max) if valid else 0
        # confidence = 100 if valid else 0

        if not valid:
            logger.info("Invalid  dvl reading, ignoring it.")
            return

        if self.rangefinder and alt > 0.05:
            self.mav.send_rangefinder(alt)

        ####################### DVL OFFSET CORRECTION ####################### 
        # Add correction for DVL offset 
        attitude = json.loads(self.mav.get("/ATTITUDE/message"))
        # extract the roll, pitch and yaw speed from the attitude message in radians
        attitudeRates = np.array([attitude[axis] for axis in ("rollspeed", "pitchspeed", "yawspeed")])
        # define DVL offset from center of vehicle in meters
        dvlOffsetBODYCS = np.array([-0.1905, -0.1143, 0.1397])
        ####################### DVL OFFSET CORRECTION #######################

        if self.should_send == MessageType.POSITION_DELTA:
            dRoll, dPitch, dYaw = [
                current_angle - last_angle
                for (current_angle, last_angle) in zip(self.current_attitude, self.last_attitude)
            ]
            if self.current_orientation == DVL_DOWN:
                # position_delta = [dx, dy, dz]

                # calculate the adjusted body coordinate system velocities
                position_delta = list( (np.array((vx, vy, vz)) + np.cross(dvlOffsetBODYCS, attitudeRates)) * dt)

                attitude_delta = [dRoll, dPitch, dYaw]
            elif self.current_orientation == DVL_FORWARD:
                position_delta = [dz, dy, -dx]
                attitude_delta = [dYaw, dPitch, -dRoll]

            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
            
            # calculate the adjusted body coordinate system velocities
            if self.current_orientation == DVL_DOWN:
                velocity = list( (np.array((vx, vy, vz)) + np.cross(dvlOffsetBODYCS, attitudeRates)) )

            self.mav.send_vision_speed_estimate(velocity)

        self.last_attitude = self.current_attitude
4

I am struggling to add this to my waterlinked dvl dockerfile. Does anyone have any ideas on this?

5

Hi @cmarq –

I think that ArduSub already handles this offset via the VISO_POS parameters: ardupilot/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp at d7ae8b8bb3dbd3a076f832c753feabf38faf9a02 · ArduPilot/ardupilot · GitHub

Caveat: I haven’t tested this. OTOH, if it doesn’t work, then I would argue that it’s a bug in EKF3.

6

Hi Clyde,

That does look to be a promising. I will have to look at the code more closely and track exactly what is happening there.

Luckily, I was able to rebuilt a new dockerfile that does the rotation appropriately.

Would be nice to confirm for others that this exists in the ardupilot codebase already

1 Like