diff --git a/.gitignore b/.gitignore index c74db46..db1ad63 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,8 @@ data/ output/ __pycache__/ *.pyc +.venv/ +data/ +viewer/data/ +viewer/screenshots/ +screenshots/ diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..a63d215 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "vendor/Kogger-Protocol"] + path = vendor/Kogger-Protocol + url = https://github.com/koggertech/Kogger-Protocol.git diff --git a/tools/check_sync.py b/tools/check_sync.py new file mode 100644 index 0000000..72cb8dd --- /dev/null +++ b/tools/check_sync.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +"""Check temporal alignment between MCAP AUV, USV PWM, and USBL data.""" +import json, os, sys +from datetime import datetime, timezone + +def fmt(ms): + if ms == 0: return 'N/A' + return datetime.fromtimestamp(ms/1000, tz=timezone.utc).strftime('%Y-%m-%d %H:%M:%S') + +def load(path): + with open(path) as f: + return json.load(f) + +base = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'output') + +sources = {} + +# MCAP signals +mcap_path = os.path.join(base, 'mcap_signals.json') +if os.path.exists(mcap_path): + d = load(mcap_path) + n = len(d.get('depth',[])) + len(d.get('pwm_auv',{}).get('samples',[])) + len(d.get('state',[])) + sources['MCAP AUV'] = {'t_min': d['t_min_utc_ms'], 't_max': d['t_max_utc_ms'], 'n': n} +else: + print(f"MISSING: {mcap_path}") + +# USV PWM +usv_path = os.path.join(base, 'usv_pwm.json') +if os.path.exists(usv_path): + d = load(usv_path) + n = sum(len(v) for v in d.get('M',{}).values()) + sum(len(v) for v in d.get('RC',{}).values()) + sources['USV PWM'] = {'t_min': d['t_min_utc_ms'], 't_max': d['t_max_utc_ms'], 'n': n} +else: + print(f"MISSING: {usv_path}") + +# USBL +usbl_path = os.path.join(base, 'usbl.json') +if os.path.exists(usbl_path): + d = load(usbl_path) + pts = d.get('points', []) + if pts: + t_vals = [p['t_ms'] for p in pts] + sources['USBL'] = {'t_min': min(t_vals), 't_max': max(t_vals), 'n': len(pts)} + else: + sources['USBL'] = {'t_min': 0, 't_max': 0, 'n': 0} +else: + print(f"MISSING: {usbl_path}") + +print(f"\n{'Source':<12} | {'t_min UTC':<20} | {'t_max UTC':<20} | {'n_pts':>6}") +print('-' * 68) +for name, s in sources.items(): + print(f"{name:<12} | {fmt(s['t_min']):<20} | {fmt(s['t_max']):<20} | {s['n']:>6}") + +# Overlap MCAP vs USV +if 'MCAP AUV' in sources and 'USV PWM' in sources: + mcap = sources['MCAP AUV'] + usv = sources['USV PWM'] + overlap_ms = min(mcap['t_max'], usv['t_max']) - max(mcap['t_min'], usv['t_min']) + print(f"\nMCAP t_min: {fmt(mcap['t_min'])} UTC") + print(f"USV t_min: {fmt(usv['t_min'])} UTC") + diff_min = (mcap['t_min'] - usv['t_min']) / 60000 + print(f"t_min diff: {diff_min:+.1f} min (MCAP vs USV)") + if overlap_ms > 60000: + print(f"OK - overlap: {overlap_ms//1000} s") + elif overlap_ms < 0: + print(f"WARNING: no overlap! gap = {-overlap_ms//1000} s") + else: + print(f"SUSPECT: overlap <60s: {overlap_ms//1000} s") + +if __name__ == '__main__': + pass diff --git a/tools/extract_mcap_signals.py b/tools/extract_mcap_signals.py new file mode 100644 index 0000000..35ea7df --- /dev/null +++ b/tools/extract_mcap_signals.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +"""Extract AUV signals from MCAP files: depth, PWM, state.""" +import argparse, glob, json, os, sys + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--session-dir', required=True) + parser.add_argument('--max-pts', type=int, default=5000) + args = parser.parse_args() + + session_name = os.path.basename(args.session_dir.rstrip('/')) + pattern = os.path.join(args.session_dir, '*.mcap') + mcap_files = sorted(glob.glob(pattern)) + if not mcap_files: + print(f"No MCAP files in {args.session_dir}", file=sys.stderr) + sys.exit(1) + print(f"Found {len(mcap_files)} MCAP files") + + try: + from mcap.reader import make_reader + from mcap_ros2.decoder import DecoderFactory + except ImportError as e: + print(f"Import error: {e}", file=sys.stderr) + sys.exit(1) + + depth_raw = [] + pwm_raw = [] + state_raw = [] + TOPICS = ['/mavros/imu/static_pressure', '/mavros/rc/out', '/mavros/state'] + + for mcap_file in mcap_files: + try: + with open(mcap_file, 'rb') as f: + reader = make_reader(f, decoder_factories=[DecoderFactory()]) + for schema, channel, message, ros_msg in reader.iter_decoded_messages(topics=TOPICS): + t_ms = message.publish_time // 1_000_000 + topic = channel.topic + if topic == '/mavros/imu/static_pressure': + try: + p = float(ros_msg.fluid_pressure) + depth_m = (p - 101325.0) / (1025.0 * 9.80665) + depth_raw.append({'t': t_ms, 'v': round(depth_m, 4)}) + except Exception: + pass + elif topic == '/mavros/rc/out': + try: + ch = list(ros_msg.channels) + pwm_raw.append({'t': t_ms, 'v': ch}) + except Exception: + pass + elif topic == '/mavros/state': + try: + state_raw.append({'t': t_ms, 'mode': str(ros_msg.mode), 'armed': bool(ros_msg.armed)}) + except Exception: + pass + except Exception as e: + print(f" Skip {os.path.basename(mcap_file)}: {e}") + + def sample(lst, max_pts): + if len(lst) <= max_pts: + return lst + stride = len(lst) // max_pts + sampled = lst[::stride] + if sampled[-1] is not lst[-1]: + sampled.append(lst[-1]) + return sampled + + depth = sample(depth_raw, args.max_pts) + pwm_samples = sample(pwm_raw, args.max_pts) + state = state_raw # events, keep all + + all_t = [p['t'] for p in depth_raw + pwm_raw + state_raw] + t_min = min(all_t) if all_t else 0 + t_max = max(all_t) if all_t else 0 + + n_ch = max((len(s['v']) for s in pwm_raw), default=0) + channels = list(range(n_ch)) + + from datetime import datetime, timezone + fmt = lambda ms: datetime.fromtimestamp(ms/1000, tz=timezone.utc).isoformat() + print(f"depth: {len(depth)} pts (raw {len(depth_raw)})") + if depth: + dvals = [p['v'] for p in depth] + print(f" depth range: {min(dvals):.3f} .. {max(dvals):.3f} m") + print(f"pwm_auv: {len(pwm_samples)} samples (raw {len(pwm_raw)}), {n_ch} channels") + print(f"state: {len(state)} events") + print(f"t_min: {fmt(t_min)}") + print(f"t_max: {fmt(t_max)}") + + out = { + 'session': session_name, + 't_min_utc_ms': t_min, + 't_max_utc_ms': t_max, + 'depth': depth, + 'pwm_auv': {'channels': channels, 'samples': pwm_samples}, + 'state': state, + } + + outdir = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'output') + os.makedirs(outdir, exist_ok=True) + outpath = os.path.join(outdir, 'mcap_signals.json') + with open(outpath, 'w') as f: + json.dump(out, f) + print(f"Written: {outpath}") + +if __name__ == '__main__': + main() diff --git a/tools/extract_usv_pwm.py b/tools/extract_usv_pwm.py new file mode 100644 index 0000000..c0b98ea --- /dev/null +++ b/tools/extract_usv_pwm.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +"""Extract USV PWM signals from navigation log CSVs.""" +import argparse, csv, glob, json, os, re, sys +from datetime import datetime, timezone + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--nav-dir', required=True) + args = parser.parse_args() + + pattern = os.path.join(args.nav_dir, '*_navigation_log.csv') + csv_files = sorted(glob.glob(pattern)) + if not csv_files: + print(f"No navigation_log.csv in {args.nav_dir}", file=sys.stderr) + sys.exit(1) + print(f"Found {len(csv_files)} nav CSV files") + + M_data = {} + RC_data = {} + + for csv_file in csv_files: + print(f" Parsing {os.path.basename(csv_file)}") + try: + with open(csv_file, 'r') as f: + reader = csv.DictReader(f) + for row in reader: + ts_str = row.get('timestamp', '').strip() + data = row.get('data', '').strip() + val_str = row.get('value', '').strip() + if not ts_str or not data or not val_str: + continue + is_M = re.match(r'^M\d+$', data) + is_RC = re.match(r'^RC\d+$', data) + if not is_M and not is_RC: + continue + try: + val = float(val_str) + except ValueError: + continue + try: + dt = datetime.strptime(ts_str, '%Y-%m-%d %H:%M:%S.%f') + except ValueError: + try: + dt = datetime.strptime(ts_str, '%Y-%m-%d %H:%M:%S') + except ValueError: + continue + # CET -> UTC: subtract 3600s + t_ms = int(dt.timestamp() * 1000) - 3600 * 1000 + pt = {'t': t_ms, 'v': val} + if is_M: + M_data.setdefault(data, []).append(pt) + else: + RC_data.setdefault(data, []).append(pt) + except Exception as e: + print(f" Error {csv_file}: {e}") + + all_t = [] + for pts in list(M_data.values()) + list(RC_data.values()): + all_t.extend(p['t'] for p in pts) + t_min = min(all_t) if all_t else 0 + t_max = max(all_t) if all_t else 0 + + for k in sorted(M_data): + print(f" {k}: {len(M_data[k])} pts") + for k in sorted(RC_data): + print(f" {k}: {len(RC_data[k])} pts") + + fmt = lambda ms: datetime.fromtimestamp(ms/1000, tz=timezone.utc).isoformat() + print(f"t_min UTC: {fmt(t_min)}") + print(f"t_max UTC: {fmt(t_max)}") + + out = { + 'tz_assumed': 'CET (UTC+1)', + 'tz_converted_to': 'UTC', + 't_min_utc_ms': t_min, + 't_max_utc_ms': t_max, + 'M': M_data, + 'RC': RC_data, + } + + outdir = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'output') + os.makedirs(outdir, exist_ok=True) + outpath = os.path.join(outdir, 'usv_pwm.json') + with open(outpath, 'w') as f: + json.dump(out, f) + print(f"Written: {outpath}") + +if __name__ == '__main__': + main() diff --git a/tools/merge_nav_usbl.py b/tools/merge_nav_usbl.py new file mode 100644 index 0000000..259fd69 --- /dev/null +++ b/tools/merge_nav_usbl.py @@ -0,0 +1,319 @@ +#!/usr/bin/env python3 +""" +merge_nav_usbl.py — Merge USBL decoded data with USV navigation log + +Inputs: + --usbl : combined_usbl.csv (output of parse_kogger_usbl.py) + --nav-dir: directory containing *_navigation_log.csv files + --output : output CSV (default: combined_nav_usbl.csv) + +Nav log format: timestamp,data,value (long format) + data=Lat → latitude_deg + data=Lon → longitude_deg + data=Heading → heading_deg + +Interpolation: for each USBL timestamp, find nearest nav point within 1s window. +If multiple sessions, use session matching by timestamp overlap. + +AUV position calculation: + azimuth_deg from USBL is RELATIVE to USV heading (yaw) based on USBL hardware mounting. + AUV bearing from North = (USV heading + azimuth_deg) mod 360 + Horizontal dist = dist_m * cos(elevation_deg * pi/180) + + Note: if azimuth is already absolute (referenced to North), do NOT add heading. + Check: usbl_yaw in payload should match nav Heading if relative azimuth. + We use RELATIVE convention (add USV heading) — documented here. + +Geodetic forward (haversine): + Using flat-earth approximation valid for distances < 500m: + dlat = horiz_dist * cos(bearing) / R_earth + dlon = horiz_dist * sin(bearing) / (R_earth * cos(lat)) + + R_earth = 6371000 m +""" + +import csv +import io +import sys +import math +import os + +R_EARTH = 6371000.0 # meters + + +def parse_nav_log(nav_file): + """ + Parse navigation_log.csv into: + - nav_points: sorted list of (timestamp_str, lat, lon) from Lat+Lon entries + - heading_series: sorted list of (timestamp_str, heading_deg) from Heading entries + + Lat/Lon and Heading have different timestamps so must be interpolated separately. + Returns (nav_points, heading_series). + """ + lat_by_ts = {} + lon_by_ts = {} + heading_series_raw = [] + + with open(nav_file) as f: + reader = csv.DictReader(f) + for row in reader: + data = row.get('data', '') + ts = row.get('timestamp', '') + try: + val_raw = row.get('value', '') or '' + if not val_raw: + continue + val = float(val_raw) + except (ValueError, TypeError): + continue + if data == 'Lat': + lat_by_ts[ts] = val + elif data == 'Lon': + lon_by_ts[ts] = val + elif data == 'Heading': + heading_series_raw.append((ts, val)) + + # Build nav_points: timestamps where both Lat and Lon appear (same ts) + nav_points = [] + for ts in sorted(set(lat_by_ts.keys()) & set(lon_by_ts.keys())): + nav_points.append((ts, lat_by_ts[ts], lon_by_ts[ts])) + + # heading_series sorted by timestamp + heading_series = sorted(heading_series_raw, key=lambda x: x[0]) + + return nav_points, heading_series + + +def ts_to_seconds(ts_str): + """Convert '2026-03-24 09:29:05.230392' to float seconds since epoch (approx).""" + # Simple: parse date+time, compute offset + try: + date_part, time_part = ts_str.strip().split(' ', 1) + y, mo, d = date_part.split('-') + parts = time_part.split(':') + h, m = int(parts[0]), int(parts[1]) + s_str = parts[2] + s = float(s_str) + # Days since fixed epoch (don't need absolute, just relative diffs) + total = (int(y)*365 + int(mo)*30 + int(d)) * 86400 + h*3600 + m*60 + s + return total + except Exception: + return 0.0 + + +def find_nearest(ts_sec, series_sec, series, max_gap=1.0): + """Find nearest entry in sorted series within max_gap seconds.""" + best_idx = -1 + best_dt = float('inf') + for i, (s_sec, entry) in enumerate(zip(series_sec, series)): + dt = abs(s_sec - ts_sec) + if dt < best_dt: + best_dt = dt + best_idx = i + elif s_sec > ts_sec + max_gap: + break + if best_idx >= 0 and best_dt <= max_gap: + return series[best_idx], best_dt + return None, None + + +def geodetic_forward(lat_deg, lon_deg, bearing_deg, dist_m): + """ + Compute destination point given start lat/lon, bearing (deg from North), distance (m). + Flat-earth approximation valid for dist < 500m. + """ + bearing_rad = math.radians(bearing_deg) + lat_rad = math.radians(lat_deg) + dlat = dist_m * math.cos(bearing_rad) / R_EARTH + dlon = dist_m * math.sin(bearing_rad) / (R_EARTH * math.cos(lat_rad)) + auv_lat = lat_deg + math.degrees(dlat) + auv_lon = lon_deg + math.degrees(dlon) + return auv_lat, auv_lon + + +def haversine_dist(lat1, lon1, lat2, lon2): + """Distance in meters between two lat/lon points.""" + phi1, phi2 = math.radians(lat1), math.radians(lat2) + dphi = math.radians(lat2 - lat1) + dlam = math.radians(lon2 - lon1) + a = math.sin(dphi/2)**2 + math.cos(phi1)*math.cos(phi2)*math.sin(dlam/2)**2 + return 2 * R_EARTH * math.asin(math.sqrt(a)) + + +def find_nav_file_for_session(usbl_file, nav_dir): + """Match nav file by common timestamp prefix.""" + base = os.path.basename(usbl_file) + # e.g. 2026-03-24_09-28-44_USV003_usbl.csv -> 2026-03-24_09-28-44_USV003 + prefix = base.replace('_usbl.csv', '') + nav_candidate = os.path.join(nav_dir, prefix + '_navigation_log.csv') + if os.path.exists(nav_candidate): + return nav_candidate + return None + + +def main(): + import argparse + parser = argparse.ArgumentParser() + parser.add_argument('--usbl', required=True, help='combined_usbl.csv') + parser.add_argument('--nav-dir', required=True, help='Directory with *_navigation_log.csv') + parser.add_argument('--output', default='combined_nav_usbl.csv') + parser.add_argument('--max-gap', type=float, default=1.0, help='Max timestamp gap in seconds') + args = parser.parse_args() + + # Load USBL records + usbl_records = [] + with open(args.usbl) as f: + reader = csv.DictReader(f) + for row in reader: + usbl_records.append(row) + + print("USBL records loaded: %d" % len(usbl_records)) + + # Group by source_file + from collections import defaultdict + by_source = defaultdict(list) + for rec in usbl_records: + by_source[rec.get('source_file', '')].append(rec) + + # Load nav files + nav_data = {} # source_file -> (nav_points, nav_points_sec, heading_series, heading_sec) + nav_dir = args.nav_dir + + for source_file in by_source.keys(): + # Try to match nav file + prefix = source_file.replace('_usbl.csv', '') + nav_file = os.path.join(nav_dir, prefix + '_navigation_log.csv') + if not os.path.exists(nav_file): + # Try to find by scanning directory + matched = None + for fn in os.listdir(nav_dir): + if prefix in fn and 'navigation_log' in fn: + matched = os.path.join(nav_dir, fn) + break + if matched is None: + print("WARNING: no nav file found for %s" % source_file) + nav_data[source_file] = ([], [], [], []) + continue + nav_file = matched + nav_points, heading_series = parse_nav_log(nav_file) + nav_points_sec = [ts_to_seconds(pt[0]) for pt in nav_points] + heading_sec = [ts_to_seconds(h[0]) for h in heading_series] + nav_data[source_file] = (nav_points, nav_points_sec, heading_series, heading_sec) + print("Nav loaded for %s: %d pos points, %d heading points" % ( + source_file, len(nav_points), len(heading_series))) + + # Process and write output + output_rows = [] + stats_match = 0 + stats_nomatch = 0 + + for source_file, records in by_source.items(): + nav_points, nav_points_sec, heading_series, heading_sec = nav_data.get( + source_file, ([], [], [], [])) + + for rec in records: + ts_str = rec.get('Timestamp', '') + ts_sec = ts_to_seconds(ts_str) + + dist_str = rec.get('Dist', '') + azimuth_str = rec.get('Azimuth', '') + elev_str = rec.get('Elev', '') + snr_str = rec.get('SNR', '') + + try: + dist = float(dist_str) if dist_str else float('nan') + azimuth = float(azimuth_str) if azimuth_str else float('nan') + elev = float(elev_str) if elev_str else float('nan') + snr = float(snr_str) if snr_str else float('nan') + except ValueError: + dist, azimuth, elev, snr = float('nan'), float('nan'), float('nan'), float('nan') + + nav_pt, dt = find_nearest(ts_sec, nav_points_sec, nav_points, args.max_gap) + hdg_pt, _ = find_nearest(ts_sec, heading_sec, heading_series, args.max_gap) + + if nav_pt is None: + stats_nomatch += 1 + lat_usv, lon_usv, heading_usv = float('nan'), float('nan'), float('nan') + auv_lat, auv_lon = float('nan'), float('nan') + else: + stats_match += 1 + _, lat_usv, lon_usv = nav_pt + heading_usv = hdg_pt[1] if hdg_pt is not None else float('nan') + + # Calculate AUV absolute position + # Azimuth from USBL is relative to USV heading (yaw convention) + # AUV bearing from North = (USV Heading + azimuth_deg) mod 360 + if not (math.isnan(dist) or math.isnan(azimuth) or + math.isnan(lat_usv) or math.isnan(heading_usv)): + horiz_dist = dist * math.cos(math.radians(elev)) if not math.isnan(elev) else dist + abs_bearing = (heading_usv + azimuth) % 360 + auv_lat, auv_lon = geodetic_forward(lat_usv, lon_usv, abs_bearing, horiz_dist) + else: + auv_lat, auv_lon = float('nan'), float('nan') + + output_rows.append({ + 'Timestamp': ts_str, + 'lat': '%.7f' % lat_usv if not math.isnan(lat_usv) else '', + 'lon': '%.7f' % lon_usv if not math.isnan(lon_usv) else '', + 'Heading': '%.2f' % heading_usv if not math.isnan(heading_usv) else '', + 'Dist': '%.4f' % dist if not math.isnan(dist) else '', + 'Azimuth': '%.4f' % azimuth if not math.isnan(azimuth) else '', + 'Elev': '%.4f' % elev if not math.isnan(elev) else '', + 'SNR': '%.4f' % snr if not math.isnan(snr) else '', + 'auv_lat': '%.7f' % auv_lat if not math.isnan(auv_lat) else '', + 'auv_lon': '%.7f' % auv_lon if not math.isnan(auv_lon) else '', + 'nav_dt_s': '%.3f' % dt if dt is not None else '', + }) + + print("\n=== Merge stats ===") + print("Matched: %d No nav match: %d" % (stats_match, stats_nomatch)) + + with open(args.output, 'w', newline='') as f: + writer = csv.writer(f) + writer.writerow(['Timestamp', 'lat', 'lon', 'Heading', 'Dist', 'Azimuth', 'Elev', 'SNR', + 'auv_lat', 'auv_lon', 'nav_dt_s']) + for row in output_rows: + writer.writerow([ + row['Timestamp'], row['lat'], row['lon'], row['Heading'], + row['Dist'], row['Azimuth'], row['Elev'], row['SNR'], + row['auv_lat'], row['auv_lon'], row['nav_dt_s'] + ]) + + print("Output: %s (%d rows)" % (args.output, len(output_rows))) + + # Sample output + if output_rows: + print("\n=== Sample (first 5 rows) ===") + print("Timestamp,lat,lon,Heading,Dist,Azimuth,Elev,SNR,auv_lat,auv_lon") + for row in output_rows[:5]: + print("%s,%s,%s,%s,%s,%s,%s,%s,%s,%s" % ( + row['Timestamp'], row['lat'], row['lon'], row['Heading'], + row['Dist'], row['Azimuth'], row['Elev'], row['SNR'], + row['auv_lat'], row['auv_lon'])) + + # AUV position stats + auv_lats = [float(r['auv_lat']) for r in output_rows if r['auv_lat']] + auv_lons = [float(r['auv_lon']) for r in output_rows if r['auv_lon']] + usv_lats = [float(r['lat']) for r in output_rows if r['lat']] + usv_lons = [float(r['lon']) for r in output_rows if r['lon']] + + if auv_lats and usv_lats: + print("\n=== Position stats ===") + print("AUV lat range: %.6f - %.6f" % (min(auv_lats), max(auv_lats))) + print("AUV lon range: %.6f - %.6f" % (min(auv_lons), max(auv_lons))) + print("USV lat range: %.6f - %.6f" % (min(usv_lats), max(usv_lats))) + + # Average USV-AUV distance + dists = [] + for r in output_rows: + if r['lat'] and r['auv_lat']: + d = haversine_dist(float(r['lat']), float(r['lon']), + float(r['auv_lat']), float(r['auv_lon'])) + dists.append(d) + if dists: + print("Avg USV-AUV dist: %.2f m (min=%.2f max=%.2f)" % ( + sum(dists)/len(dists), min(dists), max(dists))) + + +if __name__ == '__main__': + main() diff --git a/tools/parse_kogger_usbl.py b/tools/parse_kogger_usbl.py new file mode 100644 index 0000000..bf5a701 --- /dev/null +++ b/tools/parse_kogger_usbl.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python3 +""" +parse_kogger_usbl.py — Decode Kogger USBL raw CSV files (SBP protocol) + +Protocol spec: + Frame: BB 55 | ROUTE | MODE | ID | LENGTH | PAYLOAD[LENGTH] | CHKSUM1 | CHKSUM2 + Checksum: Fletcher-16 over (ROUTE + MODE + ID + LENGTH + PAYLOAD) + +Frame ID 0x65 = ID_USBL_SOLUTION + Struct (packed, little-endian): + id(U1) role(U1) watermark(U2) + timestamp_us(S8) ping_counter(U4) carrier_counter(S8) + distance_m(F4) distance_unc(F4) + azimuth_deg(F4) azimuth_unc(F4) + elevation_deg(F4) elevation_unc(F4) + snr(F4) + x_m(F4) y_m(F4) latitude_deg(D8) longitude_deg(D8) depth_m(F4) + usbl_yaw(F4) usbl_pitch(F4) usbl_roll(F4) + usbl_latitude(D8) usbl_longitude(D8) last_iTOW(U4) + beacon_n(F4) beacon_e(F4) + [+ 32 bytes extra NaN padding observed in firmware v2] + +Timestamp assignment: timestamp from the last RECEIVED packet before the frame sync byte. + +Usage: + python3 parse_kogger_usbl.py FILE1.csv [FILE2.csv ...] -o combined_usbl.csv +""" + +import ast +import csv +import io +import os +import struct +import sys +import collections +import math + +SYNC = b"\xbb\x55" +ID_USBL_SOLUTION = 0x65 + +USBL_FMT = ' len(buf): + continue + route = buf[pos+2] + mode = buf[pos+3] + frame_id = buf[pos+4] + length = buf[pos+5] + if pos + 6 + length + 2 > len(buf): + continue + payload = buf[pos+6:pos+6+length] + chk1_a = buf[pos+6+length] + chk2_a = buf[pos+6+length+1] + + c1, c2 = fletcher16(buf[pos+2:pos+6+length]) + if c1 != chk1_a or c2 != chk2_a: + continue + + valid_total += 1 + frame_id_counter[frame_id] += 1 + + if frame_id != ID_USBL_SOLUTION: + continue + + # Get timestamp: last ts_offset entry before this position + ts = ts_offsets[0][1] if ts_offsets else "" + for off, t in ts_offsets: + if off <= pos: + ts = t + else: + break + + if len(payload) < USBL_FMT_SIZE: + continue + + fields = struct.unpack_from(USBL_FMT, payload) + rec = { + 'Timestamp': ts, + 'usbl_id': fields[0], + 'usbl_role': fields[1], + 'usbl_timestamp_us': fields[3], + 'ping_counter': fields[4], + 'Dist': fields[6], + 'dist_unc': fields[7], + 'Azimuth': fields[8], + 'azimuth_unc': fields[9], + 'Elev': fields[10], + 'elev_unc': fields[11], + 'SNR': fields[12], + 'x_m': fields[13], + 'y_m': fields[14], + 'usbl_lat_computed': fields[15], + 'usbl_lon_computed': fields[16], + 'depth_m': fields[17], + 'usbl_yaw': fields[18], + 'usbl_pitch': fields[19], + 'usbl_roll': fields[20], + 'source_file': os.path.basename(csv_file), + } + usbl_records.append(rec) + + return usbl_records, frame_id_counter, valid_total, len(positions) + + +def main(): + import argparse + parser = argparse.ArgumentParser(description='Decode Kogger USBL raw CSV files') + parser.add_argument('files', nargs='+', help='Input *_usbl.csv files') + parser.add_argument('-o', '--output', default='combined_usbl.csv', help='Output CSV') + args = parser.parse_args() + + all_records = [] + total_sync = 0 + total_valid = 0 + global_id_counter = collections.Counter() + + for csv_file in args.files: + print("Processing: %s" % csv_file) + records, id_counter, valid, n_sync = parse_usbl_csv(csv_file) + all_records.extend(records) + total_sync += n_sync + total_valid += valid + global_id_counter.update(id_counter) + print(" Sync markers: %d Valid frames: %d USBL records: %d" % (n_sync, valid, len(records))) + + print("\n=== Summary ===") + print("Total sync markers (BB55): %d" % total_sync) + print("Total valid frames: %d" % total_valid) + print("Total USBL_SOLUTION records: %d" % len(all_records)) + print("\nFrame ID histogram:") + for fid, cnt in sorted(global_id_counter.items(), key=lambda x: -x[1]): + name = "USBL_SOLUTION" if fid == 0x65 else ("USBL_CONTROL" if fid == 0x68 else "UNKNOWN") + print(" ID=0x%02x(%3d) %-15s : %d frames" % (fid, fid, name, cnt)) + + if all_records: + dists = [r['Dist'] for r in all_records if not math.isnan(r['Dist'])] + azs = [r['Azimuth'] for r in all_records if not math.isnan(r['Azimuth'])] + snrs = [r['SNR'] for r in all_records if not math.isnan(r['SNR'])] + if dists: + dists_sorted = sorted(dists) + n = len(dists_sorted) + median = dists_sorted[n//2] + print("\nDist (m): min=%.2f median=%.2f max=%.2f" % (min(dists), median, max(dists))) + if azs: + print("Azimuth : min=%.2f max=%.2f" % (min(azs), max(azs))) + if snrs: + print("SNR : min=%.2f max=%.2f" % (min(snrs), max(snrs))) + + # Write output CSV + with open(args.output, 'w', newline='') as f: + writer = csv.writer(f) + writer.writerow(['Timestamp', 'Dist', 'Azimuth', 'Elev', 'SNR', 'FrameID', + 'x_m', 'y_m', 'depth_m', 'dist_unc', 'azimuth_unc', 'elev_unc', + 'usbl_yaw', 'usbl_pitch', 'usbl_roll', 'source_file']) + for r in all_records: + writer.writerow([ + r['Timestamp'], + '' if math.isnan(r['Dist']) else '%.4f' % r['Dist'], + '' if math.isnan(r['Azimuth']) else '%.4f' % r['Azimuth'], + '' if math.isnan(r['Elev']) else '%.4f' % r['Elev'], + '' if math.isnan(r['SNR']) else '%.4f' % r['SNR'], + '0x65', + '' if math.isnan(r['x_m']) else '%.4f' % r['x_m'], + '' if math.isnan(r['y_m']) else '%.4f' % r['y_m'], + '' if math.isnan(r['depth_m']) else '%.4f' % r['depth_m'], + '%.4f' % r['dist_unc'], + '%.4f' % r['azimuth_unc'], + '%.4f' % r['elev_unc'], + '%.4f' % r['usbl_yaw'], + '%.4f' % r['usbl_pitch'], + '%.4f' % r['usbl_roll'], + r['source_file'], + ]) + + print("\nOutput: %s (%d records)" % (args.output, len(all_records))) + + +if __name__ == '__main__': + main() diff --git a/tools/parse_usv_nav.py b/tools/parse_usv_nav.py index 0926d16..0ba4fc7 100644 --- a/tools/parse_usv_nav.py +++ b/tools/parse_usv_nav.py @@ -1,24 +1,39 @@ #!/usr/bin/env python3 -"""Parse USV long-format CSV → track.geojson + points.json""" +"""Parse USV long-format CSV → track.geojson + points.json + manifest.json +v2: multi-session support via --input-dir, retro-compat with --input (single file) +""" import argparse import csv +import glob import json import os import sys from collections import defaultdict +from datetime import datetime, timezone -MAX_SLIDER_POINTS = 5000 +MAX_SLIDER_POINTS = 10000 def parse_args(): - p = argparse.ArgumentParser(description="Parse USV nav CSV") - p.add_argument("--input", required=True, help="CSV navigation log") + p = argparse.ArgumentParser(description="Parse USV nav CSV v2") + g = p.add_mutually_exclusive_group(required=True) + g.add_argument("--input", help="Single CSV navigation log (v1 compat)") + g.add_argument("--input-dir", help="Directory: glob *navigation_log*.csv") p.add_argument("--output", required=True, help="Output directory") return p.parse_args() +def find_csvs(input_dir): + pattern = os.path.join(input_dir, "*navigation_log*.csv") + files = sorted(glob.glob(pattern)) + if not files: + print(f"No navigation_log CSVs found in {input_dir}", file=sys.stderr) + sys.exit(1) + return files + + def load_csv(path): - """Load long-format CSV into {timestamp: {field: value}}""" + """Load long-format CSV → {timestamp: {field: value}}""" rows_by_ts = defaultdict(dict) with open(path, newline="", encoding="utf-8") as f: reader = csv.DictReader(f) @@ -41,13 +56,26 @@ def get_float(d, *keys): return None -def build_points(rows_by_ts): - """Build sorted list of {t, lat, lon, heading} where lat/lon valid.""" - # We need to track last known lat/lon/heading per timestamp cluster. - # Strategy: walk timestamps in order, emit a point each time we see a Lat or Lon update. - # Accumulate state across timestamps. - timestamps = sorted(rows_by_ts.keys()) +def ts_to_ms(ts_str): + """Convert ISO-like timestamp string to epoch ms (UTC).""" + # Try formats: '2026-03-24 09:04:07.123456' or '2026-03-24T09:04:07.123456' + for fmt in ( + "%Y-%m-%dT%H:%M:%S.%f", + "%Y-%m-%dT%H:%M:%S", + "%Y-%m-%d %H:%M:%S.%f", + "%Y-%m-%d %H:%M:%S", + ): + try: + dt = datetime.strptime(ts_str, fmt).replace(tzinfo=timezone.utc) + return int(dt.timestamp() * 1000) + except ValueError: + continue + return None + +def build_points(rows_by_ts, source_name): + """Build sorted list of {t, t_ms, lat, lon, heading, source}.""" + timestamps = sorted(rows_by_ts.keys()) state = {} points = [] @@ -55,7 +83,6 @@ def build_points(rows_by_ts): updates = rows_by_ts[ts] state.update(updates) - # Only emit point if we have both Lat and Lon from this or earlier ts lat = get_float(state, "Lat", "RAW_Lat") lon = get_float(state, "Lon", "RAW_Lon") heading = get_float(state, "Heading", "Yaw") @@ -64,7 +91,6 @@ def build_points(rows_by_ts): continue if lat == 0.0 and lon == 0.0: continue - # GPS_RAW_INT fallback (1e-7 degrees) if abs(lat) < 1 and abs(lon) < 1: raw_lat = get_float(state, "GPS_RAW_INT_lat") raw_lon = get_float(state, "GPS_RAW_INT_lon") @@ -74,87 +100,200 @@ def build_points(rows_by_ts): else: continue - # Only emit if Lat or Lon just updated (reduce duplicate consecutive points) if "Lat" in updates or "Lon" in updates or "RAW_Lat" in updates or "RAW_Lon" in updates: + t_ms = ts_to_ms(ts) points.append({ "t": ts, + "t_ms": t_ms, "lat": round(lat, 8), "lon": round(lon, 8), "heading": round(heading, 2) if heading is not None else None, + "source": source_name, }) return points -def sample_points(points, max_n): - if len(points) <= max_n: +def sample_points_session(points, max_total, n_sessions): + """Sample per session, always keeping first+last point of each session.""" + if not points: return points - step = len(points) / max_n - return [points[int(i * step)] for i in range(max_n)] + quota = max(10, max_total // max(n_sessions, 1)) + if len(points) <= quota: + return points + + step = (len(points) - 2) / max(quota - 2, 1) + sampled = [points[0]] + for i in range(1, quota - 1): + sampled.append(points[min(int(1 + i * step), len(points) - 2)]) + sampled.append(points[-1]) + return sampled -def write_geojson(points, path): - coords = [[p["lon"], p["lat"]] for p in points] - geojson = { - "type": "FeatureCollection", - "features": [{ +def session_bbox(points): + lats = [p["lat"] for p in points] + lons = [p["lon"] for p in points] + return [min(lons), min(lats), max(lons), max(lats)] + + +def write_outputs(all_sessions, output_dir): + """Write track.geojson, points.json, manifest.json.""" + os.makedirs(output_dir, exist_ok=True) + + # Colors for multi-track + COLORS = ["#00b4d8", "#e94560", "#06d6a0", "#ffd166", "#a855f7", "#f97316"] + + # ── track.geojson (MultiLineString per session) ── + features = [] + for i, sess in enumerate(all_sessions): + coords = [[p["lon"], p["lat"]] for p in sess["points"]] + features.append({ "type": "Feature", "geometry": {"type": "LineString", "coordinates": coords}, "properties": { - "start": points[0]["t"] if points else None, - "end": points[-1]["t"] if points else None, - "n_points": len(points), + "source_file": sess["source_file"], + "source_name": sess["source_name"], + "start_iso": sess["t_start"], + "end_iso": sess["t_end"], + "n_points": len(coords), + "color": COLORS[i % len(COLORS)], + "session_index": i, } - }] - } - with open(path, "w") as f: + }) + + geojson = {"type": "FeatureCollection", "features": features} + geo_path = os.path.join(output_dir, "track.geojson") + with open(geo_path, "w") as f: json.dump(geojson, f) - print(f" track.geojson: {len(coords)} coords → {path}") + print(f" track.geojson: {len(features)} sessions → {geo_path}") + + # ── points.json (all sampled, sorted by t_ms) ── + all_points = [] + n_sessions = len(all_sessions) + for sess in all_sessions: + sampled = sample_points_session(sess["points"], MAX_SLIDER_POINTS, n_sessions) + all_points.extend(sampled) + + # Sort by t_ms (sessions may overlap in time) + all_points.sort(key=lambda p: (p["t_ms"] or 0)) + + pts_path = os.path.join(output_dir, "points.json") + with open(pts_path, "w") as f: + json.dump(all_points, f) + print(f" points.json: {len(all_points)} points (sampled) → {pts_path}") + + # ── manifest.json ── + all_lats = [p["lat"] for s in all_sessions for p in s["points"]] + all_lons = [p["lon"] for s in all_sessions for p in s["points"]] + global_bbox = [min(all_lons), min(all_lats), max(all_lons), max(all_lats)] + + all_t_ms = [p["t_ms"] for s in all_sessions for p in s["points"] if p["t_ms"]] + t_min_ms = min(all_t_ms) if all_t_ms else None + t_max_ms = max(all_t_ms) if all_t_ms else None + + sessions_meta = [] + for sess in all_sessions: + sessions_meta.append({ + "file": sess["source_file"], + "source_name": sess["source_name"], + "n_points": sess["n_points_raw"], + "t_start": sess["t_start"], + "t_end": sess["t_end"], + "t_start_ms": sess["t_start_ms"], + "t_end_ms": sess["t_end_ms"], + "bbox": sess["bbox"], + }) + + manifest = { + "generated_at": datetime.now(timezone.utc).isoformat(), + "n_sessions": len(all_sessions), + "sessions": sessions_meta, + "global_bbox": global_bbox, + "t_min": all_sessions[0]["t_start"] if all_sessions else None, + "t_max": all_sessions[-1]["t_end"] if all_sessions else None, + "t_min_ms": t_min_ms, + "t_max_ms": t_max_ms, + "n_points_total_raw": sum(s["n_points_raw"] for s in all_sessions), + "n_points_sampled": len(all_points), + } + + mf_path = os.path.join(output_dir, "manifest.json") + with open(mf_path, "w") as f: + json.dump(manifest, f, indent=2) + print(f" manifest.json → {mf_path}") + + return manifest -def write_points_json(points, path): - with open(path, "w") as f: - json.dump(points, f) - print(f" points.json: {len(points)} points → {path}") +def print_global_stats(manifest, all_sessions): + print(f"\n=== Stats globales ===") + print(f" Sessions: {manifest['n_sessions']}") + print(f" Points bruts: {manifest['n_points_total_raw']}") + print(f" Points sampled: {manifest['n_points_sampled']}") + print(f" t_min: {manifest['t_min']}") + print(f" t_max: {manifest['t_max']}") + bb = manifest["global_bbox"] + print(f" Bbox: lon [{bb[0]:.5f}, {bb[2]:.5f}] lat [{bb[1]:.5f}, {bb[3]:.5f}]") + if manifest["t_min_ms"] and manifest["t_max_ms"]: + dur_s = (manifest["t_max_ms"] - manifest["t_min_ms"]) / 1000 + h, rem = divmod(int(dur_s), 3600) + m, s = divmod(rem, 60) + print(f" Durée totale: {h}h{m:02d}m{s:02d}s") + for i, sess in enumerate(all_sessions): + print(f" Session {i+1}: {sess['source_name']} {sess['n_points_raw']} pts {sess['t_start']} → {sess['t_end']}") -def print_stats(points): +def process_file(path): + source_name = os.path.basename(path) + print(f"\nChargement {source_name} ...") + rows = load_csv(path) + print(f" {len(rows)} timestamps uniques") + points = build_points(rows, source_name) if not points: - print("No valid points found!") - return + print(f" WARNING: aucun point GPS valide dans {source_name}") + return None + + # Filter points without t_ms + points = [p for p in points if p["t_ms"] is not None] lats = [p["lat"] for p in points] lons = [p["lon"] for p in points] - print(f"\n=== Stats ===") - print(f" N points (full): {len(points)}") - print(f" First ts: {points[0]['t']}") - print(f" Last ts: {points[-1]['t']}") - print(f" Bbox lat: {min(lats):.6f} → {max(lats):.6f}") - print(f" Bbox lon: {min(lons):.6f} → {max(lons):.6f}") - headings = [p["heading"] for p in points if p["heading"] is not None] - print(f" Heading data: {'yes' if headings else 'no'} ({len(headings)} values)") + return { + "source_file": path, + "source_name": source_name, + "points": points, + "n_points_raw": len(points), + "t_start": points[0]["t"], + "t_end": points[-1]["t"], + "t_start_ms": points[0]["t_ms"], + "t_end_ms": points[-1]["t_ms"], + "bbox": [min(lons), min(lats), max(lons), max(lats)], + } def main(): args = parse_args() - os.makedirs(args.output, exist_ok=True) - print(f"Loading {args.input} ...") - rows = load_csv(args.input) - print(f" {len(rows)} unique timestamps") + if args.input: + csv_files = [args.input] + else: + csv_files = find_csvs(args.input_dir) - points = build_points(rows) - print_stats(points) + print(f"Fichiers trouvés: {len(csv_files)}") + for f in csv_files: + print(f" {os.path.basename(f)}") - if not points: + all_sessions = [] + for path in csv_files: + sess = process_file(path) + if sess: + all_sessions.append(sess) + + if not all_sessions: + print("Aucune session valide.", file=sys.stderr) sys.exit(1) - write_geojson(points, os.path.join(args.output, "track.geojson")) - - sampled = sample_points(points, MAX_SLIDER_POINTS) - if len(sampled) < len(points): - print(f" Sampled {len(sampled)} points for slider (from {len(points)})") - write_points_json(sampled, os.path.join(args.output, "points.json")) - + manifest = write_outputs(all_sessions, args.output) + print_global_stats(manifest, all_sessions) print("\nDone.") diff --git a/tools/usbl_to_json.py b/tools/usbl_to_json.py new file mode 100644 index 0000000..a3ed7af --- /dev/null +++ b/tools/usbl_to_json.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 +"""usbl_to_json.py - Convert combined_nav_usbl.csv to usbl.json + auv_track.geojson""" +import csv, json, math, argparse, statistics +from datetime import datetime, timezone +from pathlib import Path + +ROOT = Path(__file__).resolve().parent.parent +DEFAULT_INPUT = ROOT / "output" / "combined_nav_usbl.csv" +OUTPUT_DIR = ROOT / "output" + +def parse_ts(s): + for fmt in ("%Y-%m-%d %H:%M:%S.%f", "%Y-%m-%d %H:%M:%S"): + try: + dt = datetime.strptime(s.strip(), fmt).replace(tzinfo=timezone.utc) + return int(dt.timestamp() * 1000) + except ValueError: + pass + return None + +def haversine_m(lat1, lon1, lat2, lon2): + R = 6371000.0 + phi1, phi2 = math.radians(lat1), math.radians(lat2) + dphi = math.radians(lat2 - lat1) + dlam = math.radians(lon2 - lon1) + a = math.sin(dphi/2)**2 + math.cos(phi1)*math.cos(phi2)*math.sin(dlam/2)**2 + return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--input", default=str(DEFAULT_INPUT)) + ap.add_argument("--max-points", type=int, default=10000) + args = ap.parse_args() + + rows = [] + with open(args.input, newline="") as f: + reader = csv.DictReader(f) + for row in reader: + try: + dist = float(row["Dist"]) + auv_lat = float(row["auv_lat"]) + auv_lon = float(row["auv_lon"]) + except (ValueError, KeyError): + continue + if dist <= 0 or auv_lat == 0 or auv_lon == 0: + continue + t_ms = parse_ts(row["Timestamp"]) + if t_ms is None: + continue + rows.append({ + "t": row["Timestamp"].strip(), + "t_ms": t_ms, + "usv_lat": float(row["lat"]), + "usv_lon": float(row["lon"]), + "heading": float(row["Heading"]), + "dist": dist, + "az": float(row["Azimuth"]), + "elev": float(row["Elev"]), + "snr": float(row["SNR"]), + "auv_lat": auv_lat, + "auv_lon": auv_lon, + }) + + rows.sort(key=lambda r: r["t_ms"]) + n_raw = len(rows) + + # Sample if > max-points (preserve begin/end) + if n_raw > args.max_points: + step = n_raw / args.max_points + indices = set() + indices.add(0) + indices.add(n_raw - 1) + for i in range(1, args.max_points - 1): + indices.add(int(i * step)) + rows = [rows[i] for i in sorted(indices)] + + n = len(rows) + dists = [r["dist"] for r in rows] + snrs = [r["snr"] for r in rows] + auv_lats = [r["auv_lat"] for r in rows] + auv_lons = [r["auv_lon"] for r in rows] + + auv_bbox = [min(auv_lons), min(auv_lats), max(auv_lons), max(auv_lats)] + + out = { + "generated_at": datetime.now(timezone.utc).isoformat(), + "n_points": n, + "n_raw": n_raw, + "auv_bbox": auv_bbox, + "stats": { + "dist_min": round(min(dists), 3), + "dist_max": round(max(dists), 3), + "dist_median": round(statistics.median(dists), 3), + "snr_min": round(min(snrs), 4), + "snr_max": round(max(snrs), 4), + }, + "points": rows, + } + + usbl_out = OUTPUT_DIR / "usbl.json" + with open(usbl_out, "w") as f: + json.dump(out, f, separators=(",", ":")) + print(f"usbl.json: {n} points (raw={n_raw}) -> {usbl_out}") + + # AUV track GeoJSON + coords = [[r["auv_lon"], r["auv_lat"]] for r in rows] + geojson = { + "type": "FeatureCollection", + "features": [{ + "type": "Feature", + "geometry": {"type": "LineString", "coordinates": coords}, + "properties": {"name": "AUV track (USBL projection)", "color": "#ff8800"}, + }] + } + track_out = OUTPUT_DIR / "auv_track.geojson" + with open(track_out, "w") as f: + json.dump(geojson, f, separators=(",", ":")) + print(f"auv_track.geojson: {len(coords)} coords -> {track_out}") + + # Sanity check: first point haversine vs USBL dist + r0 = rows[0] + hav = haversine_m(r0["usv_lat"], r0["usv_lon"], r0["auv_lat"], r0["auv_lon"]) + print(f"Sanity [0]: USV=({r0['usv_lat']:.6f},{r0['usv_lon']:.6f}) AUV=({r0['auv_lat']:.6f},{r0['auv_lon']:.6f}) hav={hav:.2f}m USBL_dist={r0['dist']:.2f}m diff={abs(hav-r0['dist']):.3f}m") + +if __name__ == "__main__": + main() diff --git a/vendor/Kogger-Protocol/Kogger SB protocol.odt b/vendor/Kogger-Protocol/Kogger SB protocol.odt new file mode 100644 index 0000000..944b109 Binary files /dev/null and b/vendor/Kogger-Protocol/Kogger SB protocol.odt differ diff --git a/vendor/Kogger-Protocol/Kogger SB protocol.pdf b/vendor/Kogger-Protocol/Kogger SB protocol.pdf new file mode 100644 index 0000000..2eab23c Binary files /dev/null and b/vendor/Kogger-Protocol/Kogger SB protocol.pdf differ diff --git a/vendor/Kogger-Protocol/README.md b/vendor/Kogger-Protocol/README.md new file mode 100644 index 0000000..6db5544 --- /dev/null +++ b/vendor/Kogger-Protocol/README.md @@ -0,0 +1,51 @@ +# Open Serial Binary Protocol (SBP) specification + +## Protocol frame structure + +SYNC1 | SYNC2 | ROUTE | MODE | ID | LENGTH | PAYLOAD | CHECK1 | CHECK2| +|----------|----------|----------|----------|----------|----------|----------|----------|----------| +U1 | U1 | U1 | U1 | U1 | U1 | BYTE[LENGTH] | U1 | U1 | +0xBB | 0x55 | BITFIELD | BITFIELD | 1 … 255 | 0 … 128 | BYTEARRAY | 0 … 0xFF | 0 … 0xFF | + +### ROUTE +Name | Bits | Description +|----------|----------|----------| +DEV_ADDRESS | 0:3 bit | Device address. Default and broadcast address is 0x0. +RESERVED | 2 bit | Reserved + +### MODE +Name | Bits | Description +|----------|----------|----------| +TYPE | 0:1 bit | 0 — Reserved, 1 — CONTENT: DEVICE → HOST, 2 — SETTING: HOST → DEVICE, 3 —GETTING: HOST → DEVICE +RESERVED | 2 bit | Reserved +VERSION | 3:5 bit | Field defines the payload data version +MARK | 6 bit | Once device is switched on, this flag is always in reset state (ZERO). It can be set to active state (ONE) by the host (see the CMD_MARK command) and the slave device keeps the flag in active state in every frame until hardware reset occurs or is reset by the host. Therefore the host monitors the device's actual settings. +RESPONSE | 7 bit | HOST → DEVICE: Set the flag to active state (ONE) in order to get the result of processing the command. The flag doesn't affect the response if one is provided by the TYPE field. DEVICE → HOST: The flag is in reset state (ZERO) by default. Payload goes according to the command specification. If flag is set, the payload contains the result of command processing (see CMD_RESP command). + +## Checksum +The checksum algorithm used is the Fletcher-16. +Example source code for calculating the checksum: +``` +uint8_t CHECK1 = 0; +uint8_t CHECK2 = 0; +void CheckSumUpdate(uint8_t byte) { + CHECK1 += byte; + CHECK2 += CHECK1; +} +``` + +## Number Formats +- Multi-byte values are ordered in Little Endian format +- Floating point values are transmitted in IEEE754 single or double precision +- bit-field in LSB format + +Name | Type | Size (Bytes) | Range +|----------|----------|----------|----------| +S1 | int8_t | 1 | -128 ... 127 +U1 | uint8_t | 1 | 0 … 255 +S2 | int16_t | 2 | -32768 … 32767 +U2 | uint16_t | 2 | 0 … 65535 +S4 | int32_t | 4 | -2'147'483'648 ... 2'147'483'647 +U4 | uint32_t | 4 | 0 … 4'294'967'295 +F4 | float | 4 | -1*2^+127 ... 2^+127 +D8 | double | 8 | -1*2^+1023 ... 2^+1023 diff --git a/viewer/index.html b/viewer/index.html index 390e63c..a08937c 100644 --- a/viewer/index.html +++ b/viewer/index.html @@ -2,266 +2,616 @@ -COSMA — USV Track Viewer +COSMA — NAV Viewer v5 + + - +
- - - - — - Chargement calendrier… + + + + Chargement...
-
+ + - + +
+
+
+
Dist—
+
Az (rel)—
+
Elev—
+
SNR—
+
REL HEADING
+
+
+ +
- USV Track - -
Chargement…
+
+ t +
+ — + + + + +
+
+ — +
+
+ + +
+
Profondeur AUV (m)
+
PWM AUV (canaux)
+
PWM USV (M1-M8)
+
USBL Distance (m)
+ +