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