Compass/core/nmea_parser.py

"""
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)
}