feat: Compass initial commit

2026-07-03 12:23:41 -04:00
commit 72dcfeb315
26 changed files with 2551 additions and 0 deletions
@@ -0,0 +1,161 @@
+"""
+NMEA 0183 parser — populates NavData from sentences.
+Handles: HDG, HDT, HDM, RMC, VTG, ROT, XDR, MWV, GGA, ZDA
+"""
+from dataclasses import dataclass
+from typing import Optional
+
+
+def _checksum_ok(sentence: str) -> bool:
+    if '*' not in sentence:
+        return True
+    try:
+        data, cs = sentence.rsplit('*', 1)
+        val = 0
+        for c in data.lstrip('$!'):
+            val ^= ord(c)
+        return val == int(cs.strip()[:2], 16)
+    except Exception:
+        return False
+
+
+def _f(s: str) -> Optional[float]:
+    try:
+        return float(s) if s.strip() else None
+    except ValueError:
+        return None
+
+
+@dataclass
+class NavData:
+    # ── Heading ──────────────────────────────────────────────────────────────
+    hdg_mag:   Optional[float] = None   # Magnetic heading °
+    hdg_true:  Optional[float] = None   # True heading °
+    variation: Optional[float] = None   # Mag variation (+ = East)
+    deviation: Optional[float] = None
+
+    # ── Motion ───────────────────────────────────────────────────────────────
+    rot:       Optional[float] = None   # Rate of turn °/min (+ = stbd)
+
+    # ── Attitude (BNO085) ─────────────────────────────────────────────────────
+    pitch:     Optional[float] = None   # ° bow-up positive
+    roll:      Optional[float] = None   # ° stbd positive
+    heel:      Optional[float] = None   # alias for roll on some talkers
+
+    # ── Acceleration (BNO085 via XDR) ────────────────────────────────────────
+    accel_x:   Optional[float] = None   # m/s² — surge (fore-aft)
+    accel_y:   Optional[float] = None   # m/s² — sway (port-stbd)
+    accel_z:   Optional[float] = None   # m/s² — heave (up-down)
+
+    # ── Angular velocity / gyro (BNO085 via XDR) ─────────────────────────────
+    gyro_x:    Optional[float] = None   # °/s — roll rate
+    gyro_y:    Optional[float] = None   # °/s — pitch rate
+    gyro_z:    Optional[float] = None   # °/s — yaw rate
+
+    # ── Quaternion (BNO085 via XDR or proprietary) ────────────────────────────
+    quat_w:    Optional[float] = None
+    quat_x:    Optional[float] = None
+    quat_y:    Optional[float] = None
+    quat_z:    Optional[float] = None
+
+    @property
+    def hdg_true_calc(self) -> Optional[float]:
+        if self.hdg_mag is not None and self.variation is not None:
+            return (self.hdg_mag + self.variation) % 360
+        return self.hdg_true
+
+
+def parse(sentence: str, data: NavData) -> bool:
+    sentence = sentence.strip()
+    if not (sentence.startswith('$') or sentence.startswith('!')):
+        return False
+    if not _checksum_ok(sentence):
+        return False
+
+    body = sentence[1:sentence.rfind('*')] if '*' in sentence else sentence[1:]
+    p = body.split(',')
+    if not p:
+        return False
+
+    stype = p[0][-3:].upper()
+    try:
+        return _PARSERS.get(stype, lambda *_: False)(p, data)
+    except Exception:
+        return False
+
+
+# ── Individual sentence parsers ────────────────────────────────────────────
+
+def _hdg(p, d):
+    # $HCHDG,x.x,x.x,a,x.x,a
+    d.hdg_mag = _f(p[1])
+    dev = _f(p[2])
+    if dev is not None:
+        d.deviation = dev if (len(p) > 3 and p[3].upper() != 'W') else -dev
+    var = _f(p[4]) if len(p) > 4 else None
+    if var is not None:
+        d.variation = var if (len(p) > 5 and p[5].upper() != 'W') else -var
+    return True
+
+def _hdt(p, d):
+    if len(p) > 2 and p[2].upper() == 'T':
+        d.hdg_true = _f(p[1])
+    return True
+
+def _hdm(p, d):
+    if len(p) > 2 and p[2].upper() == 'M':
+        d.hdg_mag = _f(p[1])
+    return True
+
+def _rmc(p, d):
+    # Only used to extract magnetic variation as fallback
+    if len(p) > 11 and p[10]:
+        var = _f(p[10])
+        if var is not None:
+            d.variation = var if p[11].upper() != 'W' else -var
+    return True
+
+def _rot(p, d):
+    if len(p) > 2 and p[2].upper() == 'A':
+        d.rot = _f(p[1])
+    return True
+
+def _xdr(p, d):
+    """
+    XDR carries all BNO085 outputs.
+    Expected names (configurable in ESP32 firmware):
+      PITCH, ROLL, HEEL
+      ACCELX, ACCELY, ACCELZ   (or SURGE, SWAY, HEAVE)
+      GYROX,  GYROY,  GYROZ
+      QUATW,  QUATX,  QUATY,  QUATZ
+    """
+    i = 1
+    while i + 3 <= len(p) - 1:
+        name  = p[i + 3].upper()
+        value = _f(p[i + 1])
+        if value is not None:
+            if   'PITCH'  in name:              d.pitch   = value
+            elif 'ROLL'   in name:              d.roll    = value
+            elif 'HEEL'   in name:              d.heel    = value
+            elif name in ('ACCELX', 'SURGE'):   d.accel_x = value
+            elif name in ('ACCELY', 'SWAY'):    d.accel_y = value
+            elif name in ('ACCELZ', 'HEAVE'):   d.accel_z = value
+            elif name in ('GYROX',  'ROLLR'):   d.gyro_x  = value
+            elif name in ('GYROY',  'PITCHR'):  d.gyro_y  = value
+            elif name in ('GYROZ',  'YAWR'):    d.gyro_z  = value
+            elif 'QUATW'  in name:              d.quat_w  = value
+            elif 'QUATX'  in name:              d.quat_x  = value
+            elif 'QUATY'  in name:              d.quat_y  = value
+            elif 'QUATZ'  in name:              d.quat_z  = value
+        i += 4
+    return True
+
+
+_PARSERS = {
+    'HDG': _hdg,   # Heading + variation
+    'HDT': _hdt,   # True heading
+    'HDM': _hdm,   # Magnetic heading
+    'ROT': _rot,   # Rate of turn
+    'XDR': _xdr,   # Pitch / Roll transducer
+    'RMC': _rmc,   # Variation fallback (only var field used)
+}