floppyrj45/cosma-nav
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: viewer split 3D+2D — AUV depth chart + USBL XY fix

Browse Source
This commit is contained in:
Floppyrj45
2026-04-24 20:56:17 +02:00
parent 36c7889db1
commit 279e70a5e0
3 changed files with 159 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
viz/server.py
View File

@@ -37,26 +37,48 @@ def _load_data() -> dict:
if "auv_mcap" in f:
dep = f["auv_mcap/depth_m"][:]
t_auv = f["auv_mcap/t_ns"][:].tolist()
# AUV lat/lon zeros — plot depth over time as Z only
n_pts = len(dep)
out["auv_depth"] = {
"x": list(range(n_pts)), # index as proxy for time
"y": [0.0] * n_pts,
"z": (-dep).tolist(), # positive = deeper
"t": t_auv,
t_ns_auv = f["auv_mcap/t_ns"][:]
alt = f["auv_mcap/altitude_m"][:] if "auv_mcap/altitude_m" in f \
else np.full(len(dep), np.nan)
step = max(1, len(dep) // 600)
t_s = ((t_ns_auv[::step].astype(float) - float(t_ns_auv[0])) / 1e9).tolist()
out["auv_profile"] = {
"t_s": t_s,
"depth_m": dep[::step].tolist(),
"altitude_m": alt[::step].tolist(),
}
if "usbl_fixes" in f:
north = f["usbl_fixes/north_m"][:]
east = f["usbl_fixes/east_m"][:]
depth = f["usbl_fixes/depth_m"][:]
valid = ~(np.isnan(north) | (north == 0) & (east == 0))
if "usbl_fixes" in f and "usv_gps" in f:
bearing = f["usbl_fixes/bearing_deg"][:]
range_m = f["usbl_fixes/range_m"][:]
usbl_dep = f["usbl_fixes/depth_m"][:]
t_usbl = f["usbl_fixes/t_ns"][:]
usv_e_raw = f["usv_gps/easting"][:]
usv_n_raw = f["usv_gps/northing"][:]
usv_t_raw = f["usv_gps/t_ns"][:]
hdg_raw = f["usv_gps/heading_deg"][:] \
if "usv_gps/heading_deg" in f \
else np.full(len(usv_t_raw), np.nan)
valid = ~(np.isnan(bearing) | np.isnan(range_m) | np.isnan(usbl_dep))
if valid.any():
out["usbl"] = {
"x": east[valid].tolist(),
"y": north[valid].tolist(),
"z": (-depth[valid]).tolist(),
t_f = t_usbl[valid].astype(float)
t_usv = usv_t_raw.astype(float)
usv_ei = np.interp(t_f, t_usv, usv_e_raw)
usv_ni = np.interp(t_f, t_usv, usv_n_raw)
hdg_i = np.interp(t_f, t_usv, hdg_raw) \
if not np.all(np.isnan(hdg_raw)) \
else np.zeros(valid.sum())
range_h = np.sqrt(np.maximum(0.0, range_m[valid]**2 - usbl_dep[valid]**2))
angle = np.deg2rad(hdg_i + bearing[valid])
auv_e = usv_ei + range_h * np.sin(angle)
auv_n = usv_ni + range_h * np.cos(angle)
ce = out.get("origin", {}).get("easting", 0.0)
cn = out.get("origin", {}).get("northing", 0.0)
out["auv_usbl"] = {
"x": (auv_e - ce).tolist(),
"y": (auv_n - cn).tolist(),
"z": (-usbl_dep[valid]).tolist(),
}
except Exception as e:
out["error_fixes"] = str(e)