From d37fc63fad64e43f21a525a2b83e5ee12f6d80ad Mon Sep 17 00:00:00 2001 From: Jacob Neulight Date: Sat, 18 Jul 2026 19:53:53 -0600 Subject: [PATCH 1/2] tools: Ford LMC safety-check replay with reset-bypass latch modeled Frame-exact port of ford.h's Lane_Assist_Data1/LateralMotionControl TX-hook semantics (value limits, steer_angle_cmd_checks, shadow-curvature check, ROC checks, and -- the piece an earlier analysis omitted -- the reset-bypass latch) replayed against real rlogs. Ground truth is independent of the sim: a sendcan frame with no TX loopback echo in the can stream was actually blocked, and pandaStates.safetyTxBlocked counter deltas cross-check the totals, so mis-attribution of block causes can't recur. Includes a --truthful-shadow counterfactual that latches the shadow from measured curvature on the frames the truthful-shadow control fix republishes, at real LKA cadence, to validate that fix against recorded routes. --- tools/ford_lmc_safety_replay.py | 488 ++++++++++++++++++++++++++++++++ 1 file changed, 488 insertions(+) create mode 100644 tools/ford_lmc_safety_replay.py diff --git a/tools/ford_lmc_safety_replay.py b/tools/ford_lmc_safety_replay.py new file mode 100644 index 0000000000..8621dbd9a3 --- /dev/null +++ b/tools/ford_lmc_safety_replay.py @@ -0,0 +1,488 @@ +#!/usr/bin/env python3 +"""Frame-exact replay of ford.h's steering TX-hook against real rlogs. + +Attributes real panda `safetyTxBlocked` increments to specific safety checks, with the +reset-bypass latch modeled. (An earlier shadow-check-only analysis omitted the latch and +mis-attributed re-engage-edge frames as blocked; this tool exists so firmware semantics +can't silently drift from the analysis again.) + +Ported line-for-line from opendbc/safety/modes/ford.h (CAN path). Models the +configuration the analyzed road-test routes were recorded with: angle_meas sourced from +SteeringPinion_Data (Explorer geometry) and the 0.003 error band of the fork's opt-in +steering-angle measurement. Stock yaw-sourced firmware differs only in the angle_meas +source and a 0.002 band -- swap rx_steering_pinion and MAX_ANGLE_ERROR to replay stock +routes. + rx state: vehicle_speed (BrakeSysFeatures, QF==3), angle_meas (SteeringPinion_Data, + QF==3, Explorer geometry) + LKA hook (0x3CA): action!=0 block; latches angle_mode_engaged + shadow_curvature + LMC hook (0x3D3): value limits, steer_angle_cmd_checks (curvature mode) + explicit + controls gate at curvature==0, shadow-curvature check (angle mode), + path_angle/path_offset/curvature_rate ROC checks, reset-bypass latch + +Ground truth is independent of the sim: a `sendcan` frame with no matching TX loopback +echo in `can` (src >= 128) was actually blocked by panda; pandaStates.safetyTxBlocked +counter deltas cross-check the totals. The sim then explains each real block (which +check fired) and a no-latch counterfactual shows what the latch masked. + +Usage: + FORD_REPLAY_DONGLE_ID= tools/ford_lmc_safety_replay.py [ ...] + e.g. tools/ford_lmc_safety_replay.py 00000002--71f65bbf45 +Optional: --json to dump per-frame records for further analysis. +""" +import argparse +import json +import math +import os +from collections import defaultdict, deque + +from openpilot.tools.lib.logreader import LogReader + +DONGLE = os.environ.get('FORD_REPLAY_DONGLE_ID', '') + +# ---- ford.h constants (CAN path, FORD_LIMITS / FORD_*_LIMITS) ---- +VEHICLE_SPEED_FACTOR = 1000.0 +MAX_SAMPLE_VALS = 6 + +FORD_INACTIVE_CURVATURE = 1000 +FORD_INACTIVE_CURVATURE_RATE = 4096 +FORD_INACTIVE_PATH_OFFSET = 512 +FORD_INACTIVE_PATH_ANGLE = 1000 + +STEERING = dict( # FORD_LIMITS values, with the 150-unit (0.003) pinion band; stock band is 100 + max_angle=1000, deg_to_can=50000, max_angle_error=150, + rate_up=([5., 16., 25.], [0.0025, 0.0014, 0.00018]), + rate_down=([5., 16., 25.], [0.0025, 0.0014, 0.00018]), + angle_error_min_speed=10.0, +) +PATH_ANGLE = dict( # FORD_PATH_ANGLE_LIMITS + deg_to_can=2000, rate_up=([10., 15., 25.], [0.0561, 0.04335, 0.00918]), +) +PATH_OFFSET = dict( # FORD_PATH_OFFSET_LIMITS + deg_to_can=100, rate_up=([5., 15., 25.], [0.05, 0.025, 0.01]), +) +CURV_RATE = dict( # FORD_CURVATURE_RATE_LIMITS_CAN + deg_to_can=4000000, rate_up=([5., 15., 25.], [0.05, 0.025, 0.01]), +) + +FORD_CURVATURE_MIN, FORD_CURVATURE_MAX = -0.02, 0.02 +FORD_CURVATURE_RATE_MIN, FORD_CURVATURE_RATE_MAX = -0.001024, 0.00102375 +FORD_PATH_OFFSET_MIN, FORD_PATH_OFFSET_MAX = -1.0, 1.0 +FORD_PATH_ANGLE_MIN, FORD_PATH_ANGLE_MAX = -0.25, 0.25 +FORD_DBC_PATH_ANGLE_MIN, FORD_DBC_PATH_ANGLE_MAX = -0.5, 0.5235 + +RESET_BYPASS_LATCH_DURATION = 60 + +# Explorer pinion->curvature geometry (slip factor / steer ratio / wheelbase) +SLIP, SR, WB = -0.00055447339, 16.8, 3.025 + +ADDR_LKA, ADDR_LMC = 0x3CA, 0x3D3 +ADDR_PINION, ADDR_BRAKE_SYS = 0x7E, 0x415 +ECHO_TIMEOUT_S = 0.5 + +CHECK_KEYS = ['v_curv_val', 'v_curv_rate_val', 'v_po_val', 'v_pa_val', 'v_curv_check', + 'v_controls_gate', 'v_shadow', 'v_pa_roc', 'v_po_roc', 'v_curv_rate_roc'] + + +def interp_hold(xy, x): + xs, ys = xy + if x <= xs[0]: + return ys[0] + for i in range(len(xs) - 1): + if x < xs[i + 1]: + dx = max(xs[i + 1] - xs[i], 0.0001) + return ys[i] + (ys[i + 1] - ys[i]) * (x - xs[i]) / dx + return ys[-1] + + +def limit_check(val, max_val, min_val): + return (val > max_val) or (val < min_val) + + +class Sample: + def __init__(self): + self.values = deque([0] * MAX_SAMPLE_VALS, maxlen=MAX_SAMPLE_VALS) + + def update(self, v): + self.values.appendleft(int(v)) + + @property + def min(self): + return min(self.values) + + @property + def max(self): + return max(self.values) + + @property + def latest(self): + return self.values[0] + + +class FordLmcSafetySim: + """Firmware state machine for the CAN LMC/LKA tx hooks + relevant rx state.""" + + def __init__(self): + self.vehicle_speed = Sample() + self.angle_meas = Sample() + self.desired_angle_last = 0 + self.desired_path_angle_last = 0 + self.desired_path_offset_last = 0 + self.desired_curvature_rate_last = 0 + self.reset_bypass_latch_counter = 0 + self.angle_mode_engaged = False + self.shadow_curvature_raw = 0 + # externally-fed panda state (from pandaStates log) + self.controls_allowed = False + self.controls_allowed_lateral = False + + # ---- rx side ---- + def rx_brake_sys_features(self, d): + if (d[2] >> 6) == 0x3: # VehVActlBrk_D_Qf + speed_ms = ((d[0] << 8) | d[1]) * 0.01 / 3.6 + self.vehicle_speed.update(round(speed_ms * VEHICLE_SPEED_FACTOR)) + + def rx_steering_pinion(self, d): + if ((d[5] >> 2) & 0x3) != 0x3: # StePinCompAnEst_D_Qf + return + angle_raw = ((d[2] & 0x7F) << 8) | d[3] + pinion_angle_rad = math.radians((angle_raw * 0.1) - 1600.0) + speed = max(self.vehicle_speed.latest / VEHICLE_SPEED_FACTOR, 0.1) + curvature_factor = 1. / (1. - (SLIP * speed * speed)) / WB + current_curvature = pinion_angle_rad * curvature_factor / SR + self.angle_meas.update(round(current_curvature * STEERING['deg_to_can'])) + + # ---- tx side ---- + def tx_lka(self, d, pressed=False, truthful_shadow=False): + """Returns True if blocked. Latches angle-mode statics regardless (as firmware does). + + truthful_shadow: counterfactual for the truthful-shadow control fix -- on frames the + fix would publish the shadow from measured curvature (driver pressing, or the old + code's zeroed override/inactive frames), latch the measured value instead, at real + LKA cadence so latch-age timing skew is modeled faithfully. + """ + action = d[0] >> 5 + self.angle_mode_engaged = (d[4] & 0x1) != 0 + raw = (d[5] << 8) | d[6] + raw = raw - 0x10000 if raw >= 0x8000 else raw # int16 + if truthful_shadow and (pressed or raw == 0): + raw = int(self.angle_meas.latest * 20) # CAN units (2e-5) -> shadow raw units (1e-6) + self.shadow_curvature_raw = raw + return action != 0 + + def _steer_angle_cmd_checks(self, desired_angle, en, lim): + """lateral.h steer_angle_cmd_checks, angle_is_curvature=false, inactive_angle_is_zero=true.""" + violation = False + if (self.controls_allowed or self.controls_allowed_lateral) and en: + fudged_speed = (self.vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1. + delta_up = int(interp_hold(lim['rate_up'], fudged_speed) * lim['deg_to_can'] + 1.) + delta_down = int(interp_hold(lim['rate_down'], fudged_speed) * lim['deg_to_can'] + 1.) + last = self.desired_angle_last + highest = last + (delta_up if last > 0 else delta_down) + lowest = last - (delta_down if last >= 0 else delta_up) + if (self.vehicle_speed.latest / VEHICLE_SPEED_FACTOR) > lim['angle_error_min_speed']: + fudged_speed_error = (self.vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1. + delta_up_rlx = int(interp_hold(lim['rate_up'], fudged_speed_error) * lim['deg_to_can'] - 1.) + delta_down_rlx = int(interp_hold(lim['rate_down'], fudged_speed_error) * lim['deg_to_can'] - 1.) + lowest_err = self.angle_meas.min - lim['max_angle_error'] - 1 + highest_err = self.angle_meas.max + lim['max_angle_error'] + 1 + if last > highest_err: + delta = delta_down_rlx if last >= 0 else delta_up_rlx + highest = max(last - delta, highest_err) + elif last < lowest_err: + delta = delta_down_rlx if last <= 0 else delta_up_rlx + lowest = min(last + delta, lowest_err) + else: + highest = min(highest, highest_err) + lowest = max(lowest, lowest_err) + lowest = min(max(lowest, -lim['max_angle']), lim['max_angle']) + highest = min(max(highest, -lim['max_angle']), lim['max_angle']) + violation |= limit_check(desired_angle, highest, lowest) + self.desired_angle_last = desired_angle + if not en: + violation |= desired_angle != 0 + # No angle control allowed when controls are not allowed (lateral.h:267-269) + if not (self.controls_allowed or self.controls_allowed_lateral): + violation |= en + # reset on violation or controls-not-allowed (lateral.h:271-277, inactive_angle_is_zero); + # firmware does this BEFORE the reset-bypass latch can clear the violation + if violation or not (self.controls_allowed or self.controls_allowed_lateral): + self.desired_angle_last = 0 + return violation + + def _roc_check(self, desired, last_attr, en, lim): + violation = False + if en: + speed = (self.vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1. + delta = int(interp_hold(lim['rate_up'], speed) * lim['deg_to_can'] + 1.) + last = getattr(self, last_attr) + violation |= limit_check(desired, last + delta, last - delta) + setattr(self, last_attr, desired) + if not en: + violation |= desired != 0 + return violation + + def _shadow_check(self, shadow_can, en, lim): + if en and (self.vehicle_speed.latest / VEHICLE_SPEED_FACTOR) > lim['angle_error_min_speed']: + return limit_check(shadow_can, self.angle_meas.max + lim['max_angle_error'] + 1, + self.angle_meas.min - lim['max_angle_error'] - 1) + return False + + def tx_lmc(self, d): + """Full LMC tx-hook. Returns a dict of per-check verdicts + final pre/post-latch.""" + en = ((d[4] >> 2) & 0x7) != 0 + raw_curvature = (d[0] << 3) | (d[1] >> 5) + raw_curvature_rate = ((d[1] & 0x1F) << 8) | d[2] + raw_path_angle = (d[3] << 3) | (d[4] >> 5) + raw_path_offset = (d[5] << 2) | (d[6] >> 6) + + curv = raw_curvature - FORD_INACTIVE_CURVATURE + curv_rate = raw_curvature_rate - FORD_INACTIVE_CURVATURE_RATE + pa = raw_path_angle - FORD_INACTIVE_PATH_ANGLE + po = raw_path_offset - FORD_INACTIVE_PATH_OFFSET + + r = {'en': en, 'curv': curv, 'pa': pa, 'po': po, 'curv_rate': curv_rate, + 'engaged': self.angle_mode_engaged, 'shadow_raw': self.shadow_curvature_raw, + 'shadow_can': None, 'meas_min': self.angle_meas.min, 'meas_max': self.angle_meas.max, + 'latch_pre': self.reset_bypass_latch_counter} + + # value limits + r['v_curv_val'] = limit_check(curv, int(FORD_CURVATURE_MAX * STEERING['deg_to_can']), + int(FORD_CURVATURE_MIN * STEERING['deg_to_can'])) + r['v_curv_rate_val'] = limit_check(curv_rate, int(FORD_CURVATURE_RATE_MAX * CURV_RATE['deg_to_can']), + int(FORD_CURVATURE_RATE_MIN * CURV_RATE['deg_to_can'])) + r['v_po_val'] = limit_check(po, int(FORD_PATH_OFFSET_MAX * PATH_OFFSET['deg_to_can']), + int(FORD_PATH_OFFSET_MIN * PATH_OFFSET['deg_to_can'])) + pa_min = FORD_DBC_PATH_ANGLE_MIN if self.angle_mode_engaged else FORD_PATH_ANGLE_MIN + pa_max = FORD_DBC_PATH_ANGLE_MAX if self.angle_mode_engaged else FORD_PATH_ANGLE_MAX + r['v_pa_val'] = limit_check(pa, int(pa_max * PATH_ANGLE['deg_to_can']), + int(pa_min * PATH_ANGLE['deg_to_can'])) + + # curvature checks: always call (keeps desired_angle_last in sync), apply if curv != 0 + curv_violation = self._steer_angle_cmd_checks(curv, en, STEERING) + if curv != 0: + r['v_curv_check'] = curv_violation + r['v_controls_gate'] = False + else: + r['v_curv_check'] = False + r['v_controls_gate'] = en and not (self.controls_allowed or self.controls_allowed_lateral) + + # angle mode's shadow-curvature deviation check + r['v_shadow'] = False + if curv == 0 and self.angle_mode_engaged: + shadow_can = int(float(self.shadow_curvature_raw) * 0.05) + r['shadow_can'] = shadow_can + r['v_shadow'] = self._shadow_check(shadow_can, en, STEERING) + + # ROC checks + r['v_pa_roc'] = self._roc_check(pa, 'desired_path_angle_last', en, PATH_ANGLE) + r['v_po_roc'] = self._roc_check(po, 'desired_path_offset_last', en, PATH_OFFSET) + r['v_curv_rate_roc'] = self._roc_check(curv_rate, 'desired_curvature_rate_last', en, CURV_RATE) + + violation = any(r[k] for k in CHECK_KEYS) + r['pre_latch_violation'] = violation + + # reset-bypass latch + if curv == 0 and pa == 0: + self.reset_bypass_latch_counter = RESET_BYPASS_LATCH_DURATION + violation = False + elif self.reset_bypass_latch_counter > 0: + self.reset_bypass_latch_counter -= 1 + violation = False + r['blocked'] = violation + return r + + +class EchoMatcher: + """Ground truth: sent frames that never echo back (src >= 128) were blocked by panda.""" + + def __init__(self): + self.pending = defaultdict(deque) # addr -> deque of (t, dat, seq) + self.recent_echoes = defaultdict(deque) # addr -> deque of (t, dat), reorder tolerance + self.blocked = [] # (t, addr, dat, seq) + self.sent = defaultdict(int) + self.echoed = defaultdict(int) + + def on_send(self, t, addr, dat, seq): + self.sent[addr] += 1 + dat = bytes(dat) + # tolerate log reordering: echo may have been logged just before the sendcan event + for i, (te, de) in enumerate(self.recent_echoes[addr]): + if de == dat and (t - te) < 0.2: + del self.recent_echoes[addr][i] + return + self.pending[addr].append((t, dat, seq)) + + def on_echo(self, t, addr, dat): + self.echoed[addr] += 1 + dat = bytes(dat) + q = self.pending[addr] + for i, (_ts, d, _seq) in enumerate(q): + if d == dat: + for _ in range(i): # frames sent before this one and never echoed -> blocked + tb, db, sb = q.popleft() + self.blocked.append((tb, addr, db, sb)) + q.popleft() + return + re = self.recent_echoes[addr] + re.append((t, dat)) + while len(re) > 8: + re.popleft() + + def expire(self, now): + for addr, q in self.pending.items(): + while q and (now - q[0][0]) > ECHO_TIMEOUT_S: + tb, db, sb = q.popleft() + self.blocked.append((tb, addr, db, sb)) + + def finish(self): + for addr, q in self.pending.items(): + while q: + tb, db, sb = q.popleft() + self.blocked.append((tb, addr, db, sb)) + + +def iter_route(route): + """Yield log events segment by segment, skipping segments that were never uploaded.""" + misses = 0 + seg = 0 + while misses < 3 and seg < 100: + try: + lr = LogReader(f'{DONGLE}|{route}/{seg}') + yield from lr + misses = 0 + except Exception as e: + print(f' (seg {seg} unavailable: {type(e).__name__})') + misses += 1 + seg += 1 + + +def run_route(route, json_path=None, truthful_shadow=False): + assert DONGLE, 'set FORD_REPLAY_DONGLE_ID' + sim = FordLmcSafetySim() + echo = EchoMatcher() + ctx = {'v_ego': 0.0, 'pressed': False, 'mads': False, 'safety_model': '', + 'controls_allowed': False, 'controls_allowed_lat': False} + t0 = None + seq = 0 + lmc_records = [] # (t, seq, record, ctx snapshot) + lka_blocks = [] + tx_blocked_counter = [] # (t, value) + latch_empty_frames = 0 + + for m in iter_route(route): + w = m.which() + t = m.logMonoTime * 1e-9 + if t0 is None: + t0 = t + + if w == 'can': + for c in m.can: + if c.src == 0: + if c.address == ADDR_BRAKE_SYS: + sim.rx_brake_sys_features(c.dat) + elif c.address == ADDR_PINION: + sim.rx_steering_pinion(c.dat) + elif c.src >= 128: + echo.on_echo(t, c.address, c.dat) + echo.expire(t) + elif w == 'sendcan': + for c in m.sendcan: + seq += 1 + echo.on_send(t, c.address, c.dat, seq) + if c.address == ADDR_LKA: + if sim.tx_lka(c.dat, pressed=ctx['pressed'], truthful_shadow=truthful_shadow): + lka_blocks.append((t, seq)) + elif c.address == ADDR_LMC: + if sim.reset_bypass_latch_counter == 0: + latch_empty_frames += 1 + rec = sim.tx_lmc(c.dat) + lmc_records.append((t, seq, rec, dict(ctx))) + elif w == 'carState': + ctx['v_ego'] = m.carState.vEgo + ctx['pressed'] = m.carState.steeringPressed + elif w == 'selfdriveStateSP': + ctx['mads'] = m.selfdriveStateSP.mads.active + elif w == 'pandaStates': + if len(m.pandaStates) > 0: + ps = m.pandaStates[0] + ctx['safety_model'] = str(ps.safetyModel) + ctx['controls_allowed'] = bool(ps.controlsAllowed) + ctx['controls_allowed_lat'] = bool(ps.controlsAllowedLateral) + sim.controls_allowed = ctx['controls_allowed'] + sim.controls_allowed_lateral = ctx['controls_allowed_lat'] + if not tx_blocked_counter or tx_blocked_counter[-1][1] != ps.safetyTxBlocked: + tx_blocked_counter.append((t, int(ps.safetyTxBlocked))) + echo.finish() + + # ---- report ---- + mode = ' [truthful-shadow counterfactual]' if truthful_shadow else '' + print(f'\n===== {route} ====={mode} (t0 mono = {t0:.1f}s)') + def rt(t): + return t - t0 + n = len(lmc_records) + pre = [x for x in lmc_records if x[2]['pre_latch_violation']] + post = [x for x in lmc_records if x[2]['blocked']] + print(f'LMC frames: {n}; latch empty at frame: {latch_empty_frames} ({100.0 * latch_empty_frames / max(n, 1):.1f}%)') + print(f'sim violations pre-latch (no-latch counterfactual): {len(pre)}; post-latch (predicted real blocks): {len(post)}') + for name, group in [('pre-latch', pre), ('post-latch', post)]: + if group: + counts = {k: sum(1 for _, _, r, _ in group if r[k]) for k in CHECK_KEYS} + print(f' {name} by check: ' + ', '.join(f'{k}={v}' for k, v in counts.items() if v)) + print(f'LKA action blocks (sim): {len(lka_blocks)}') + + print('echo ground truth per addr (sent / echoed / no-echo):') + blocked_by_addr = defaultdict(list) + for tb, addr, db, sb in echo.blocked: + blocked_by_addr[addr].append((tb, db, sb)) + for addr in sorted(echo.sent): + print(f' 0x{addr:X}: sent={echo.sent[addr]} echoed={echo.echoed[addr]} no-echo={len(blocked_by_addr.get(addr, []))}') + + if tx_blocked_counter: + print(f'safetyTxBlocked counter: start={tx_blocked_counter[0][1]}, end={tx_blocked_counter[-1][1]}') + for i in range(1, len(tx_blocked_counter)): + tprev, vprev = tx_blocked_counter[i - 1] + tcur, vcur = tx_blocked_counter[i] + print(f' t_route={rt(tcur):8.1f}s (mono {tcur:.1f}) counter {vprev} -> {vcur}') + + # join: actually-blocked LMC frames vs sim verdicts + blocked_seqs = {sb for _, addr, _, sb in echo.blocked if addr == ADDR_LMC} + print(f'actually-blocked LMC frames (no echo): {len(blocked_seqs)}') + for t, s, r, c in lmc_records: + if s in blocked_seqs or r['blocked'] or r['pre_latch_violation']: + fired = [k for k in CHECK_KEYS if r[k]] or ['NONE(unexplained)'] + tag = ('REAL+SIM' if (s in blocked_seqs and r['blocked']) else + 'REAL only' if s in blocked_seqs else + 'SIM block' if r['blocked'] else 'SIM pre-latch only') + line = (f' [{tag}] t_route={rt(t):8.1f}s en={int(r["en"])} curv={r["curv"]} pa={r["pa"]}' + + f' shadow={r["shadow_can"]} meas=[{r["meas_min"]},{r["meas_max"]}] latch={r["latch_pre"]}' + + f' checks={fired} v={c["v_ego"]:.1f} pressed={int(c["pressed"])} mads={int(c["mads"])}' + + f' ctl={int(c["controls_allowed"])}/{int(c["controls_allowed_lat"])} sm={c["safety_model"]}') + print(line) + # non-LMC real blocks, grouped + for addr, items in sorted(blocked_by_addr.items()): + if addr == ADDR_LMC: + continue + times = ', '.join(f'{rt(tb):.1f}' for tb, _, _ in items[:20]) + print(f' non-LMC no-echo 0x{addr:X}: n={len(items)} t_route=[{times}{", ..." if len(items) > 20 else ""}]') + + if json_path: + with open(json_path, 'w') as f: + json.dump({'route': route, 't0': t0, + 'lmc': [{'t': t, 'seq': s, **r, 'ctx': c} for t, s, r, c in lmc_records], + 'blocked': [[tb, addr, sb] for tb, addr, _, sb in echo.blocked], + 'tx_blocked_counter': tx_blocked_counter}, f) + print(f'wrote {json_path}') + return lmc_records, echo, tx_blocked_counter, t0 + + +if __name__ == '__main__': + ap = argparse.ArgumentParser() + ap.add_argument('routes', nargs='+') + ap.add_argument('--json', help='dump per-frame records (one file per route, suffixed)') + ap.add_argument('--truthful-shadow', action='store_true', + help=('counterfactual: latch the shadow from measured curvature on frames the ' + + 'truthful-shadow fix would republish (pressed / previously-zeroed)')) + args = ap.parse_args() + for route in args.routes: + jp = f'{args.json}.{route}.json' if args.json else None + run_route(route, jp, truthful_shadow=args.truthful_shadow) From b6c45bc4ec7dc9b4523ee46781b9ce5e391a06a2 Mon Sep 17 00:00:00 2001 From: Jacob Neulight Date: Sat, 18 Jul 2026 19:53:46 -0600 Subject: [PATCH 2/2] Ford: publish angle-mode shadow curvature from measured curvature during overrides The shadow curvature (bp_kappa_cmd, carried in Lane_Assist_Data1 bytes 5-6 and judged against angle_meas by ford.h's angle-mode deviation check) was zeroed during human-turn overrides, stall blips, and inactive frames, and kept publishing the clipped planner kappa while the driver pressed. Two problems: 1. Driver pressing mid-curve with the mode still enabled: the clipped planner kappa cannot follow the wheel, so the shadow exits the deviation band. Frame-exact replay of ~3h of road-test routes (tools/ford_lmc_safety_replay.py) attributed the ONLY in-drive lateral safety block to exactly this scenario (sustained curve, latch drained, shadow -630 vs measured [-795,-782] CAN units). 2. Zeroed shadow during mode-0/inactive frames: the panda latches the shadow from LKA (33 Hz) and enable from LMC (20 Hz), so the first re-engaged frame can compare a stale zero against real measured curvature. Today this race is masked by ford.h's reset-bypass latch (replay: 14 such frames across three routes, every one bypassed, zero actually blocked) -- but the published value was still dishonest, and any future tightening of that latch would surface the race. Fix: route every measurement read through LateralCurvExt.get_current_curvature() and publish the shadow from it whenever the planner kappa cannot honestly describe the car's steering -- while pressed, during human-turn/blip mode-0 pulses, and while inactive. The latched shadow then tracks reality continuously and the check compares measured-vs-measured at every re-engage edge. Replay validation (same tool, truthful-shadow counterfactual): shadow-check violations 2/2/14 -> 0/0/0 across the three routes; the one real block is eliminated; no new violations introduced (the value is inside the band by construction at latch time). --- .../sunnypilot/car/ford/lateral_angle_ext.py | 30 +++- .../sunnypilot/car/ford/lateral_curv_ext.py | 13 +- .../sunnypilot/car/ford/tests/__init__.py | 0 .../car/ford/tests/test_lateral_angle_ext.py | 155 ++++++++++++++++++ 4 files changed, 190 insertions(+), 8 deletions(-) create mode 100644 opendbc_repo/opendbc/sunnypilot/car/ford/tests/__init__.py create mode 100644 opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py index 14975b5cd3..42aa95ed59 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -228,7 +228,15 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False self.sim_curvature_last = 0.0 - self.bp_kappa_cmd = 0.0 + # Publish the shadow curvature from the measured curvature while inactive. LKA keeps + # carrying angle_mode_engaged whenever angle mode is configured (independent of + # latActive), and ford.h latches the shadow from every LKA frame -- so the latched + # value must track reality here, not sit at a stale zero. Otherwise the first enabled + # LMC frame after (re-)engage races LKA's 33Hz latch against LMC's 20Hz enable bit and + # ford.h's deviation check compares a zero shadow against real measured curvature. + # (ford.h skips the check while steer_control_enabled is 0, so the value is free to + # follow the measurement during the inactive period itself.) + self.bp_kappa_cmd = self.get_current_curvature(CS) self.human_turn_detector.reset() self.angle_human_turn_active = False self.stall_blip_hold_s = 0.0 @@ -265,9 +273,10 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False self.sim_curvature_last = 0.0 - # Zero the shadow curvature on the wire during the override (mirrors the inactive path); - # ford.h skips the deviation check while steer_control_enabled is 0 either way. - self.bp_kappa_cmd = 0.0 + # Truthful shadow during the override (mirrors the inactive path -- see the comment + # there): the driver is steering, so the honest command is the car's actual curvature, + # and the panda-latched shadow stays current for the re-engage frame. + self.bp_kappa_cmd = self.get_current_curvature(CS) # Keep exit detection current so resume doesn't compare against a stale pre-turn value. self._desired_curvature_last = float(actuators.curvature) # A human turn ends any stall episode -- its own mode 0 does the PSCM reset job. That also @@ -317,7 +326,8 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False self.sim_curvature_last = 0.0 - self.bp_kappa_cmd = 0.0 + # Truthful shadow during the blip (see the inactive-path comment). + self.bp_kappa_cmd = self.get_current_curvature(CS) self._desired_curvature_last = float(actuators.curvature) self.precision_type = 1 if self.stall_blip_frames_left <= 0: @@ -427,7 +437,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # routinely, not just on genuine pothole/override divergence. Curvature mode has always clipped # here; this brings angle mode's actual steering intent in line with that proven behavior rather # than only clipping the value reported to panda (which would make the check a no-op). - current_curvature = -CS.out.yawRate / max(v_ego, 0.1) + current_curvature = self.get_current_curvature(CS) self.bp_curvature_deviation_limited = False if v_ego > 9: _kappa_cmd_pre_error_clip = kappa_cmd @@ -505,7 +515,13 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # BluePilot: the error-clipped kappa path_angle was derived from -- carcontroller.py reads this # as shadow_curvature for ford.h's angle-mode deviation check (see fordcan_ext.create_lka_msg). # Not just telemetry: an actively-consumed value, unlike the removed *_kappa_cmd_raw stubs. - self.bp_kappa_cmd = kappa_cmd + # While the driver is pressing (before the human-turn override latches), the clipped planner + # kappa can't follow the wheel: the driver moves the measured curvature faster than the + # deviation clip tracks it, so the shadow can exit ford.h's error band mid-curve -- the one + # in-drive lateral safety block observed across ~3h of replayed road-test routes was exactly + # this (driver fighting a sustained curve with the mode still enabled). The honest command + # during a press is the driver's actual curvature. + self.bp_kappa_cmd = self.get_current_curvature(CS) if CS.out.steeringPressed else kappa_cmd # BluePilot: would the equivalent curvature (kappa_cmd) have been rate-limited by curvature-mode's # ROC (apply_std_steer_angle_limits)? kappa_cmd is already error-clipped above (same clip diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_curv_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_curv_ext.py index c6c69683b8..c2c6e32938 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_curv_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_curv_ext.py @@ -230,6 +230,17 @@ def _ensure_lateral_curv_initialized(self, CP): # branch LateralCurvExt state is initialized eagerly in __init__, so nothing to do here. pass + def get_current_curvature(self, CS): + """Measured curvature of the car right now (OP sign convention). + + The single measurement source for every BluePilot lateral consumer: the deviation + clip, the stall detector, and the shadow curvature published to ford.h's angle-mode + deviation check. Sourced from the RCM yaw rate -- the same family ford.h derives its + angle_meas from. The shadow value judged against that check must always come from + the same measurement as the check's own reference, so route all reads through here. + """ + return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) + def update_sm(self): """Update SubMaster and vehicle model. Called each frame before lateral/long update.""" self.sm.update(0) @@ -285,7 +296,7 @@ def update(self, CC, CS, actuators, apply_curvature_last, CP): self.pc_blend_ratio_v = [self.pc_blend_ratio_low_C, self.pc_blend_ratio_high_C] # Current and desired curvature - current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) + current_curvature = self.get_current_curvature(CS) desired_curvature = actuators.curvature # Extract predicted curvature from modelV2 diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/__init__.py b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py new file mode 100644 index 0000000000..e79c7230bd --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py @@ -0,0 +1,155 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +# Unit tests for angle-mode shadow-curvature publishing (bp_kappa_cmd). +# +# The shadow value is consumed by carcontroller as the input to ford.h's angle-mode +# deviation check (Lane_Assist_Data1 bytes 5-6, judged against angle_meas). These tests +# pin the truthfulness contract: whenever the planner kappa cannot honestly describe the +# car's steering -- inactive, human-turn override, stall blip, driver pressing -- the +# published shadow must equal the measured curvature, so the panda-latched value always +# stays inside the check's band and re-engage frames never compare a stale zero against +# real measured curvature. + +import unittest +from dataclasses import dataclass +from unittest import mock + +from opendbc.car import structs +from opendbc.car.ford.values import CarControllerParams +from opendbc.car.interfaces import scale_tire_stiffness +from opendbc.sunnypilot.car.ford import lateral_curv_ext +from opendbc.sunnypilot.car.ford.lateral_curv_ext import LateralCurvExt +from opendbc.sunnypilot.car.ford.lateral_angle_ext import LateralAngleExt + + +def _explorer_cp(): + CP = structs.CarParams() + CP.mass = 2050. + CP.wheelbase = 3.025 + CP.steerRatio = 16.8 + CP.centerToFront = CP.wheelbase * 0.44 + CP.tireStiffnessFactor = 0.82 + CP.tireStiffnessFront, CP.tireStiffnessRear = scale_tire_stiffness( + CP.mass, CP.wheelbase, CP.centerToFront, CP.tireStiffnessFactor) + return CP + + +class _FakeLiveDelay: + lateralDelay = 0.2 + + +class _FakeSubMaster: + def __init__(self, *args, **kwargs): + self.updated = {s: False for s in ('modelV2', 'liveParameters', 'selfdriveState', 'radarState', 'liveDelay')} + + def update(self, timeout=0): + pass + + def __getitem__(self, key): + if key == 'liveDelay': + return _FakeLiveDelay() + raise KeyError(key) + + +class _ForcedDetector: + def __init__(self, active): + self.active = active + + def update(self, *_args): + return self.active + + def reset(self): + pass + + +@dataclass +class _CSOut: + vEgoRaw: float = 15.0 + vEgo: float = 15.0 + steeringPressed: bool = False + steeringAngleDeg: float = 0.0 + yawRate: float = 0.0 + + +class _CS: + def __init__(self, **kwargs): + self.out = _CSOut(**kwargs) + self.lat_ctl_lim_stat = 0 + + +@dataclass +class _CC: + latActive: bool = True + + +@dataclass +class _Actuators: + curvature: float = 0.0 + + +class _Harness(LateralCurvExt, LateralAngleExt): + """Mirrors CarController's mixin composition (see carcontroller.py).""" + + def __init__(self, CP): + with mock.patch.object(lateral_curv_ext.messaging, 'SubMaster', _FakeSubMaster): + LateralCurvExt.__init__(self, CP, None) + LateralAngleExt.__init__(self, CP, None) + + +class TestShadowCurvaturePublishing(unittest.TestCase): + V_EGO = 15.0 + YAW_RATE = 0.75 # rad/s -> measured curvature = -0.75 / 15 = -0.05 (OP convention) + + def setUp(self): + self.CP = _explorer_cp() + self.ext = _Harness(self.CP) + self.ext.human_turn_detector = _ForcedDetector(False) + self.cs = _CS(vEgoRaw=self.V_EGO, vEgo=self.V_EGO, yawRate=self.YAW_RATE) + self.measured = -self.YAW_RATE / self.V_EGO + + def _update(self, lat_active=True): + return self.ext.update_angle_strategy(_CC(latActive=lat_active), self.cs, _Actuators(curvature=0.01), self.CP) + + def test_inactive_publishes_measured(self): + result = self._update(lat_active=False) + self.assertEqual(result.path_angle, 0.0) + self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured) + + def test_human_turn_override_publishes_measured(self): + self.ext.human_turn_detector = _ForcedDetector(True) + result = self._update() + self.assertTrue(self.ext.angle_human_turn_active) + self.assertEqual(result.path_angle, 0.0) + self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured) + + def test_stall_blip_publishes_measured(self): + self.ext.stall_blip_frames_left = 3 + result = self._update() + self.assertTrue(self.ext.angle_stall_blip_active) + self.assertEqual(result.path_angle, 0.0) + self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured) + + def test_pressed_publishes_measured(self): + self.cs.out.steeringPressed = True + self._update() + self.assertFalse(self.ext.angle_human_turn_active) + self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured) + + def test_hands_off_publishes_clipped_planner_kappa(self): + # planner wants +0.01 while measured is -0.05: the deviation clip (active above 9 m/s) + # bounds the shadow to measured + CURVATURE_ERROR, not measured itself -- hands-off + # behavior is unchanged by the truthful-shadow sites. + self._update() + expected = self.measured + CarControllerParams.CURVATURE_ERROR + self.assertAlmostEqual(self.ext.bp_kappa_cmd, expected) + self.assertNotAlmostEqual(self.ext.bp_kappa_cmd, self.measured) + self.assertTrue(self.ext.bp_curvature_deviation_limited) + + +if __name__ == '__main__': + unittest.main()