From 4ad433953066b1f22609d0c39dd12fca8da3c710 Mon Sep 17 00:00:00 2001 From: JamesL787 Date: Sun, 18 Oct 2026 10:25:03 -0400 Subject: [PATCH 1/3] lateral: firmware calibrations for every corrected PTM image, R6 computed from Table A, and each image's own E4 scale Every PTM EPS image runs the same firmware control law with its own tables. This brings nrdr's Firmware Controller to the corrected (2026-10-16, revision b) build of every image, models R6 the way the firmware computes it, and sends each image the E4 scale its rows were built for. Each needs its corrected build (REQUIRED in its profile): older builds report the same EPS version. Calibrations carry their own key clamp, P/I trims, fixed command delay and required build. New starting calibrations, ported from JamesL787 ns-bosch-radar-testing: - Civic Bosch TBA-C120, Civic hatch TGG-A120, Civic TBA-A030, CR-V TLA-A040, CR-V TLA-A220. Each corrected build puts the Clarity's R5 for each command key on rows 0-5 (each row keeps its own key axis) and the Clarity's P knots on rows 0-5, scaled to the image's E4, so which variant row a car runs no longer changes its command. Firmware fields are read from each corrected image; load, trims and command delay are carried from the nearest measured car until the owner's drives measure them. The A220 gets its own envelope (1330 from 150 km/h) and is recognized on the A040's Table A (byte-identical). - TEG-A010 has been driven on its corrected build (0000013d), so it is no longer provisional. - C020: the P row's key axis is the image's own (0, 223, 441, 665, 883, ...), not the Clarity's knots. R6, the firmware's rate feedback, is computed from each image's Table A, and clamped: -122 x A[0]/16384 per published deg/s at centre (NORM 1650, the same motor-to-angle constant in every image), shaped by Table A's slope (+/-10 deg chords), clamped at +/-32765 as the firmware does (firmware_r6 did not, so past the rail the feedforward over-modelled damping up to ~2.5x on fast crawl turn-ins and unwinds). Checked against the telemetry R6 word on unrailed frames: mean error 0.7% Clarity, 1.9% C020, 0.8% TEG-A010. The C020's flat -173 (an angle average against 0x14A, ~12% strong at centre) and the C020/TEG telemetry shapes (fitted with railed frames, 4-7% low past 150 deg) are replaced. E4 per unit: every Civic Bosch image (C020, C120, TGG-A120) and the Insight's clamp their command input at key 1663 = E4 3840, in stock firmware as well as the PTM builds (which keep the stock decode). They were sent 4096, so the last ~6% of the output range did nothing and the output-based thresholds were 6.7% off the firmware. nrdr/opendbc (efd32b6f4) now sends HONDA_CIVIC_BOSCH and HONDA_INSIGHT 3840, as the Clarity and Nidec Civic already were, and these calibrations follow in this commit so the inversion and the sent scale cannot drift apart. The C020 and TGG-A120 maps were built for 4096, so per unit of output they now ask 0.94x the R5 they did and reach the clamp at full output; the Firmware Controller's targets are unchanged, the PID path gets ~6% less. The request only shrinks; no limit grows. Checked in every corrected image: the P row runs to 1774 (3840 cars) or 1892 (4096 cars), the key input clamps at 1663 (CR-V 1774), and Kp is a flat 265 at the clamp on every car. Co-Authored-By: Claude Opus 5.5 --- opendbc_repo | 2 +- .../nrdr/docs/firmware_controller_profiles.md | 35 +++- .../features/lateral/controller_selection.py | 32 ++- openpilot/nrdr/features/lateral/honda_vgr.py | 4 +- .../nrdr/features/lateral/latcontrol_fw.py | 33 ++-- .../nrdr/features/lateral/vfn_eps_core.py | 183 +++++++++++++----- .../features/lateral/yaw_control_timing.py | 15 +- .../tests/test_civic_firmware_profiles.py | 77 +++++--- .../nrdr/tests/test_controller_ui_labels.py | 6 +- openpilot/nrdr/tests/test_r6_from_firmware.py | 45 +++++ .../nrdr/tests/test_starting_calibrations.py | 84 ++++++++ .../nrdr/tests/test_vfn_controller_runtime.py | 2 +- openpilot/nrdr/ui/settings/pidf_ground.py | 12 +- .../nrdr/ui/sunnylink/pages/steering.yaml | 2 +- .../sunnypilot/sunnylink/settings_ui.json | 2 +- .../sunnylink/tests/test_capabilities.py | 10 +- 16 files changed, 426 insertions(+), 118 deletions(-) create mode 100644 openpilot/nrdr/tests/test_r6_from_firmware.py create mode 100644 openpilot/nrdr/tests/test_starting_calibrations.py diff --git a/opendbc_repo b/opendbc_repo index f6f8ab891a7..efd32b6f42a 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit f6f8ab891a760f34098b90f6ea0c6cd9c1fd6ddc +Subproject commit efd32b6f42a5d21dfe0c3a8580af2155240755d1 diff --git a/openpilot/nrdr/docs/firmware_controller_profiles.md b/openpilot/nrdr/docs/firmware_controller_profiles.md index f660c39aecb..e28239e00c7 100644 --- a/openpilot/nrdr/docs/firmware_controller_profiles.md +++ b/openpilot/nrdr/docs/firmware_controller_profiles.md @@ -8,23 +8,40 @@ changing it requires offroad confirmation. PIF's Device Yaw Correction is separa | --- | --- | --- | | HONDA_CLARITY / 39990-TRW-A020, modified EPS | Existing Clarity A020 | Measured speed schedule | | HONDA_CIVIC_BOSCH / 39990-TBA-C020, modified EPS | Civic C020 TargetMapD / Tracker4500 / Norm1650 / P117to265 / D737 / KFF45 | Normal live/manual path | -| HONDA_CIVIC / 39990-TEG-A010, modified EPS | TEG-A010 corrected PTM build, command row 3 (provisional until driven on that build) | Normal live/manual path | -| HONDA_INSIGHT / 39990-TXM-A040, modified EPS | Insight TXM-A040 corrected PTM build (the Clarity's own command map and P table, 4096 E4 per unit, measured load, R6 from its A table) | Clarity's measured speed schedule | +| HONDA_CIVIC / 39990-TEG-A010, modified EPS | TEG-A010 corrected PTM build, command row 3 (driven on that build) | Normal live/manual path | +| HONDA_INSIGHT / 39990-TXM-A040, modified EPS | Insight TXM-A040 corrected PTM build (the Clarity's own command map and P table, 3840 E4 per unit, measured load, R6 from its A table) | Clarity's measured speed schedule | +| HONDA_CIVIC_BOSCH / 39990-TBA-C120, modified EPS | C120 corrected PTM starting calibration: rows 0-4 the Clarity's R5 per key (3840 E4 per unit, its image's key clamp 1663), R6 from its A table, the C020's load and trims | Normal live/manual path | +| HONDA_CIVIC_BOSCH / 39990-TGG-A120, modified EPS | TGG-A120 corrected PTM starting calibration: row 0, built for 4096 E4 per unit but sent 3840 (its key clamp 1663), R6 from its A table (the C020's), the C020's load and trims | Normal live/manual path | +| HONDA_CIVIC / 39990-TBA-A030, modified EPS | A030 corrected PTM starting calibration: the TEG-A010's row 3 on every row, R6 from its A table (the TEG's), the TEG's measured load | Normal live/manual path | +| HONDA_CRV_5G / 39990-TLA-A040, modified EPS | CR-V A040 corrected PTM starting calibration: row 0, key clamp 1774, R6 from its A table, the owner's measured load, untrimmed | Normal live/manual path | +| HONDA_CRV_5G / 39990-TLA-A220, modified EPS | CR-V A220 corrected PTM starting calibration: rows 0-5 Clarity-equivalent, its own envelope, the A040's A table, R6, load and trims | Normal live/manual path | PID CarParams and a matching firmware steer-ratio profile are also required. No -other Civic, firmware version, diesel, CR-V, or generic Honda inherits the fallback. +other Civic, firmware version, diesel, CR-V, or generic Honda inherits a calibration. +The five starting calibrations are provisional: their firmware fields are read from +each corrected image, while load, trims and command delay are carried from the +nearest measured car until the owner's drives measure them. ## Required corrected firmware The Civic TEG-A010 calibration requires `39990-TEG,A010-20261016b-PTMFix-ClarityEquivMap-ClarityPKnots-Row6Stock-A280FlatRows023` -(RWD SHA-256 starts `04147917`). It replaces the Bosch placeholder, but remains -provisional: it has not yet been road-validated on this corrected build. +(RWD SHA-256 starts `04147917`). It replaces the Bosch placeholder. It has been +driven on this corrected build (owner's route `0000013d`, 2026-10-17): the car runs +the corrected map (median R5 residual 35 counts over 104k frames, against 822 for the +earlier build's), the column load +holds out at R^2 0.810 (the same as a refit), and its telemetry R6 word agrees with the R6 computed from its Table A +(-156.16 at centre) within 0.8% on unrailed frames. The Insight TXM-A040 calibration requires `39990-TXM,A040-PTM-20261016b` (RWD SHA-256 starts `7ba37432`). Its source configuration has been driven on the corrected build; that is not independent validation of this port. +The starting calibrations require, by RWD SHA-256 prefix: Civic Bosch TBA-C120 +`9261b8db`, Civic TGG-A120 `6640806d`, Civic TBA-A030 `55c37266`, CR-V TLA-A040 +`2e6a8802`, CR-V TLA-A220 `2c992997` (each `...-20261016b-PTMFix-ClarityEquivMap-...`). +The controller logs the required build at start and shows it in its description. + Older builds report the same EPS version strings as these corrected builds. Family admission cannot verify the actual flashed image. Confirm the flashed build separately before selecting Firmware Controller; do not pair these @@ -58,13 +75,13 @@ newer angle-dependent R6 conversion and all existing core numerical behavior. TEG-A010 now uses its corrected firmware's command row 3 and shared Clarity P knots, with its own telemetry-derived column load and angle-dependent R6. It retains the C020 base gains, P/I trims and Civic command delay. Its existing -3840-count transport and user-selected geometry are unchanged. It remains -provisional, especially at crawl speeds not covered by the source telemetry. +3840-count transport and user-selected geometry are unchanged. Crawl speeds +are still thinly covered by its owner's drives (few turns below 9 m/s). Insight TXM-A040 uses the corrected firmware's Clarity command map and P table, its own load and angle-dependent R6, the C020 base gains/P-I trims, Civic command -delay, and Clarity prediction schedule. Its 4096-count transport is accounted -for in the inversion; no transport or safety limit is increased. +delay, and Clarity prediction schedule. It is sent 3840 counts per unit, the +image's own command clamp (as the Clarity); no transport or safety limit is increased. Shared steer-ratio selection, driver override, torque clipping, safety checks, inactive reset/history behavior and single output LPF are preserved. Optimized diff --git a/openpilot/nrdr/features/lateral/controller_selection.py b/openpilot/nrdr/features/lateral/controller_selection.py index 5387db25d39..c24d2dbae42 100644 --- a/openpilot/nrdr/features/lateral/controller_selection.py +++ b/openpilot/nrdr/features/lateral/controller_selection.py @@ -3,8 +3,9 @@ Firmware version strings identify the family, not the contents of a modified image. The owner must still confirm the flashed build. TEG-A010 runs its own calibration for the corrected PTM build (command row 3; rows 2 and 3 make the same command there), with R6 and load measured on one -car's telemetry. It stays provisional until it has been driven on that build, and it is never a +car's telemetry, and driven on that build (owner's route 0000013d, 2026-10-17). It is never a generic Honda fallback. Insight TXM-A040 runs its corrected PTM build's calibration, which has been driven. +The corrected builds of the C120, TGG-A120, A030 and both CR-V images have provisional starting calibrations. """ from dataclasses import dataclass @@ -19,21 +20,46 @@ class FirmwareControllerProfile: calibration: str prediction_schedule: bool = False provisional: bool = False + # The corrected PTM build the calibration models (RWD SHA-256 prefix). Older builds report the same EPS version, + # so admission cannot check it; it is shown and logged so the owner can. + required_rwd: str = "" CLARITY_PROFILE = FirmwareControllerProfile("Clarity A020", "clarity_a020", prediction_schedule=True) CIVIC_PROFILE = FirmwareControllerProfile("Civic Bosch C020", "civic_bosch_c020") TEG_PROFILE = FirmwareControllerProfile( - "Civic TEG-A010 corrected PTM, row 3 (not yet driven)", "civic_teg_a010", provisional=True, + "Civic TEG-A010 corrected PTM, row 3", "civic_teg_a010", required_rwd="04147917", ) INSIGHT_PROFILE = FirmwareControllerProfile( - "Insight TXM-A040 corrected PTM", "insight_txm_a040", prediction_schedule=True, + "Insight TXM-A040 corrected PTM", "insight_txm_a040", prediction_schedule=True, required_rwd="7ba37432", +) +# Corrected-PTM (revision b) starting calibrations: firmware read from each image, the vehicle side carried from +# the nearest measured car until its owner's drives measure it. +C120_PROFILE = FirmwareControllerProfile( + "Civic Bosch TBA-C120 corrected PTM (starting calibration)", "civic_tba_c120", provisional=True, required_rwd="9261b8db", +) +TGG_PROFILE = FirmwareControllerProfile( + "Civic TGG-A120 corrected PTM (starting calibration)", "civic_tgg_a120", provisional=True, required_rwd="6640806d", +) +A030_PROFILE = FirmwareControllerProfile( + "Civic TBA-A030 corrected PTM (starting calibration)", "civic_tba_a030", provisional=True, required_rwd="55c37266", +) +CRV_A040_PROFILE = FirmwareControllerProfile( + "CR-V TLA-A040 corrected PTM (starting calibration)", "crv_tla_a040", provisional=True, required_rwd="2e6a8802", +) +CRV_A220_PROFILE = FirmwareControllerProfile( + "CR-V TLA-A220 corrected PTM (starting calibration)", "crv_tla_a220", provisional=True, required_rwd="2c992997", ) _PROFILES = { ("HONDA_CLARITY", "39990-TRW-A020"): CLARITY_PROFILE, ("HONDA_CIVIC_BOSCH", "39990-TBA-C020"): CIVIC_PROFILE, ("HONDA_CIVIC", "39990-TEG-A010"): TEG_PROFILE, ("HONDA_INSIGHT", "39990-TXM-A040"): INSIGHT_PROFILE, + ("HONDA_CIVIC_BOSCH", "39990-TBA-C120"): C120_PROFILE, + ("HONDA_CIVIC_BOSCH", "39990-TGG-A120"): TGG_PROFILE, + ("HONDA_CIVIC", "39990-TBA-A030"): A030_PROFILE, + ("HONDA_CRV_5G", "39990-TLA-A040"): CRV_A040_PROFILE, + ("HONDA_CRV_5G", "39990-TLA-A220"): CRV_A220_PROFILE, } diff --git a/openpilot/nrdr/features/lateral/honda_vgr.py b/openpilot/nrdr/features/lateral/honda_vgr.py index 609ff625dc7..8e606b3781c 100644 --- a/openpilot/nrdr/features/lateral/honda_vgr.py +++ b/openpilot/nrdr/features/lateral/honda_vgr.py @@ -126,7 +126,8 @@ def physical_to_linear(self, angle_deg: float) -> float: ("39990-TBA-C120", "39990-TBA-A030", "39990-TEG-A010"), _CIVIC_C120_X, _CIVIC_C120_Y), HondaVgrProfile("Civic C020/TGG-A120", ("HONDA_CIVIC_BOSCH",), ("39990-TBA-C020", "39990-TGG-A120"), _CIVIC_C020_X, _CIVIC_C020_Y), - HondaVgrProfile("CR-V TLA-A040", ("HONDA_CRV_5G",), ("39990-TLA-A040",), _TLA_A040_X, _TLA_A040_Y), + # The corrected TLA-A220 image carries the A040's Table A byte for byte (same offset, same values). + HondaVgrProfile("CR-V TLA-A040", ("HONDA_CRV_5G",), ("39990-TLA-A040", "39990-TLA-A220"), _TLA_A040_X, _TLA_A040_Y), HondaVgrProfile("Insight TXM-A040", ("HONDA_INSIGHT",), ("39990-TXM-A040",), _TXM_A040_X, _TXM_A040_Y), ) @@ -138,6 +139,7 @@ def physical_to_linear(self, angle_deg: float) -> float: ("HONDA_CIVIC_BOSCH", "39990-TBA-C020"): HONDA_VGR_PROFILES[2], ("HONDA_CIVIC_BOSCH", "39990-TGG-A120"): HONDA_VGR_PROFILES[2], ("HONDA_CRV_5G", "39990-TLA-A040"): HONDA_VGR_PROFILES[3], + ("HONDA_CRV_5G", "39990-TLA-A220"): HONDA_VGR_PROFILES[3], ("HONDA_INSIGHT", "39990-TXM-A040"): HONDA_VGR_PROFILES[4], } diff --git a/openpilot/nrdr/features/lateral/latcontrol_fw.py b/openpilot/nrdr/features/lateral/latcontrol_fw.py index 4b2a7efcb4c..42fba416f2b 100644 --- a/openpilot/nrdr/features/lateral/latcontrol_fw.py +++ b/openpilot/nrdr/features/lateral/latcontrol_fw.py @@ -7,9 +7,9 @@ delay ends (NrdrYawCommandDelayLow/High) remain Clarity-specific. Civic uses the source Civic schedule plus the existing SunnyPilot port compensation. TEG-A010 has its own calibration (the corrected PTM build's row 3, 3840 E4 per unit, its own -measured load and R6) and keeps the C020's base gains, trims and command delay; it -stays provisional. Insight TXM-A040 has its own calibration for its corrected PTM build -(the Clarity's own map and P table, 4096 E4 per unit, measured load, R6 from its A table) +measured load, R6 from its A table) and keeps the C020's base gains, trims and command delay; it has +been driven on its corrected build. Insight TXM-A040 has its own calibration for its corrected PTM build +(the Clarity's own map and P table, 3840 E4 per unit, measured load, R6 from its A table) with the C020's trims, the Civic command delay and the Clarity's model-delay schedule. """ import numpy as np @@ -19,8 +19,9 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.nrdr.features.lateral.controller_selection import firmware_controller_profile from openpilot.nrdr.features.lateral.vfn_eps_core import ( - CIVIC_BOSCH_C020, CIVIC_EPS_LOAD, CIVIC_I_SCALE, CIVIC_P_SCALE, CIVIC_TEG_A010, CIVIC_TEG_EPS_LOAD, INSIGHT_TXM_A040, - INSIGHT_TXM_EPS_LOAD, + CIVIC_BOSCH_C020, CIVIC_EPS_LOAD, CIVIC_I_SCALE, CIVIC_P_SCALE, CIVIC_TBA_A030, CIVIC_TBA_C120, CIVIC_TEG_A010, + CIVIC_TEG_EPS_LOAD, CIVIC_TGG_A120, CRV_I_SCALE, CRV_P_SCALE, CRV_TLA_A040, CRV_TLA_A220, CRV_TLA_EPS_LOAD, + INSIGHT_TXM_A040, INSIGHT_TXM_EPS_LOAD, HondaEpsFirmwareFeedforward, HondaEpsLateralCore, ) from openpilot.nrdr.features.lateral.yaw_control_timing import CommandDelay, command_delay, prediction_delay_schedule @@ -32,11 +33,17 @@ KP = [0.018, 0.024, 0.048, 0.060] KI = [0.006, 0.008, 0.016, 0.020] ANGLE_RATE_LIMIT = 300.0 -# Calibrations with their own image constants and column load; the Clarity uses the module defaults. +# Calibrations with their own image constants, column load and residual-PID trims; the Clarity uses the module defaults. _OWN_CALIBRATIONS = { - "civic_bosch_c020": (CIVIC_BOSCH_C020, CIVIC_EPS_LOAD), - "civic_teg_a010": (CIVIC_TEG_A010, CIVIC_TEG_EPS_LOAD), - "insight_txm_a040": (INSIGHT_TXM_A040, INSIGHT_TXM_EPS_LOAD), + "civic_bosch_c020": (CIVIC_BOSCH_C020, CIVIC_EPS_LOAD, CIVIC_P_SCALE, CIVIC_I_SCALE), + "civic_teg_a010": (CIVIC_TEG_A010, CIVIC_TEG_EPS_LOAD, CIVIC_P_SCALE, CIVIC_I_SCALE), + "insight_txm_a040": (INSIGHT_TXM_A040, INSIGHT_TXM_EPS_LOAD, CIVIC_P_SCALE, CIVIC_I_SCALE), + # Corrected-PTM starting calibrations: load and trims carried from the nearest measured car (see vfn_eps_core) + "civic_tba_c120": (CIVIC_TBA_C120, CIVIC_EPS_LOAD, CIVIC_P_SCALE, CIVIC_I_SCALE), + "civic_tgg_a120": (CIVIC_TGG_A120, CIVIC_EPS_LOAD, CIVIC_P_SCALE, CIVIC_I_SCALE), + "civic_tba_a030": (CIVIC_TBA_A030, CIVIC_TEG_EPS_LOAD, CIVIC_P_SCALE, CIVIC_I_SCALE), + "crv_tla_a040": (CRV_TLA_A040, CRV_TLA_EPS_LOAD, CRV_P_SCALE, CRV_I_SCALE), + "crv_tla_a220": (CRV_TLA_A220, CRV_TLA_EPS_LOAD, CRV_P_SCALE, CRV_I_SCALE), } @@ -51,15 +58,15 @@ def __init__(self, CP, CP_SP, CI, dt): if self.firmware_profile.calibration in _OWN_CALIBRATIONS: # Source Bosch base gains as well as its fixed trims, also on the TEG and the Insight, whose # trims are not measured. Never retune CP or increase CAN/safety limits. - cal, load = _OWN_CALIBRATIONS[self.firmware_profile.calibration] + cal, load, p_scale, i_scale = _OWN_CALIBRATIONS[self.firmware_profile.calibration] self.core = HondaEpsLateralCore(GAIN_BP, KP, GAIN_BP, KI, dt, ff=HondaEpsFirmwareFeedforward(dt, cal=cal, load=load), - p_scale=CIVIC_P_SCALE, i_scale=CIVIC_I_SCALE) + p_scale=p_scale, i_scale=i_scale) else: self.core = HondaEpsLateralCore(GAIN_BP, KP, GAIN_BP, KI, dt) - cloudlog.info("Firmware Controller profile=%s calibration=%s provisional=%s prediction_schedule=%s", + cloudlog.info("Firmware Controller profile=%s calibration=%s provisional=%s prediction_schedule=%s required_rwd=%s", self.firmware_profile.name, self.firmware_profile.calibration, - self.firmware_profile.provisional, self.delay_schedule) + self.firmware_profile.provisional, self.delay_schedule, self.firmware_profile.required_rwd or "-") self.cmd_delay = CommandDelay(dt) self.model_v2 = None self.prev_angle = 0.0 diff --git a/openpilot/nrdr/features/lateral/vfn_eps_core.py b/openpilot/nrdr/features/lateral/vfn_eps_core.py index f29d68c340e..7373fb7cf3c 100644 --- a/openpilot/nrdr/features/lateral/vfn_eps_core.py +++ b/openpilot/nrdr/features/lateral/vfn_eps_core.py @@ -1,14 +1,14 @@ """Honda modified-EPS firmware-inversion control, with isolated per-image calibrations. Civic C020 calibration/load/P-I trims adapted from JamesL787/openpilot -59eb99e3183eb68963394daee8e52e51eb034f38. Clarity retains the newer angle-dependent -R6 conversion below; the Civic source's older constant Clarity R6 is NOT ported. -TEG-A010 has its own calibration (the corrected PTM build's row 3, R6 and load from its owner's telemetry), -still provisional. +59eb99e3183eb68963394daee8e52e51eb034f38. Every image's R6 is computed from its own Table A +(computed_r6); the Civic source's older constant Clarity R6 is NOT ported. +TEG-A010 has its own calibration (the corrected PTM build's row 3, load from its owner's telemetry), +driven on that build (owner's route 0000013d, 2026-10-17). The Clarity's LKAS path is not a torque command. The firmware turns our 0xE4 value into a target R5, -compares it with R6 -- a filtered steering RATE, taken before its angle table (see R6_PER_CENTRE_DEG_S) -- -and runs P + D + KFF on the difference at 1 kHz. So every command first has to cancel +compares it with R6 -- a filtered steering RATE, taken before its angle table (see computed_r6) -- +and runs P + D + KFF on the difference in its fast control loop (1-2 kHz). So every command first has to cancel the firmware's own rate damping (Kp * 133 / 1024 = 15..34 counts per deg/s, 2-5x the rack's physical damping), which is why vfn's angle PID trails a turn-in by ~250 ms x steering rate below 25 mph. On a turn exit that same damping is the braking that holds the line -- which is why the symmetric rate feedforward @@ -36,7 +36,7 @@ import numpy as np -from openpilot.nrdr.features.lateral.vfn_geometry import NRDR_CLARITY_VGR_ANGLE_BP, NRDR_CLARITY_VGR_LINEAR_BP +from openpilot.nrdr.features.lateral.honda_vgr import HONDA_VGR_PROFILES, HondaVgrProfile from openpilot.common.filter_simple import FirstOrderFilter from openpilot.nrdr.features.lateral.integral_pid import IntegralScaledPIDController as PIDController @@ -63,20 +63,47 @@ for lo, hi, kp_lo, kp_hi in zip(KP_KEY_BP[:-1], KP_KEY_BP[1:], KP_V[:-1], KP_V[1:], strict=True)] KP_PIECES.append((KP_KEY_BP[-1] * R5_PER_KEY, math.inf, KP_V[-1], 0.0)) -# R6 comes from the column rate BEFORE the firmware's angle tables, the domain 0x18F STEER_ANGLE_RATE reports -# (R6 = -31.6 per 0x18F count, flat to 2% at every angle on route 369). The feedforward's rate is the derivative -# of the published (0x14A) angle, which the A (position) table compresses, so per deg/s of it the damping grows -# with angle by the A table's local slope (the chain rule): engaged, routes 363/365/366/369, -119 near centre -# and -141..-146 past 60 deg, where a single -133 was 11% too strong near centre and 10% too weak in big turns. -# Scaled by the slope it is flat at -120..-123 on each route (-122 pooled). -# steeringRateDeg (0x14A STEER_ANGLE_RATE) is NOT that derivative: the firmware publishes it through its second, -# B (rate) table, indexed by angle. It reads 1.1% fast at centre (B[0] 16204 vs 16384) and 2-5% slow at 45-100 -# deg, so don't feed it here without converting (see steer_ratio.py). -R6_PER_CENTRE_DEG_S = -122.0 # NORM 1650 / tracker-1 3200 -# d(pre-table angle) / d(published angle) along the A020 angle table, 1.0 at centre, ~1.19 from 150 deg -_VGR_SLOPE = np.gradient(NRDR_CLARITY_VGR_LINEAR_BP, NRDR_CLARITY_VGR_ANGLE_BP) -R6_ANGLE_BP = np.asarray(NRDR_CLARITY_VGR_ANGLE_BP) -R6_ANGLE_GAIN = _VGR_SLOPE / _VGR_SLOPE[0] +# R6, the firmware's rate feedback, is computed from the image's own Table A, not measured. The estimator (FUN_343FC) +# takes the per-sample advance of the motor-side, PRE-table angle; NORM scales it into the tracker-1 target (1650 in +# every corrected image: Clarity, C020, C120, TGG-A120, A030, TEG-A010, TXM-A040, TLA-A040, TLA-A220; stock is 3429) and +# tracker-1 filters it (its coefficient, 3200 / 4000 / 4250 by image, sets a 4-5 ms time constant, not a gain). The +# motor-to-angle constant (3121) is the same in every image, so per published deg/s (the derivative of the 0x14A angle, +# which comes out of Table A: published = raw * 2**14 / A): +# R6(angle) = -122 * (A[0] / 16384) * slope(angle) / slope(0), then clamped to +/-32765 (0x1380C, every image) +# with slope = d(linear)/d(published) as a +/-10 deg chord of Table A and -122 the Clarity's R6 per raw deg/s at NORM +# 1650 (flat at -120..-123 on routes 363/365/366/369 once scaled by the slope). Checked 2026-10-18 against the telemetry +# R6 word on every car that has it, unrailed frames (10-150 deg/s): mean |error| over the angle bands 0.7% on the +# Clarity (363/365/369), 1.9% on the C020 (287/289/294), 0.8% on the TEG-A010 (133/134/137/13d). The rail is real: +# R6 sits at 32765 for 92% of frames at 200-260 deg/s on the Civics (from ~185 deg/s; ~260 on the Clarity), which is +# why the C020's and TEG's earlier telemetry-fitted shapes read 4-7% low past 150 deg (railed frames in the fit). +# Ported from JamesL787 ns-bosch-radar-testing (computed_r6). +# steeringRateDeg (0x14A STEER_ANGLE_RATE) is NOT the derivative of the published angle: the firmware publishes it +# through its second, B (rate) table. It reads 1.1% fast at centre on the Clarity and 2-5% slow at 45-100 deg, so +# don't feed it here without converting (see steer_ratio.py). +R6_PER_RAW_DEG_S = -122.0 # NORM 1650 +R6_CLAMP = 32765.0 +COMPUTED_R6_ANGLE_BP = (0, 15, 30, 45, 60, 75, 90, 105, 120, 135, 150, 180, 210, 240, 270, 300, 360, 420, 480) # |angle| + + +def computed_r6(profile: HondaVgrProfile, norm: float = 1650.0) -> tuple[float, tuple, tuple]: + """(R6 per published deg/s at centre, |angle| axis, gain by angle) from an image's Table A; see above.""" + def slope(a: float) -> float: + lo = max(a - 10.0, 0.0) + return (profile.physical_to_linear(a + 10.0) - profile.physical_to_linear(lo)) / (a + 10.0 - lo) + + centre = R6_PER_RAW_DEG_S * norm / 1650.0 * profile.center_divisor / 16384.0 + return round(centre, 2), COMPUTED_R6_ANGLE_BP, tuple(round(slope(a) / slope(0.0), 4) for a in COMPUTED_R6_ANGLE_BP) + + +def _table_a(name: str) -> HondaVgrProfile: + return next(p for p in HONDA_VGR_PROFILES if p.name == name) + + +R6_PER_CENTRE_DEG_S, R6_ANGLE_BP, R6_ANGLE_GAIN = computed_r6(_table_a("Clarity TRW-A020")) +CIVIC_C020_R6 = computed_r6(_table_a("Civic C020/TGG-A120")) # C020, TGG-A120 +CIVIC_C120_R6 = computed_r6(_table_a("Civic C120/A030/TEG")) # C120, A030, TEG-A010 +CRV_TLA_R6 = computed_r6(_table_a("CR-V TLA-A040")) # TLA-A040, TLA-A220 +INSIGHT_TXM_R6 = computed_r6(_table_a("Insight TXM-A040")) # 0x6A2 scale word while the request is held, re-measured on routes 35e/360/361: median 256 (252 on 352). Hands # off it only moves on a fast column-torque RATE (A280) or above ~400 counts of torque (helper A). SCALE_Q8 = 256.0 # helper A * B / 256 while the request is held @@ -152,8 +179,11 @@ class EpsFirmwareCalibration: """Per-image command map, P key axis, envelope and rate feedback; no shared mutable state.""" def __init__(self, e4_per_output, r5_key_bp, r5_v, envelope_bp, envelope_v, r6_per_deg_s, - *, r5_per_key=None, r6_angle_bp=None, r6_angle_gain=None, kp_key_bp=None): + *, r5_per_key=None, r6_angle_bp=None, r6_angle_gain=None, kp_key_bp=None, key_clamp=KEY_CLAMP): self.e4_per_output = e4_per_output + # The command-input clamp, the header word just before the row 0 command map: 1663 (E4 3840) on every image here + # except the CR-V's, which clamp at 1774 (E4 4096). + self.key_clamp = key_clamp self.r5_key_bp, self.r5_v = tuple(r5_key_bp), tuple(r5_v) self.envelope_bp, self.envelope_v = tuple(envelope_bp), tuple(envelope_v) self.r6_per_deg_s = r6_per_deg_s @@ -184,13 +214,20 @@ def __init__(self, e4_per_output, r5_key_bp, r5_v, envelope_bp, envelope_v, r6_p # Only for the C020 TargetMapD / Tracker4500 / Norm1650 / P117to265 / D737 / # KFF45 modified build. Version strings alone cannot confirm this image. The # source's high-speed envelope is firmware-derived, not road-validated at its rail. +# R6 computed from its Table A (-153.74 at centre; the telemetry R6 word on the owner's routes 287/289/294 agrees within +# 1.9% unrailed). The flat -173 it replaces was an angle average fitted against 0x14A: ~12% too strong at centre and too +# weak in big turns. The P row's key axis is the image's own (0x13B5E row 0 and row 1 alike), not the Clarity's knots. +# HONDA_CIVIC_BOSCH is sent 3840 E4 per unit: the image clamps its command input at key 1663 = E4 3840 (as stock +# firmware does), so 4096 left the last ~6% of the output range dead. Its map was built for 4096, so per unit of output +# it now asks 0.94x the R5 it did, and reaches the clamp at full output; the inversion uses 3840. CIVIC_BOSCH_C020 = EpsFirmwareCalibration( - e4_per_output=4096.0, + e4_per_output=3840.0, r5_key_bp=[0, 115, 254, 449, 654, 862, 1111, 1549, 1774], r5_v=[0, 1926, 4938, 8455, 12036, 15926, 20138, 26955, 30000], envelope_bp=[0, 50, 100, 150, 200, 240, 300, 321, 400], envelope_v=[1774, 1774, 1774, 1774, 1774, 1552, 1219, 1108, 1108], - r6_per_deg_s=-173.0, + r6_per_deg_s=CIVIC_C020_R6[0], r6_angle_bp=CIVIC_C020_R6[1], r6_angle_gain=CIVIC_C020_R6[2], + kp_key_bp=(0, 223, 441, 665, 883, 1108, 1330, 1552, 1774), ) CLARITY_EPS_LOAD = (LOAD_K0, LOAD_K1, LOAD_C, LOAD_FRICTION, LOAD_BIAS, LOAD_KROLL) # Source fit on Civic C020 firmware-controller route 294 (not its separate PID shadow fit). @@ -204,24 +241,17 @@ def __init__(self, e4_per_output, r5_key_bp, r5_v, envelope_bp, envelope_v, r6_p # (a knot past the 1663 key clamp sits on the line through the Clarity's 30000 at 1663), and it moves rows 0-5 # P onto the Clarity's knots. So rows 2 and 3 now make the same R5 for the same key, and the TEGA1/TEGA2 # question no longer changes the command. Row 3 and the envelope (= the C020's) are read from the image. -# HONDA_CIVIC is sent 3840 E4 per unit of output, as the Clarity is, not the C020's 4096. -# R6 and the column load below come from that telemetry, the earlier build. The fix touches neither the rate -# path (tracker-1 3200, NORM 1650) nor the row 0-5 P/D/KFF values, so they carry over. R6: the telemetry R6 word -# (0x6A1 w1, tracker-1's filtered output) at centre, -152.5 per deg/s (route 00000139, R^2 0.94). Its angle shape is -# measured per published deg/s (the rate this feedforward differentiates): the R6 word against d(0x14A angle)/dt, both -# as 0.2 s window means, routes 00000133/134/137 (centre -149.8 / -152.4 / -151.9). By |angle| band 0-15 / 15-45 / -# 45-90 / 90-150 / 150-250 / 250-420 deg: 1.000 / 1.006 / 1.094 / 1.126 / 1.165 / 1.093; the routes agree within -# 0.02-0.04 up to 250 deg. Flat was 9-15% weak past 45 deg. Same CAN layout as the Clarity: R6 is flat against 0x18F -# (the raw rate) to 150 deg, and 0x14A's angle and rate are the published ones. -CIVIC_TEG_A010_R6_BP = (0.0, 30.0, 67.5, 120.0, 200.0, 335.0) -CIVIC_TEG_A010_R6_GAIN = (1.0, 1.006, 1.094, 1.126, 1.165, 1.093) +# HONDA_CIVIC is sent 3840 E4 per unit of output, as the Clarity is. +# The column load below comes from that telemetry, the earlier build; the fix touches neither the rate path +# (tracker-1 3200, NORM 1650) nor the row 0-5 P/D/KFF values, so it carries over. R6 is computed from its Table A (the +# C120's, -156.16 at centre); the telemetry R6 word on routes 133/134/137/13d agrees within 0.8% unrailed. CIVIC_TEG_A010 = EpsFirmwareCalibration( e4_per_output=3840.0, r5_key_bp=[0, 222, 333, 495, 656, 887, 1108, 1552, 1774], r5_v=[0, 4000, 6000, 8937, 11838, 16000, 20000, 28000, 32000], envelope_bp=[0, 50, 100, 150, 200, 240, 300, 321, 400], envelope_v=[1774, 1774, 1774, 1774, 1774, 1552, 1219, 1108, 1108], - r6_per_deg_s=-152.5, r6_angle_bp=CIVIC_TEG_A010_R6_BP, r6_angle_gain=CIVIC_TEG_A010_R6_GAIN, + r6_per_deg_s=CIVIC_C120_R6[0], r6_angle_bp=CIVIC_C120_R6[1], r6_angle_gain=CIVIC_C120_R6[2], ) # TEG column, the same recipe as the Clarity's and the C020's: target = the telemetry's own P + D + KFF*R5/1024 # (row-independent), engaged, unpressed, |steering torque| < 300, motion lag fitted; 81 min of frames on the 11 @@ -238,27 +268,88 @@ def __init__(self, e4_per_output, r5_key_bp, r5_v, envelope_bp, envelope_v, r6_p # every key). Row 0 is live: all three variant records (TXMA0/1/2) select row 0. Road-proven on JamesL787 # ns-bosch-radar-testing (f3a6a124bd) on this build. Both builds report 39990-TXM,A040: a car on the released image must # be reflashed with this calibration. -# This tree sends the Insight 4096 E4 per unit of output (opendbc _EXTENDED_TORQUE_LIMITS), not 3840, so the 1663 key -# clamp binds at u ~0.94; the inversion uses that scale, so the firmware target is unchanged. The envelope was not +# This tree sends the Insight 3840 E4 per unit of output (opendbc _EXTENDED_TORQUE_LIMITS), the image's own 1663 key +# clamp, as the Clarity: the same output makes the same firmware target as on the Clarity. The envelope was not # located in the image: flat (the key clamp binds first). R6 is computed from its A table (centre divisor 17613), # per published deg/s. Load: the owner's konik route 0000001e, the Clarity recipe, in firmware output counts (so it # holds across the reflash); route 00000020 held out scores R^2 0.70 / 0.75 against 0.70 / 0.75 for a fit on it. -INSIGHT_TXM_R6_ANGLE_BP = (0, 15, 30, 45, 60, 75, 90, 105, 120, 135, 150, 180, 210, 240, 270, 300, 360, 420, 480) -INSIGHT_TXM_R6_ANGLE_GAIN = (1.0, 1.0247, 1.0326, 1.052, 1.0852, 1.1305, 1.1692, 1.207, 1.2242, 1.2239, 1.2156, 1.2234, - 1.2254, 1.2249, 1.2273, 1.2253, 1.2279, 1.2436, 1.1917) INSIGHT_TXM_A040 = EpsFirmwareCalibration( - 4096.0, R5_KEY_BP, R5_V, [0, 50, 100, 150, 200, 250, 300, 350, 400], [1774] * 9, -131.15, - r5_per_key=R5_PER_KEY, r6_angle_bp=INSIGHT_TXM_R6_ANGLE_BP, r6_angle_gain=INSIGHT_TXM_R6_ANGLE_GAIN, + 3840.0, R5_KEY_BP, R5_V, [0, 50, 100, 150, 200, 250, 300, 350, 400], [1774] * 9, INSIGHT_TXM_R6[0], + r5_per_key=R5_PER_KEY, r6_angle_bp=INSIGHT_TXM_R6[1], r6_angle_gain=INSIGHT_TXM_R6[2], ) INSIGHT_TXM_EPS_LOAD = (-4.876, -0.1376, -4.452, -397.2, 65.41, -4.29) +# Starting calibrations for the corrected PTM builds of 2026-10-16 (revision b), ported from JamesL787 +# ns-bosch-radar-testing. Each build is a data-only fix that puts the Clarity's R5 for each command key on rows 0-5 +# (each row keeps its own key axis) and the Clarity's P knots on rows 0-5, so which variant row a car runs no longer +# changes its command. Firmware fields are read from each corrected image; R6 is computed from its Table A (see +# computed_r6); load, trims +# and command delay are carried from the nearest measured car until the owner's drives measure them. Each needs its +# corrected build: older builds report the same EPS version. + +# Civic Bosch hatch 39990-TBA-C120 (RWD 9261b8db...). Rows 1 and 2 shown; rows 0-4 make the same R5 per key. The image +# clamps at 1663 and builds its rows at E4 3840, and HONDA_CIVIC_BOSCH is sent 3840 per unit, so the full output range +# reaches the clamp, as on the Clarity. R6 from its Table A (centre divisor 20972); load and trims the C020's, command +# delay the C020's. +CIVIC_TBA_C120 = EpsFirmwareCalibration( + 3840.0, (0, 103, 263, 459, 660, 861, 1111, 1549, 1774), (0, 1856, 4739, 8288, 11910, 15532, 20054, 27946, 32000), + (0, 50, 100, 150, 200, 250, 300, 350, 400), (1774,) * 9, CIVIC_C120_R6[0], + r6_angle_bp=CIVIC_C120_R6[1], r6_angle_gain=CIVIC_C120_R6[2], +) + +# Civic hatch 39990-TGG-A120 (RWD 6640806d...). Both variant records (TGGA5, TGGA6) select row 0. Built for 4096 E4 per +# unit: each key carries the Clarity's R5 at key x 3840/4096, and P sits on the Clarity's knots at the same scale. It +# clamps its command input at key 1663 = E4 3840, so it is sent 3840 (per unit of output 0.94x the R5 the map was built +# for, reaching the clamp at full output). It shares the C020's Table A, so the C020's computed R6; load, trims and +# command delay the C020's. +CIVIC_TGG_A120 = EpsFirmwareCalibration( + 3840.0, (0, 103, 263, 459, 660, 862, 1111, 1549, 1774), (0, 1740, 4443, 7769, 11167, 14579, 18798, 26202, 30002), + (0, 50, 100, 150, 200, 250, 300, 350, 400), (1774,) * 9, CIVIC_C020_R6[0], + r6_angle_bp=CIVIC_C020_R6[1], r6_angle_gain=CIVIC_C020_R6[2], + kp_key_bp=(0, 237, 473, 709, 946, 1182, 1419, 1655, 1892), +) + +# Civic 39990-TBA-A030 (RWD 55c37266...): the TEG-A010's row 3 on rows 0-5, so every variant (TBCA1 row 2, TBCA2 +# row 3) makes the same command as the TEG. Same Table A, tracker-1 and NORM as the TEG, so the TEG's computed R6; its +# envelope is the C020's. Load the TEG owner's, trims the C020's, command delay the TEG's. +CIVIC_TBA_A030 = EpsFirmwareCalibration( + CIVIC_TEG_A010.e4_per_output, CIVIC_TEG_A010.r5_key_bp, CIVIC_TEG_A010.r5_v, + CIVIC_TEG_A010.envelope_bp, CIVIC_TEG_A010.envelope_v, CIVIC_TEG_A010.r6_per_deg_s, + r6_angle_bp=CIVIC_TEG_A010.r6_angle_bp, r6_angle_gain=CIVIC_TEG_A010.r6_angle_gain, +) + +# CR-V 39990-TLA-A040 (RWD 2e6a8802...). Every variant record selects row 0 (TLBA2's row 1 is identical). Clamps at 1774 +# (E4 4096), and each key carries the Clarity's R5 at key x 3840/4096; P on the Clarity's knots at the same scale. R6 +# from its Table A (centre divisor 16783: -125 at centre; the owner's telemetry fit, an angle average, read -138.4). +# Load: the owner's telemetry route 82bb. Trims: none, as the owner drives it. Command delay: the default 0.12 s (the +# owner's PID drive ran 0.08 s late), re-check on this controller. +CRV_TLA_A040 = EpsFirmwareCalibration( + 4096.0, (0, 219, 443, 662, 887, 1108, 1330, 1552, 1663), (0, 3699, 7497, 11200, 15001, 18747, 22502, 26252, 28127), + (0, 50, 100, 150, 200, 250, 300, 350, 400), (1774,) * 9, CRV_TLA_R6[0], + r6_angle_bp=CRV_TLA_R6[1], r6_angle_gain=CRV_TLA_R6[2], + kp_key_bp=(0, 237, 473, 709, 946, 1182, 1419, 1655, 1892), key_clamp=1774, +) +CRV_TLA_EPS_LOAD = (-7.29446, -0.143159, -4.60337, -297.83, -19.899, -3.58048) +CRV_P_SCALE = (1.0, 1.0, 1.0) +CRV_I_SCALE = (1.0, 1.0, 1.0) + +# CR-V 39990-TLA-A220 (RWD 2c992997...): rows 0-5 Clarity-equivalent per key at 4096 E4 per unit, so the untraced variant +# row does not matter; its own envelope (1330 from 150 km/h). Table A byte-identical to the A040's, so the A040's R6; +# load and trims the A040 owner's (the A220 owner's drives ran an earlier build: refit after the reflash). +CRV_TLA_A220 = EpsFirmwareCalibration( + 4096.0, (0, 161, 222, 322, 409, 534, 696, 998, 1663), (0, 2720, 3750, 5439, 6917, 9038, 11775, 16880, 28127), + (0, 50, 100, 150, 200, 250, 300, 350, 400), (1774, 1774, 1774, 1774, 1774, 1774, 1330, 1330, 1330), CRV_TLA_R6[0], + r6_angle_bp=CRV_TLA_R6[1], r6_angle_gain=CRV_TLA_R6[2], + kp_key_bp=(0, 237, 473, 709, 946, 1182, 1419, 1655, 1892), key_clamp=1774, +) + def command_key(e4: float) -> int: return int(math.trunc(math.trunc(e4 * 56756 / 32768) / 4)) def key_ceiling(v_ego: float, cal: EpsFirmwareCalibration = CLARITY_A020) -> float: - return min(float(np.interp(v_ego * 3.6 * 2.0, cal.envelope_bp, cal.envelope_v)), KEY_CLAMP) + return min(float(np.interp(v_ego * 3.6 * 2.0, cal.envelope_bp, cal.envelope_v)), cal.key_clamp) def r5_from_output(output: float, v_ego: float, cal: EpsFirmwareCalibration = CLARITY_A020) -> float: @@ -282,9 +373,9 @@ def firmware_kp(r5: float, cal: EpsFirmwareCalibration = CLARITY_A020) -> float: def firmware_r6(steering_rate_deg_s: float, angle_deg: float, cal: EpsFirmwareCalibration = CLARITY_A020) -> float: - """The firmware's rate feedback for a published steering rate at a published angle.""" + """The firmware's rate feedback for a published steering rate at a published angle, clamped where it clamps.""" gain = 1.0 if cal.r6_angle_bp is None else float(np.interp(abs(angle_deg), cal.r6_angle_bp, cal.r6_angle_gain)) - return cal.r6_per_deg_s * gain * steering_rate_deg_s + return max(min(cal.r6_per_deg_s * gain * steering_rate_deg_s, R6_CLAMP), -R6_CLAMP) def firmware_output(r5: float, steering_rate_deg_s: float, angle_deg: float = 0.0, diff --git a/openpilot/nrdr/features/lateral/yaw_control_timing.py b/openpilot/nrdr/features/lateral/yaw_control_timing.py index de2ae7df818..71ce6e20a54 100644 --- a/openpilot/nrdr/features/lateral/yaw_control_timing.py +++ b/openpilot/nrdr/features/lateral/yaw_control_timing.py @@ -37,6 +37,15 @@ # The Insight takes it too: its source command delay is the C020's 0.15 s (JamesL787 ns-bosch-radar-testing, the # calibration it drove on). CIVIC_COMMAND_DELAY_CALIBRATIONS = ("civic_bosch_c020", "civic_teg_a010", "insight_txm_a040") +# Calibrations whose town command delay is fixed (their source's measured or inferred value + 0.025 s port +# compensation), so persisted Clarity command-delay settings never retune them. +FIXED_COMMAND_DELAY_LOW = dict.fromkeys(CIVIC_COMMAND_DELAY_CALIBRATIONS, CIVIC_COMMAND_DELAY_LOW) +# Corrected-PTM starting calibrations: the C120, TGG-A120 and A030 take the C020's / TEG's 0.15 s; the CR-Vs the +# default 0.12 s (their owner's PID drive ran late, not early). Each + 0.025 s port compensation, re-checked with +# plan timing after a drive. +FIXED_COMMAND_DELAY_LOW.update({"civic_tba_c120": CIVIC_COMMAND_DELAY_LOW, "civic_tgg_a120": CIVIC_COMMAND_DELAY_LOW, + "civic_tba_a030": CIVIC_COMMAND_DELAY_LOW, "crv_tla_a040": DEFAULT_COMMAND_DELAY_LOW, + "crv_tla_a220": DEFAULT_COMMAND_DELAY_LOW}) MAX_COMMAND_DELAY = 0.30 # s @@ -56,9 +65,9 @@ def _command_delay_setting(settings, key, default: float) -> float: def command_delay(settings, speed: float, *, calibration: str = "clarity_a020") -> float: - if calibration in CIVIC_COMMAND_DELAY_CALIBRATIONS: - # Do not let persisted Clarity command-delay settings silently retune Civic. - return float(np.interp(speed, COMMAND_DELAY_BP, (CIVIC_COMMAND_DELAY_LOW, DEFAULT_COMMAND_DELAY_HIGH))) + if calibration in FIXED_COMMAND_DELAY_LOW: + # Do not let persisted Clarity command-delay settings silently retune another car. + return float(np.interp(speed, COMMAND_DELAY_BP, (FIXED_COMMAND_DELAY_LOW[calibration], DEFAULT_COMMAND_DELAY_HIGH))) low = _command_delay_setting(settings, NrdrParamKey.NRDR_YAW_COMMAND_DELAY_LOW, DEFAULT_COMMAND_DELAY_LOW) high = _command_delay_setting(settings, NrdrParamKey.NRDR_YAW_COMMAND_DELAY_HIGH, DEFAULT_COMMAND_DELAY_HIGH) return float(np.interp(speed, COMMAND_DELAY_BP, (low, high))) diff --git a/openpilot/nrdr/tests/test_civic_firmware_profiles.py b/openpilot/nrdr/tests/test_civic_firmware_profiles.py index 8e12d2e5fe0..877e8ce1b88 100644 --- a/openpilot/nrdr/tests/test_civic_firmware_profiles.py +++ b/openpilot/nrdr/tests/test_civic_firmware_profiles.py @@ -8,8 +8,8 @@ from opendbc.sunnypilot.car.honda.values_ext import HondaFlagsSP from openpilot.nrdr.features.lateral.controller_selection import ( - CIVIC_PROFILE, CLARITY_PROFILE, INSIGHT_PROFILE, TEG_PROFILE, firmware_controller_profile, - firmware_controller_selected, + A030_PROFILE, C120_PROFILE, CIVIC_PROFILE, CLARITY_PROFILE, CRV_A040_PROFILE, CRV_A220_PROFILE, INSIGHT_PROFILE, + TEG_PROFILE, TGG_PROFILE, firmware_controller_profile, firmware_controller_selected, ) from openpilot.nrdr.features.lateral import vfn_eps_core as core from openpilot.nrdr.features.lateral.yaw_control_timing import command_delay, prediction_delay_schedule @@ -17,6 +17,9 @@ from openpilot.nrdr.ui.sunnylink_schema import apply_sunnylink_metadata +STARTING_PROFILES = (C120_PROFILE, TGG_PROFILE, A030_PROFILE, CRV_A040_PROFILE, CRV_A220_PROFILE) + + def car(fingerprint, firmware, *, modified=True, tuning="pid", brand="honda"): return (NS(brand=brand, carFingerprint=fingerprint, lateralTuning=NS(which=lambda: tuning), carFw=[NS(ecu="eps", fwVersion=firmware)]), @@ -28,6 +31,11 @@ def car(fingerprint, firmware, *, modified=True, tuning="pid", brand="honda"): ("HONDA_CIVIC_BOSCH", b"39990-TBA,C020", CIVIC_PROFILE), ("HONDA_CIVIC", b"39990-TEG,A010\0", TEG_PROFILE), ("HONDA_INSIGHT", b"39990-TXM,A040\0", INSIGHT_PROFILE), + ("HONDA_CIVIC_BOSCH", b"39990-TBA,C120\0", C120_PROFILE), + ("HONDA_CIVIC_BOSCH", b"39990-TGG,A120\0", TGG_PROFILE), + ("HONDA_CIVIC", b"39990-TBA,A030\0", A030_PROFILE), + ("HONDA_CRV_5G", b"39990-TLA,A040\0", CRV_A040_PROFILE), + ("HONDA_CRV_5G", b"39990-TLA,A220\0", CRV_A220_PROFILE), ]) def test_exact_profiles_and_selected_controller(fingerprint, firmware, profile): cp, sp = car(fingerprint, firmware) @@ -35,15 +43,14 @@ def test_exact_profiles_and_selected_controller(fingerprint, firmware, profile): for value in (1, "1", b"1"): assert firmware_controller_selected({"NrdrLateralController": value}, cp, sp) assert not firmware_controller_selected({"NrdrLateralController": 0}, cp, sp) - assert profile.provisional == (profile is TEG_PROFILE) + assert profile.provisional == (profile in STARTING_PROFILES) @pytest.mark.parametrize("fingerprint,firmware", [ - ("HONDA_CIVIC", b"39990-TBA-A030"), ("HONDA_CIVIC", b"39990-TBA-C020"), - ("HONDA_CIVIC_BOSCH", b"39990-TEG-A010"), ("HONDA_CIVIC_BOSCH", b"39990-TBA-C120"), - ("HONDA_CIVIC_BOSCH", b"39990-TGG-A120"), ("HONDA_CIVIC_2022", b"39990-TEG-A010"), - ("HONDA_CIVIC_BOSCH_DIESEL", b"39990-TBA-C020"), ("HONDA_CRV_5G", b"39990-TLA-A040"), - ("HONDA_CLARITY", b"39990-TRW-A010"), ("LEXUS_ES_TSS2", b"39990-TEG-A010"), + ("HONDA_CIVIC", b"39990-TBA-C020"), ("HONDA_CIVIC", b"39990-TBA-C120"), ("HONDA_CIVIC_BOSCH", b"39990-TBA-A030"), + ("HONDA_CIVIC_BOSCH", b"39990-TEG-A010"), ("HONDA_CIVIC_2022", b"39990-TEG-A010"), + ("HONDA_CIVIC_BOSCH_DIESEL", b"39990-TBA-C020"), ("HONDA_CRV_HYBRID", b"39990-TLA-A040"), + ("HONDA_CRV_5G", b"39990-TLA-A030"), ("HONDA_CLARITY", b"39990-TRW-A010"), ("LEXUS_ES_TSS2", b"39990-TEG-A010"), ]) def test_other_known_vgrs_do_not_inherit_the_c020_exception(fingerprint, firmware): cp, sp = car(fingerprint, firmware) @@ -74,18 +81,22 @@ def test_c020_nonlinear_inverse_satisfies_its_firmware_law(load, rate, guess): def test_c020_tables_load_trims_and_clarity_angle_feedback_are_separate(): cal = core.CIVIC_BOSCH_C020 - assert cal.e4_per_output == 4096. + assert cal.e4_per_output == 3840. # its image clamps at key 1663 = E4 3840 assert core.CLARITY_A020.e4_per_output == 3840. assert cal.r5_key_bp == (0, 115, 254, 449, 654, 862, 1111, 1549, 1774) assert core.CIVIC_EPS_LOAD == (-5.574, -.1831, -4.540, -326.5, -83.6, -3.185) assert core.CIVIC_P_SCALE == (1.15, 1.25, 1.15) assert core.CIVIC_I_SCALE == (.75, .95, 1.) - assert core.firmware_r6(10., 0., cal) == core.firmware_r6(10., 200., cal) == -1730. + # R6 computed from its Table A (test_r6_from_firmware.py): -153.74 per published deg/s at centre, growing with angle + assert core.firmware_r6(10., 0., cal) == pytest.approx(-1537.4) + assert core.firmware_r6(10., 200., cal) == pytest.approx(-1537.4 * 1.2059, rel=1e-3) + assert core.firmware_r6(-10., 80., cal) == pytest.approx(1537.4 * 1.1159, rel=1e-3) assert core.firmware_r6(10., 200.) != core.firmware_r6(10., 0.) - # The nonlinear P pieces reproduce the source's interpolation over key. + # The nonlinear P pieces reproduce the source's interpolation over key, on the image's own P axis. + assert cal.kp_key_bp == (0, 223, 441, 665, 883, 1108, 1330, 1552, 1774) for target in np.linspace(0., 35000., 350): key = np.interp(target, cal.r5_v, cal.r5_key_bp) - assert core.firmware_kp(target, cal) == pytest.approx(np.interp(key, core.KP_KEY_BP, core.KP_V)) + assert core.firmware_kp(target, cal) == pytest.approx(np.interp(key, cal.kp_key_bp, core.KP_V)) @pytest.mark.parametrize("speed", [1., 3., 5., 12., 20., 30., 40.]) @@ -149,11 +160,12 @@ def test_tegs_can_and_safety_limits_are_not_replaced_with_bosch_limits(): and any(isinstance(target, ast.Name) and target.id == "_EXTENDED_TORQUE_LIMITS" for target in node.targets)] limits = {key.attr: value.value for key, value in zip(assignment.value.keys, assignment.value.values, strict=True)} assert limits["HONDA_CIVIC"] == 3840 - assert limits["HONDA_CIVIC_BOSCH"] == 4096 + assert limits["HONDA_CIVIC_BOSCH"] == 3840 def test_calibrations_default_to_the_shared_p_axis_and_honour_their_own(): - assert core.CIVIC_BOSCH_C020.kp_key_bp == core.CLARITY_A020.kp_key_bp == tuple(core.KP_KEY_BP) + assert core.CLARITY_A020.kp_key_bp == tuple(core.KP_KEY_BP) + assert core.CIVIC_BOSCH_C020.kp_key_bp != tuple(core.KP_KEY_BP) # the C020 image's own P axis c020 = core.CIVIC_BOSCH_C020 own = core.EpsFirmwareCalibration(c020.e4_per_output, c020.r5_key_bp, c020.r5_v, c020.envelope_bp, c020.envelope_v, c020.r6_per_deg_s, kp_key_bp=(0, 111, 222, 333, 499, 665, 887, 1108, 1774)) @@ -172,14 +184,17 @@ def test_calibrations_default_to_the_shared_p_axis_and_honour_their_own(): def test_teg_runs_the_corrected_ptm_builds_row_3_at_its_own_can_scale(): cal = core.CIVIC_TEG_A010 - assert TEG_PROFILE.calibration == "civic_teg_a010" and TEG_PROFILE.provisional + assert TEG_PROFILE.calibration == "civic_teg_a010" and not TEG_PROFILE.provisional assert cal.e4_per_output == 3840. # = the HONDA_CIVIC transport limit, see test_tegs_can_and_safety_limits... assert cal.r5_key_bp == TEG_ROW_AXES[3] and cal.r5_v == (0, 4000, 6000, 8937, 11838, 16000, 20000, 28000, 32000) assert cal.kp_key_bp == tuple(core.KP_KEY_BP) # the corrected build moved rows 0-5 P onto the Clarity's knots - assert cal.envelope_v == core.CIVIC_BOSCH_C020.envelope_v and cal.r6_per_deg_s == -152.5 - # R6 per published deg/s by |angle| band, measured on routes 133/134/137, within 1.5% - for angle, measured in ((7.5, -151.4), (30., -152.2), (67.5, -165.7), (120., -170.4), (200., -176.4)): - assert core.firmware_r6(1.0, angle, cal) == pytest.approx(measured, rel=.015) + assert cal.envelope_v == core.CIVIC_BOSCH_C020.envelope_v and cal.r6_per_deg_s == core.CIVIC_C120_R6[0] == -156.16 + # R6 by |angle| band against the telemetry R6 word, 0.2 s window means on routes 133/134/137: the computed shape + # tracks it within 0.5% up to 120 deg, 3.3% above (window means read low: a 0.06 s derivative gives -156.2 at + # centre). Past ~185 deg/s R6 rails, so the 200 deg band, fitted with railed frames, is not checked. + ratios = [core.firmware_r6(1.0, angle, cal) / measured + for angle, measured in ((7.5, -151.4), (30., -152.2), (67.5, -165.7), (120., -170.4))] + assert max(ratios) - min(ratios) < .01 and 1.02 < min(ratios) < 1.045 assert core.CIVIC_TEG_EPS_LOAD == (-7.833, -.169, -8.29, -311.2, 59.18, -4.457) # Why row 3: the owner's telemetry R5 by command key (route 0000012f medians, earlier build) sits on that # build's row 3 within 3 %. @@ -217,31 +232,35 @@ def _extended_torque_limits(): import ast root = Path(__file__).resolve().parents[3] tree = ast.parse((root / "opendbc_repo/opendbc/sunnypilot/car/honda/interface_ext.py").read_text()) - assignment, = [node for node in tree.body if isinstance(node, ast.Assign) - and any(isinstance(target, ast.Name) and target.id == "_EXTENDED_TORQUE_LIMITS" for target in node.targets)] - return {key.attr: value.value for key, value in zip(assignment.value.keys, assignment.value.values, strict=True)} + limits = {} # platform limits by name, and per modified EPS image by (platform, part number) + for node in tree.body: + if isinstance(node, ast.Assign) and any(isinstance(target, ast.Name) and target.id in ("_EXTENDED_TORQUE_LIMITS", "_EPS_TORQUE_LIMITS") + for target in node.targets): + for key, value in zip(node.value.keys, node.value.values, strict=True): + limits[(key.elts[0].attr, key.elts[1].value) if isinstance(key, ast.Tuple) else key.attr] = value.value + return limits def test_insight_runs_the_corrected_ptm_at_this_trees_can_scale(): cal = core.INSIGHT_TXM_A040 assert INSIGHT_PROFILE.calibration == "insight_txm_a040" and not INSIGHT_PROFILE.provisional assert prediction_delay_schedule(INSIGHT_PROFILE) == prediction_delay_schedule(CLARITY_PROFILE) - assert cal.e4_per_output == _extended_torque_limits()["HONDA_INSIGHT"] == 4096 + # the Insight's EPS clamps at key 1663 = E4 3840 (stock and PTM alike), so the platform is sent 3840 + assert cal.e4_per_output == _extended_torque_limits()["HONDA_INSIGHT"] == 3840 # rows 0-1 of the corrected image are the Clarity's own command map and P table assert (cal.r5_key_bp, cal.r5_v, cal.kp_key_bp) == (core.CLARITY_A020.r5_key_bp, core.CLARITY_A020.r5_v, core.CLARITY_A020.kp_key_bp) assert cal.kp_pieces == core.CLARITY_A020.kp_pieces and cal.envelope_v == (1774,) * 9 assert cal.r6_per_deg_s == -131.15 and core.INSIGHT_TXM_EPS_LOAD == (-4.876, -.1376, -4.452, -397.2, 65.41, -4.29) - # R6 per published deg/s follows its own A table: 1.0 at centre, ~1.22 past 120 deg + # R6 per published deg/s follows its own A table: 1.0 at centre, ~1.19-1.20 past 120 deg assert core.firmware_r6(1.0, 0.0, cal) == pytest.approx(-131.15) - assert core.firmware_r6(1.0, 150.0, cal) == pytest.approx(-131.15 * 1.2156) + assert core.firmware_r6(1.0, 150.0, cal) == pytest.approx(-131.15 * 1.1927, rel=1e-3) assert command_delay(None, 5.0, calibration="insight_txm_a040") == pytest.approx(.175) @pytest.mark.parametrize("u", np.linspace(.02, .93, 40)) def test_insight_asks_the_clarity_target_for_the_same_e4(u): - # the same 0xE4 value makes the same firmware target on both images; this tree just sends the Insight 4096 per unit - e4 = u * 3840. - assert core.r5_from_output(e4 / 4096., 20., core.INSIGHT_TXM_A040) == pytest.approx(core.r5_from_output(u, 20., core.CLARITY_A020)) + # the corrected image is the Clarity's command map at the Clarity's 3840 E4 per unit: same output, same target + assert core.r5_from_output(u, 20., core.INSIGHT_TXM_A040) == pytest.approx(core.r5_from_output(u, 20., core.CLARITY_A020)) target = core.r5_from_output(u, 20., core.CLARITY_A020) - assert core.output_from_r5(target, core.INSIGHT_TXM_A040) == pytest.approx(core.output_from_r5(target, core.CLARITY_A020) * 3840 / 4096) + assert core.output_from_r5(target, core.INSIGHT_TXM_A040) == pytest.approx(core.output_from_r5(target, core.CLARITY_A020)) diff --git a/openpilot/nrdr/tests/test_controller_ui_labels.py b/openpilot/nrdr/tests/test_controller_ui_labels.py index daab124b306..e44dfdba3cb 100644 --- a/openpilot/nrdr/tests/test_controller_ui_labels.py +++ b/openpilot/nrdr/tests/test_controller_ui_labels.py @@ -133,8 +133,10 @@ def test_controller_copy_requires_the_corrected_firmware_not_the_old_placeholder self.assertIn("same version strings", selector["details"]) source = (ROOT / "openpilot/nrdr/ui/settings/pidf_ground.py").read_text(encoding="utf-8") self.assertNotIn("uses the Bosch C020 tune", source) - self.assertIn("04147917", source) - self.assertIn("7ba37432", source) + # The device shows each profile's required corrected build, which the profile carries. + self.assertIn("profile.required_rwd", source) + from openpilot.nrdr.features.lateral.controller_selection import INSIGHT_PROFILE, TEG_PROFILE + self.assertEqual((TEG_PROFILE.required_rwd, INSIGHT_PROFILE.required_rwd), ("04147917", "7ba37432")) if __name__ == "__main__": diff --git a/openpilot/nrdr/tests/test_r6_from_firmware.py b/openpilot/nrdr/tests/test_r6_from_firmware.py new file mode 100644 index 00000000000..797fd2b6a9c --- /dev/null +++ b/openpilot/nrdr/tests/test_r6_from_firmware.py @@ -0,0 +1,45 @@ +"""R6, the EPS firmware's rate feedback, computed from each image's own Table A and clamped where the firmware clamps it.""" +import pytest + +from openpilot.nrdr.features.lateral import vfn_eps_core as core +from openpilot.nrdr.features.lateral.honda_vgr import HONDA_VGR_PROFILES + +# Centre values from JamesL787 ns-bosch-radar-testing computed_r6 (the same Table A, -122 * A[0] / 16384) +CENTRE = {"Clarity TRW-A020": -122.0, "Civic C020/TGG-A120": -153.74, "Civic C120/A030/TEG": -156.16, + "CR-V TLA-A040": -124.97, "Insight TXM-A040": -131.15} +# Every calibration and the Table A its image carries +TABLE_A = { + "CLARITY_A020": "Clarity TRW-A020", "CIVIC_BOSCH_C020": "Civic C020/TGG-A120", "CIVIC_TGG_A120": "Civic C020/TGG-A120", + "CIVIC_TEG_A010": "Civic C120/A030/TEG", "CIVIC_TBA_A030": "Civic C120/A030/TEG", "CIVIC_TBA_C120": "Civic C120/A030/TEG", + "CRV_TLA_A040": "CR-V TLA-A040", "CRV_TLA_A220": "CR-V TLA-A040", "INSIGHT_TXM_A040": "Insight TXM-A040", +} +PROFILE = {p.name: p for p in HONDA_VGR_PROFILES} + + +@pytest.mark.parametrize("name", CENTRE) +def test_centre_is_the_clarity_constant_through_the_images_centre_divisor(name): + centre, _, gain = core.computed_r6(PROFILE[name]) + assert centre == pytest.approx(CENTRE[name], abs=0.01) and gain[0] == 1.0 + + +@pytest.mark.parametrize("cal_name,table", TABLE_A.items()) +def test_every_calibration_takes_its_table_as_computed_r6(cal_name, table): + cal = getattr(core, cal_name) + centre, bp, gain = core.computed_r6(PROFILE[table]) + assert (cal.r6_per_deg_s, tuple(cal.r6_angle_bp), tuple(cal.r6_angle_gain)) == (centre, bp, gain) + + +@pytest.mark.parametrize("cal_name", TABLE_A) +def test_r6_rails_at_the_firmware_clamp(cal_name): + cal = getattr(core, cal_name) + for angle in (0.0, 90.0, 300.0): + assert core.firmware_r6(1000.0, angle, cal) == -core.R6_CLAMP + assert core.firmware_r6(-1000.0, angle, cal) == core.R6_CLAMP + assert core.firmware_r6(20.0, angle, cal) == pytest.approx(20.0 * core.firmware_r6(1.0, angle, cal)) + + +def test_the_clarity_rails_later_than_the_civics(): + # the rail sits at ~260 deg/s on the Clarity and ~185-210 deg/s on the Civics near centre + def rail(cal): + return core.R6_CLAMP / abs(cal.r6_per_deg_s) + assert rail(core.CLARITY_A020) > 260 and 200 < rail(core.CIVIC_BOSCH_C020) < 215 diff --git a/openpilot/nrdr/tests/test_starting_calibrations.py b/openpilot/nrdr/tests/test_starting_calibrations.py new file mode 100644 index 00000000000..543ce93fb1b --- /dev/null +++ b/openpilot/nrdr/tests/test_starting_calibrations.py @@ -0,0 +1,84 @@ +"""Corrected-PTM (revision b) starting calibrations: the C120, TGG-A120, A030 and both CR-V images.""" +import pytest + +from openpilot.nrdr.features.lateral import vfn_eps_core as core +from openpilot.nrdr.features.lateral.controller_selection import ( + A030_PROFILE, C120_PROFILE, CRV_A040_PROFILE, CRV_A220_PROFILE, TGG_PROFILE, +) +from openpilot.nrdr.features.lateral.yaw_control_timing import command_delay + +STARTING_PROFILES = (C120_PROFILE, TGG_PROFILE, A030_PROFILE, CRV_A040_PROFILE, CRV_A220_PROFILE) +# Corrected-PTM starting calibrations. Golden R5 for the same 0xE4 at 20 m/s, from JamesL787 ns-bosch-radar-testing +# honda_eps_firmware_ff (r5_from_output(E4 / its e4_per_output)): the firmware target must not depend on the tree. +STARTING = { + "civic_tba_c120": (core.CIVIC_TBA_C120, "9261b8db", 3840., 1663), + "civic_tgg_a120": (core.CIVIC_TGG_A120, "6640806d", 3840., 1663), + "civic_tba_a030": (core.CIVIC_TBA_A030, "55c37266", 3840., 1663), + "crv_tla_a040": (core.CRV_TLA_A040, "2e6a8802", 4096., 1774), + "crv_tla_a220": (core.CRV_TLA_A220, "2c992997", 4096., 1774), +} +STARTING_E4 = (300., 1000., 2000., 3000., 3800., 4096.) +GOLDEN_C120 = (-2324.49, -7817.21, -15622.44, -23441.43, -29675.71, -30000.03) +GOLDEN_TGG = (-2179.24, -7327.8, -14646.78, -21975.97, -27823.33, -28127.33) +GOLDEN_A030 = (-2324.32, -7812.96, -15621.64, -23441.44, -29675.68, -30000.0) +GOLDEN_A040 = (-2178.86, -7327.45, -14646.24, -21977.65, -27822.95, -28127.0) +GOLDEN_A220 = (-2179.38, -7324.23, -14648.68, -21970.75, -27822.57, -28127.0) +STARTING_GOLDEN_R5 = { + "civic_tba_c120": GOLDEN_C120, + "civic_tgg_a120": GOLDEN_TGG, + "civic_tba_a030": GOLDEN_A030, + "crv_tla_a040": GOLDEN_A040, + "crv_tla_a220": GOLDEN_A220, +} + + +@pytest.mark.parametrize("name", STARTING) +def test_starting_calibrations_match_the_source_tree_and_their_build(name): + cal, rwd, e4, clamp = STARTING[name] + profile = next(p for p in STARTING_PROFILES if p.calibration == name) + assert profile.required_rwd == rwd and profile.provisional and not profile.prediction_schedule + assert cal.e4_per_output == e4 and cal.key_clamp == clamp + for e4_value, expected in zip(STARTING_E4, STARTING_GOLDEN_R5[name], strict=True): + assert core.r5_from_output(e4_value / cal.e4_per_output, 20., cal) == pytest.approx(expected, abs=0.5) + + +@pytest.mark.parametrize("name", STARTING) +@pytest.mark.parametrize("load,rate", [(500., 0.), (-1500., 30.), (4000., -80.), (-6000., 0.)]) +def test_starting_calibration_inverse_satisfies_its_firmware_law(name, load, rate): + cal = STARTING[name][0] + target = core.r5_for_motion(load, rate, cal=cal) + assert core.firmware_output(target, rate, cal=cal) == pytest.approx(load, abs=1e-6) + + +def test_starting_calibration_vehicle_side_and_command_delay(): + from openpilot.nrdr.features.lateral.latcontrol_fw import _OWN_CALIBRATIONS + assert _OWN_CALIBRATIONS["crv_tla_a040"][1:] == (core.CRV_TLA_EPS_LOAD, (1., 1., 1.), (1., 1., 1.)) + assert _OWN_CALIBRATIONS["crv_tla_a220"][1:] == _OWN_CALIBRATIONS["crv_tla_a040"][1:] + assert _OWN_CALIBRATIONS["civic_tba_a030"][1] == core.CIVIC_TEG_EPS_LOAD + assert _OWN_CALIBRATIONS["civic_tba_c120"][1] == _OWN_CALIBRATIONS["civic_tgg_a120"][1] == core.CIVIC_EPS_LOAD + for name in ("civic_tba_c120", "civic_tgg_a120", "civic_tba_a030"): + assert command_delay({"NrdrYawCommandDelayLow": 0.3}, 5.0, calibration=name) == pytest.approx(.175) + for name in ("crv_tla_a040", "crv_tla_a220"): + assert command_delay({"NrdrYawCommandDelayLow": 0.3}, 5.0, calibration=name) == pytest.approx(.145) + # The CR-V's key clamp is 1774 (the C120's 1663), though its row ends at key 1663, so R5 is flat from there to the clamp. + assert core.key_ceiling(20., core.CRV_TLA_A040) == 1774 and core.key_ceiling(20., core.CIVIC_TBA_C120) == 1663 + assert core.CIVIC_TBA_A030.r5_v == core.CIVIC_TEG_A010.r5_v and core.CIVIC_TBA_A030.r6_angle_gain == core.CIVIC_TEG_A010.r6_angle_gain + + +def test_every_civic_and_the_insight_are_sent_their_images_3840(): + # opendbc sends 3840 per unit to every platform whose EPS clamps at key 1663 (the CR-V's clamps at 1774 = 4096) + import ast + from pathlib import Path + path = Path(__file__).resolve().parents[3] / "opendbc_repo/opendbc/sunnypilot/car/honda/interface_ext.py" + tree = ast.parse(path.read_text()) + assignment, = [node for node in tree.body if isinstance(node, ast.Assign) + and any(isinstance(t, ast.Name) and t.id == "_EXTENDED_TORQUE_LIMITS" for t in node.targets)] + limits = {key.attr: value.value for key, value in zip(assignment.value.keys, assignment.value.values, strict=True)} + for cal in (core.CIVIC_BOSCH_C020, core.CIVIC_TBA_C120, core.CIVIC_TGG_A120): + assert cal.e4_per_output == limits["HONDA_CIVIC_BOSCH"] == 3840 + assert core.INSIGHT_TXM_A040.e4_per_output == limits["HONDA_INSIGHT"] == 3840 + assert core.CRV_TLA_A040.e4_per_output == limits["HONDA_CRV_5G"] == 4096 + # at 3840 the full output reaches the clamp: u = 1 decodes to key 1662, the last key under it (1773 at 4096, clamped + # from u ~0.94 on) + assert core.command_key(-1.0 * 3840) == -1662 and core.command_key(-1.0 * 4096) == -1773 + diff --git a/openpilot/nrdr/tests/test_vfn_controller_runtime.py b/openpilot/nrdr/tests/test_vfn_controller_runtime.py index c8f6499b1a9..683a4ca0de6 100644 --- a/openpilot/nrdr/tests/test_vfn_controller_runtime.py +++ b/openpilot/nrdr/tests/test_vfn_controller_runtime.py @@ -67,7 +67,7 @@ def test_civic_wrapper_uses_its_own_calibration_and_the_c020_trims_without_mutat assert control.core.ff.load_coefficients == (CIVIC_TEG_EPS_LOAD if teg else CIVIC_EPS_LOAD) assert control.core.p_scale == CIVIC_P_SCALE and control.core.i_scale == CIVIC_I_SCALE assert control.delay_schedule is None - assert control.firmware_profile.provisional == (changes["fingerprint"] == "HONDA_CIVIC") + assert not control.firmware_profile.provisional # User PIF/yaw settings cannot leak into either firmware profile. control.set_live_tuning_snapshot(ParamSnapshot(1, {"NrdrDeviceYawCorrection": True, "LatFScaleHighway": 500, "NrdrLatRateDampingHighway": 300})) diff --git a/openpilot/nrdr/ui/settings/pidf_ground.py b/openpilot/nrdr/ui/settings/pidf_ground.py index 49bcdfe7c2a..42511ccd7be 100644 --- a/openpilot/nrdr/ui/settings/pidf_ground.py +++ b/openpilot/nrdr/ui/settings/pidf_ground.py @@ -105,11 +105,13 @@ def _update_state(self): super()._update_state() self._controller.action_item.set_enabled(ui_state.is_offroad()) profile = firmware_controller_profile(ui_state.CP, ui_state.CP_SP) - self._controller.set_description(tr("TEG-A010 uses its own provisional calibration for the corrected PTM firmware (04147917). " - + "The older firmware is not compatible; this calibration is not yet road-validated.") - if profile is not None and profile.provisional else - tr("Insight TXM-A040 requires the corrected PTM firmware (7ba37432), not the older released build.") - if profile is not None and profile.calibration == "insight_txm_a040" else "") + description = "" + if profile is not None and profile.required_rwd: + description = tr("Requires the corrected PTM firmware (RWD {}). Older builds report the same EPS version and are not " + + "compatible.").format(profile.required_rwd) + if profile.provisional: + description += " " + tr("This calibration is not yet road-validated on that build.") + self._controller.set_description(description) for key, item in self._tuning_items.items(): available = key != "NrdrDeviceYawCorrection" or supports_interpolated_torque_pif(ui_state.CP, ui_state.CP_SP) if key == "NrdrDeviceYawCorrection": diff --git a/openpilot/nrdr/ui/sunnylink/pages/steering.yaml b/openpilot/nrdr/ui/sunnylink/pages/steering.yaml index 0685793392d..d3cf617a055 100644 --- a/openpilot/nrdr/ui/sunnylink/pages/steering.yaml +++ b/openpilot/nrdr/ui/sunnylink/pages/steering.yaml @@ -24,7 +24,7 @@ sections: widget: multiple_button title: Controller Type description: Firmware Controller requires the matching modified EPS build. TEG-A010 uses its own provisional corrected-PTM calibration, not the Bosch placeholder. - details: TEG-A010 requires the corrected PTM firmware (RWD SHA256 starts 04147917); its calibration is not yet road-validated. Insight TXM-A040 requires the corrected PTM firmware (7ba37432). Older builds report the same version strings but are not compatible with these calibrations. Confirm the actual flashed build before selecting Firmware Controller. PIF and Device Yaw Correction remain separate. + details: TEG-A010 requires the corrected PTM firmware (RWD SHA256 starts 04147917); its calibration has been driven on that build. Insight TXM-A040 requires the corrected PTM firmware (7ba37432). Starting calibrations, not yet road-validated, cover the corrected builds of Civic Bosch TBA-C120 (9261b8db), Civic TGG-A120 (6640806d), Civic TBA-A030 (55c37266), CR-V TLA-A040 (2e6a8802) and CR-V TLA-A220 (2c992997). Older builds report the same version strings but are not compatible with these calibrations. Confirm the actual flashed build before selecting Firmware Controller. PIF and Device Yaw Correction remain separate. options: - value: 0 label: PIF Control diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 3e34abacfe0..abdc770e0bb 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -604,7 +604,7 @@ "widget": "multiple_button", "title": "Controller Type", "description": "Firmware Controller requires the matching modified EPS build. TEG-A010 uses its own provisional corrected-PTM calibration, not the Bosch placeholder.", - "details": "TEG-A010 requires the corrected PTM firmware (RWD SHA256 starts 04147917); its calibration is not yet road-validated. Insight TXM-A040 requires the corrected PTM firmware (7ba37432). Older builds report the same version strings but are not compatible with these calibrations. Confirm the actual flashed build before selecting Firmware Controller. PIF and Device Yaw Correction remain separate.", + "details": "TEG-A010 requires the corrected PTM firmware (RWD SHA256 starts 04147917); its calibration has been driven on that build. Insight TXM-A040 requires the corrected PTM firmware (7ba37432). Starting calibrations, not yet road-validated, cover the corrected builds of Civic Bosch TBA-C120 (9261b8db), Civic TGG-A120 (6640806d), Civic TBA-A030 (55c37266), CR-V TLA-A040 (2e6a8802) and CR-V TLA-A220 (2c992997). Older builds report the same version strings but are not compatible with these calibrations. Confirm the actual flashed build before selecting Firmware Controller. PIF and Device Yaw Correction remain separate.", "options": [ { "value": 0, diff --git a/openpilot/sunnypilot/sunnylink/tests/test_capabilities.py b/openpilot/sunnypilot/sunnylink/tests/test_capabilities.py index 406b3f0498a..65f27c5d019 100644 --- a/openpilot/sunnypilot/sunnylink/tests/test_capabilities.py +++ b/openpilot/sunnypilot/sunnylink/tests/test_capabilities.py @@ -159,10 +159,14 @@ def test_firmware_profiles_are_exact_and_carry_their_schedule(self, params): for fingerprint, firmware, available, scheduled, provisional in ( ("HONDA_CLARITY", b"39990-TRW-A020", True, True, False), ("HONDA_CIVIC_BOSCH", b"39990-TBA-C020", True, False, False), - ("HONDA_CIVIC", b"39990-TEG-A010", True, False, True), + ("HONDA_CIVIC", b"39990-TEG-A010", True, False, False), ("HONDA_INSIGHT", b"39990-TXM-A040", True, True, False), - ("HONDA_CIVIC", b"39990-TBA-A030", False, False, False), - ("HONDA_CIVIC_BOSCH", b"39990-TBA-C120", False, False, False), + ("HONDA_CIVIC", b"39990-TBA-A030", True, False, True), + ("HONDA_CIVIC_BOSCH", b"39990-TBA-C120", True, False, True), + ("HONDA_CIVIC_BOSCH", b"39990-TGG-A120", True, False, True), + ("HONDA_CRV_5G", b"39990-TLA-A040", True, False, True), + ("HONDA_CRV_5G", b"39990-TLA-A220", True, False, True), + ("HONDA_CIVIC", b"39990-TBA-C120", False, False, False), ): params.put("CarPlatformBundle", {"brand": "honda", "platform": fingerprint}, block=True) put_car_params(params, fingerprint, "honda", firmware) From 75f838844c796581a214a5c231923069ffea5a6e Mon Sep 17 00:00:00 2001 From: JamesL787 Date: Sun, 18 Oct 2026 13:20:27 -0400 Subject: [PATCH 2/3] lateral: modeld says where the steering comes from, and the Civics get their own measured delay schedules modelV2.action gains lateralSource (plan / actionHead / curvatureHead) and delayInput (actionT / lateralControlParams / none), set by both modeld runtimes from the output get_action_from_model steers with. On a scheduled profile a plan model's command delay is max(MIN_STABLE_DELAY - lateralActionTime, 0): the schedule tells the model the car's own lag and this undoes only the 0.3 s floor. Action heads keep the calibration's delay. The applied delay slews at 0.3 s/s, so a model change never steps it. The TEG-A010 and the C020 get prediction schedules, measured by speed band on the local routes (2026-10-18): TEG 0.20 s flat (its owner's 12e-13d: the car reaches a command 0.26-0.31 s after the frame while lagd told it 0.37-0.39), C020 0.28 / 0.26 s, provisional (route 380, an older controller). Their command delay holds 0.175 s above 15 m/s, where both ran as early as in town; the Insight keeps its fade. On top of the corrected-PTM calibrations (PR #24): the A030 takes the TEG's schedule and the C120 and TGG the C020's, with the same flat Civic delay; the CR-Vs keep live/manual delay and the fading delay until a CR-V drive is measured. Co-Authored-By: Claude Opus 5.5 --- openpilot/cereal/log.capnp | 16 +++ .../nrdr/docs/firmware_controller_profiles.md | 34 ++++-- .../features/lateral/controller_selection.py | 13 +-- .../nrdr/features/lateral/latcontrol_fw.py | 17 ++- .../nrdr/features/lateral/model_source.py | 61 +++++++++++ .../features/lateral/yaw_control_timing.py | 37 ++++++- .../tests/test_civic_firmware_profiles.py | 23 ++-- openpilot/nrdr/tests/test_cleanup_policy.py | 2 +- openpilot/nrdr/tests/test_model_source.py | 102 ++++++++++++++++++ .../nrdr/tests/test_model_timing_metadata.py | 3 + .../nrdr/tests/test_starting_calibrations.py | 20 +++- .../nrdr/tests/test_vfn_controller_runtime.py | 6 +- openpilot/selfdrive/modeld/modeld.py | 12 ++- openpilot/sunnypilot/modeld_v2/modeld.py | 4 +- .../sunnypilot/sunnylink/settings_ui.json | 6 +- .../settings_ui_src/pages/models.yaml | 6 +- 16 files changed, 317 insertions(+), 45 deletions(-) create mode 100644 openpilot/nrdr/features/lateral/model_source.py create mode 100644 openpilot/nrdr/tests/test_model_source.py diff --git a/openpilot/cereal/log.capnp b/openpilot/cereal/log.capnp index 1e611b47a05..45d4e73516a 100644 --- a/openpilot/cereal/log.capnp +++ b/openpilot/cereal/log.capnp @@ -1168,6 +1168,22 @@ struct ModelDataV2 { desiredAcceleration @1 :Float32; shouldStop @2 :Bool; lateralActionTime @3 :Float32; # Seconds from the source vision frame to the lateral action target; zero means unavailable. + lateralSource @4 :LateralSource; # where desiredCurvature came from; unknown in older logs + delayInput @5 :DelayInput; # which model input carries the told lateral delay; unknown in older logs + + enum LateralSource { + unknown @0; + plan @1; # read off the plan at lateralActionTime (floored at MIN_STABLE_DELAY) + actionHead @2; # the model's own action output + curvatureHead @3; # the model's desired_curvature output (non-tinygrad models) + } + + enum DelayInput { + unknown @0; + none @1; + actionT @2; + lateralControlParams @3; + } } deprecated :group { diff --git a/openpilot/nrdr/docs/firmware_controller_profiles.md b/openpilot/nrdr/docs/firmware_controller_profiles.md index e28239e00c7..fc71dccc07c 100644 --- a/openpilot/nrdr/docs/firmware_controller_profiles.md +++ b/openpilot/nrdr/docs/firmware_controller_profiles.md @@ -7,12 +7,12 @@ changing it requires offroad confirmation. PIF's Device Yaw Correction is separa | Detected vehicle / EPS family | Calibration | Prediction delay | | --- | --- | --- | | HONDA_CLARITY / 39990-TRW-A020, modified EPS | Existing Clarity A020 | Measured speed schedule | -| HONDA_CIVIC_BOSCH / 39990-TBA-C020, modified EPS | Civic C020 TargetMapD / Tracker4500 / Norm1650 / P117to265 / D737 / KFF45 | Normal live/manual path | -| HONDA_CIVIC / 39990-TEG-A010, modified EPS | TEG-A010 corrected PTM build, command row 3 (driven on that build) | Normal live/manual path | +| HONDA_CIVIC_BOSCH / 39990-TBA-C020, modified EPS | Civic C020 TargetMapD / Tracker4500 / Norm1650 / P117to265 / D737 / KFF45 | Its own speed schedule (provisional) | +| HONDA_CIVIC / 39990-TEG-A010, modified EPS | TEG-A010 corrected PTM build, command row 3 (driven on that build) | Its own measured schedule | | HONDA_INSIGHT / 39990-TXM-A040, modified EPS | Insight TXM-A040 corrected PTM build (the Clarity's own command map and P table, 3840 E4 per unit, measured load, R6 from its A table) | Clarity's measured speed schedule | -| HONDA_CIVIC_BOSCH / 39990-TBA-C120, modified EPS | C120 corrected PTM starting calibration: rows 0-4 the Clarity's R5 per key (3840 E4 per unit, its image's key clamp 1663), R6 from its A table, the C020's load and trims | Normal live/manual path | -| HONDA_CIVIC_BOSCH / 39990-TGG-A120, modified EPS | TGG-A120 corrected PTM starting calibration: row 0, built for 4096 E4 per unit but sent 3840 (its key clamp 1663), R6 from its A table (the C020's), the C020's load and trims | Normal live/manual path | -| HONDA_CIVIC / 39990-TBA-A030, modified EPS | A030 corrected PTM starting calibration: the TEG-A010's row 3 on every row, R6 from its A table (the TEG's), the TEG's measured load | Normal live/manual path | +| HONDA_CIVIC_BOSCH / 39990-TBA-C120, modified EPS | C120 corrected PTM starting calibration: rows 0-4 the Clarity's R5 per key (3840 E4 per unit, its image's key clamp 1663), R6 from its A table, the C020's load and trims | The C020's speed schedule | +| HONDA_CIVIC_BOSCH / 39990-TGG-A120, modified EPS | TGG-A120 corrected PTM starting calibration: row 0, built for 4096 E4 per unit but sent 3840 (its key clamp 1663), R6 from its A table (the C020's), the C020's load and trims | The C020's speed schedule | +| HONDA_CIVIC / 39990-TBA-A030, modified EPS | A030 corrected PTM starting calibration: the TEG-A010's row 3 on every row, R6 from its A table (the TEG's), the TEG's measured load | The TEG-A010's speed schedule | | HONDA_CRV_5G / 39990-TLA-A040, modified EPS | CR-V A040 corrected PTM starting calibration: row 0, key clamp 1774, R6 from its A table, the owner's measured load, untrimmed | Normal live/manual path | | HONDA_CRV_5G / 39990-TLA-A220, modified EPS | CR-V A220 corrected PTM starting calibration: rows 0-5 Clarity-equivalent, its own envelope, the A040's A table, R6, load and trims | Normal live/manual path | @@ -54,16 +54,28 @@ The temporary fixed 0.30-second prediction override is removed. Clarity uses 0.15 / 0.08 / 0.10 / 0.20 / 0.30 seconds at 3.5 / 7 / 12 / 20 / 30 m/s, interpolated every frame, with endpoints held. Both model runtimes and controlsd use the same profile. Saved live/manual settings are preserved but unavailable -while that schedule is selected. Civic has no measured prediction schedule in the -source and therefore keeps live/manual delay normally, subject to Suggested locks. +while that schedule is selected. The Civics have their own schedules (2026-10-18): +TEG-A010 0.20 seconds at every speed, measured on its owner's drives, and C020 0.28 / +0.28 / 0.28 / 0.26 / 0.26 seconds, provisional (from a drive on an older controller). +Each is the car's lag from the camera frame to its yaw-sensor curvature, less modeld's +fixed 0.075 seconds. lagd told the TEG 0.37-0.39 seconds, which ran it early. Model-specific smoothing remains separate, as in the source implementation. Command delay is a separate interpolated curvature history buffer, not a blocking sleep. It fades between 10 and 15 m/s. Clarity retains its saved endpoints, default -0.145 / 0.025 seconds. Civic (and the Insight) uses fixed 0.175 / 0.025 seconds (source 0.150 / 0.000 -plus the existing port's 0.025-second model-action compensation); saved Clarity -command-delay settings do not retune Civic. These are model-dependent calibrations, -not a guarantee for every driving model. +0.145 / 0.025 seconds. The Insight uses fixed 0.175 / 0.025 seconds and the Civics +0.175 seconds at every speed, since they ran as early on the highway as in town +(source 0.150 plus the existing port's 0.025-second model-action compensation); saved +Clarity command-delay settings do not retune them. These are model-dependent +calibrations, not a guarantee for every driving model. + +They are what an action-head model needs. modeld now records where the steering came +from (`modelV2.action.lateralSource`: `plan`, `actionHead` or `curvatureHead`) and how +the told delay reached the model (`delayInput`). On a plan model (Pop v2, TCP v3) the +plan is read no earlier than `MIN_STABLE_DELAY` (0.3 s); with the schedule telling it +the car's own lag, the command delay becomes `max(0.3 - lateralActionTime, 0)`, which +undoes that floor and nothing else. The delay slews at 0.3 s per second when the +source changes. ## Calibration and unchanged behavior diff --git a/openpilot/nrdr/features/lateral/controller_selection.py b/openpilot/nrdr/features/lateral/controller_selection.py index c24d2dbae42..a2f7046f1e2 100644 --- a/openpilot/nrdr/features/lateral/controller_selection.py +++ b/openpilot/nrdr/features/lateral/controller_selection.py @@ -26,23 +26,24 @@ class FirmwareControllerProfile: CLARITY_PROFILE = FirmwareControllerProfile("Clarity A020", "clarity_a020", prediction_schedule=True) -CIVIC_PROFILE = FirmwareControllerProfile("Civic Bosch C020", "civic_bosch_c020") +CIVIC_PROFILE = FirmwareControllerProfile("Civic Bosch C020", "civic_bosch_c020", prediction_schedule=True) TEG_PROFILE = FirmwareControllerProfile( - "Civic TEG-A010 corrected PTM, row 3", "civic_teg_a010", required_rwd="04147917", + "Civic TEG-A010 corrected PTM, row 3", "civic_teg_a010", prediction_schedule=True, required_rwd="04147917", ) INSIGHT_PROFILE = FirmwareControllerProfile( "Insight TXM-A040 corrected PTM", "insight_txm_a040", prediction_schedule=True, required_rwd="7ba37432", ) # Corrected-PTM (revision b) starting calibrations: firmware read from each image, the vehicle side carried from -# the nearest measured car until its owner's drives measure it. +# the nearest measured car until its owner's drives measure it. The Civics take that car's delay schedule too (the +# A030 the TEG's, the C120 and TGG the C020's); the CR-Vs keep live/manual delay until a CR-V drive is measured. C120_PROFILE = FirmwareControllerProfile( - "Civic Bosch TBA-C120 corrected PTM (starting calibration)", "civic_tba_c120", provisional=True, required_rwd="9261b8db", + "Civic Bosch TBA-C120 corrected PTM (starting calibration)", "civic_tba_c120", prediction_schedule=True, provisional=True, required_rwd="9261b8db", ) TGG_PROFILE = FirmwareControllerProfile( - "Civic TGG-A120 corrected PTM (starting calibration)", "civic_tgg_a120", provisional=True, required_rwd="6640806d", + "Civic TGG-A120 corrected PTM (starting calibration)", "civic_tgg_a120", prediction_schedule=True, provisional=True, required_rwd="6640806d", ) A030_PROFILE = FirmwareControllerProfile( - "Civic TBA-A030 corrected PTM (starting calibration)", "civic_tba_a030", provisional=True, required_rwd="55c37266", + "Civic TBA-A030 corrected PTM (starting calibration)", "civic_tba_a030", prediction_schedule=True, provisional=True, required_rwd="55c37266", ) CRV_A040_PROFILE = FirmwareControllerProfile( "CR-V TLA-A040 corrected PTM (starting calibration)", "crv_tla_a040", provisional=True, required_rwd="2e6a8802", diff --git a/openpilot/nrdr/features/lateral/latcontrol_fw.py b/openpilot/nrdr/features/lateral/latcontrol_fw.py index 42fba416f2b..41e3bb324d3 100644 --- a/openpilot/nrdr/features/lateral/latcontrol_fw.py +++ b/openpilot/nrdr/features/lateral/latcontrol_fw.py @@ -11,6 +11,10 @@ been driven on its corrected build. Insight TXM-A040 has its own calibration for its corrected PTM build (the Clarity's own map and P table, 3840 E4 per unit, measured load, R6 from its A table) with the C020's trims, the Civic command delay and the Clarity's model-delay schedule. + +On a plan model (modelV2.action.lateralSource) a scheduled profile's command delay is +max(MIN_STABLE_DELAY - lateralActionTime, 0) instead (nrdr.features.lateral.model_source): +the schedule tells the model the car's own lag, and that undoes only the floor. """ import numpy as np @@ -24,6 +28,7 @@ INSIGHT_TXM_A040, INSIGHT_TXM_EPS_LOAD, HondaEpsFirmwareFeedforward, HondaEpsLateralCore, ) +from openpilot.nrdr.features.lateral.model_source import PLAN, CommandDelaySlew, plan_floor_command_delay from openpilot.nrdr.features.lateral.yaw_control_timing import CommandDelay, command_delay, prediction_delay_schedule from openpilot.nrdr.features.lateral.latcontrol_pid import _eps_modified_steering_pressed from openpilot.nrdr.features.lateral.lane_change_tuning import optimized_lane_change_active @@ -68,6 +73,7 @@ def __init__(self, CP, CP_SP, CI, dt): self.firmware_profile.name, self.firmware_profile.calibration, self.firmware_profile.provisional, self.delay_schedule, self.firmware_profile.required_rwd or "-") self.cmd_delay = CommandDelay(dt) + self.cmd_delay_slew = CommandDelaySlew(dt) self.model_v2 = None self.prev_angle = 0.0 self.pressed_duration = 0.0 @@ -76,6 +82,15 @@ def __init__(self, CP, CP_SP, CI, dt): def update_model_v2(self, model_v2): self.model_v2 = model_v2 + def _command_delay(self, v_ego: float) -> float: + delay = command_delay(self.live_tuning_snapshot, v_ego, calibration=self.firmware_profile.calibration) + action = getattr(self.model_v2, "action", None) + if self.delay_schedule is not None and str(getattr(action, "lateralSource", "unknown")) == PLAN: + floor_delay = plan_floor_command_delay(getattr(action, "lateralActionTime", 0.0)) + if floor_delay is not None: + delay = floor_delay + return self.cmd_delay_slew.update(delay) + def reset(self): super().reset() self.core.reset() @@ -87,7 +102,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) - delay = command_delay(self.live_tuning_snapshot, CS.vEgo, calibration=self.firmware_profile.calibration) + delay = self._command_delay(CS.vEgo) desired_curvature = self.cmd_delay.update(desired_curvature, delay) desired = self.steer_ratio_selection.desired_angle_no_offset( VM, CS.steeringAngleDeg, CS.vEgo, params.roll, desired_curvature) diff --git a/openpilot/nrdr/features/lateral/model_source.py b/openpilot/nrdr/features/lateral/model_source.py new file mode 100644 index 00000000000..c15412a9b42 --- /dev/null +++ b/openpilot/nrdr/features/lateral/model_source.py @@ -0,0 +1,61 @@ +"""Where the model's steering comes from, and the command delay that follows from it. + +modeld attaches the source to modelV2.action next to lateralActionTime (set_lateral_action_timing), decided where +get_action_from_model decides it: + actionHead the model has an action output (e.g. Cinque Terre V3). It follows the told delay only in part: Cinque's + aim moves ~0.53 s per 1 s of action_t. + curvatureHead a non-tinygrad model's desired_curvature output. + plan everything else (e.g. Pop v2, The Cool Peoples Model v3): the curvature that reaches the plan's heading at + lateralActionTime, read no earlier than MIN_STABLE_DELAY (0.3 s). + +On a plan model the floor is the whole timing story. When the told delay is the car's measured lag (a profile with a +prediction schedule), the plan should be read at lateralActionTime; the floor reads it at 0.3 s instead, so the curvature +it hands over is meant for MIN_STABLE_DELAY - lateralActionTime later than the car will reach it. Holding it back that +long (plan_floor_command_delay) lands it on time, and above the floor it needs no delay at all. +""" +from openpilot.selfdrive.controls.lib.drive_helpers import MIN_STABLE_DELAY + +ACTION_HEAD = "actionHead" +CURVATURE_HEAD = "curvatureHead" +PLAN = "plan" +UNKNOWN = "unknown" + + +def lateral_source(model_output, mlsim: bool) -> str: + """The source, exactly as get_action_from_model / get_curvature_from_output choose it.""" + if "action" in model_output: + return ACTION_HEAD + if not mlsim and model_output.get("desired_curvature") is not None: + return CURVATURE_HEAD + return PLAN + + +def delay_input(numpy_inputs) -> str: + if "action_t" in numpy_inputs: + return "actionT" + if "lateral_control_params" in numpy_inputs: + return "lateralControlParams" + return "none" + + +def plan_floor_command_delay(lateral_action_time: float) -> float | None: + """The command delay that cancels the MIN_STABLE_DELAY floor; None without a published action time.""" + if not lateral_action_time > 0.0: + return None + return max(MIN_STABLE_DELAY - float(lateral_action_time), 0.0) + + +class CommandDelaySlew: + """Moves the applied command delay at most rate seconds per second, so a model change (or the speed fade) never steps + the executed curvature.""" + + def __init__(self, dt: float, rate: float = 0.3): + self.step = dt * rate + self.value: float | None = None + + def update(self, target: float) -> float: + if self.value is None: + self.value = float(target) + else: + self.value += max(min(float(target) - self.value, self.step), -self.step) + return self.value diff --git a/openpilot/nrdr/features/lateral/yaw_control_timing.py b/openpilot/nrdr/features/lateral/yaw_control_timing.py index 71ce6e20a54..c6fcaf71951 100644 --- a/openpilot/nrdr/features/lateral/yaw_control_timing.py +++ b/openpilot/nrdr/features/lateral/yaw_control_timing.py @@ -3,9 +3,11 @@ Ported by JamesL787 in nrdr/openpilot PR #18 (ab868ea15561dd65a4bc52199ec11abbbd4bf789), from vfn-yaw-trim a434a79b19 (096aedb9 delay refit, 0fb4a6dc command delay). Clarity's measured table replaces the temporary fixed 0.30-second override. -Civic has no measured prediction table and retains live/manual delay. Its command -delay comes from JamesL787 59eb99e318 (0.15 -> 0 seconds), with the same +25 ms -SunnyPilot model-action interpolation compensation as the existing Clarity port. +The TEG-A010 and the C020 have their own prediction tables (2026-10-18), measured +by speed band like the Clarity's; before them lagd told the Civics 0.37-0.39 s and +they ran early. Civic command delay comes from JamesL787 59eb99e318 (0.15 seconds), +with the same +25 ms SunnyPilot model-action interpolation compensation as the +existing Clarity port, held flat with speed. """ from collections import deque import math @@ -21,6 +23,22 @@ # stage lags. lagd only learns above 15 m/s, so it cannot find the low-speed end. DELAY_SCHEDULE_BP = (3.5, 7.0, 12.0, 20.0, 30.0) # m/s, centres of the measured bands DELAY_SCHEDULE_V = (0.15, 0.08, 0.10, 0.20, 0.30) # s +# The Civics' own, on the same speeds: each car's lag from the camera frame to its yaw-sensor curvature, less modeld's +# fixed 0.075 s (frame + half a model step), measured per speed band (JamesL787 ns-bosch-radar-testing, 2026-10-18). +# - TEG-A010: 0.26-0.31 s at every speed on its owner's routes 12e/12f/133/134/137/139/13d (Pop v2 on this +# controller, the command delay taken out), so 0.20 s flat. lagd told it 0.37-0.39 s: with the plan read 0.44 s +# out and the car there at ~0.28 s, it turned in early even with the 0.175 s command delay, and ~0.15 s early above +# 15 m/s, where that delay fades out. +# - C020 (provisional): 0.37-0.43 s on route 380 (NovaSpark, an older PID + firmware FF with a 0.09-0.10 s output filter +# and no command delay); with this controller's filter that is ~0.35-0.41 s in town and ~0.33 s on the highway. +TEG_A010_DELAY_SCHEDULE_V = (0.20, 0.20, 0.20, 0.20, 0.20) # s +CIVIC_C020_DELAY_SCHEDULE_V = (0.28, 0.28, 0.28, 0.26, 0.26) # s +# The A030 (the same car as the TEG) takes the TEG's; the C120 and TGG (the C020's chassis) the C020's. +_PREDICTION_SCHEDULE_V = { + "civic_teg_a010": TEG_A010_DELAY_SCHEDULE_V, "civic_tba_a030": TEG_A010_DELAY_SCHEDULE_V, + "civic_bosch_c020": CIVIC_C020_DELAY_SCHEDULE_V, "civic_tba_c120": CIVIC_C020_DELAY_SCHEDULE_V, + "civic_tgg_a120": CIVIC_C020_DELAY_SCHEDULE_V, +} # Command delay: the curvature controlsd hands over is executed this much later. This controller # reaches a command ~0.07 s after controlsd issues it, faster than the PID it replaced, so on the @@ -46,6 +64,11 @@ FIXED_COMMAND_DELAY_LOW.update({"civic_tba_c120": CIVIC_COMMAND_DELAY_LOW, "civic_tgg_a120": CIVIC_COMMAND_DELAY_LOW, "civic_tba_a030": CIVIC_COMMAND_DELAY_LOW, "crv_tla_a040": DEFAULT_COMMAND_DELAY_LOW, "crv_tla_a220": DEFAULT_COMMAND_DELAY_LOW}) +# The Civics hold it above 15 m/s too: they ran as early there as in town (TEG -0.13 to -0.17 s at 12-40 m/s with +# it faded out; C020 -0.16 s at 20-40 m/s on route 380), and the C120, TGG and A030 follow them. The Insight's +# highway timing did not need it; the CR-Vs have no timing measured. +CIVIC_FLAT_COMMAND_DELAY_CALIBRATIONS = ("civic_bosch_c020", "civic_teg_a010", "civic_tba_c120", "civic_tgg_a120", + "civic_tba_a030") MAX_COMMAND_DELAY = 0.30 # s @@ -54,7 +77,9 @@ def clarity_lateral_delay(speed: float) -> float: def prediction_delay_schedule(profile): - return (DELAY_SCHEDULE_BP, DELAY_SCHEDULE_V) if profile is not None and profile.prediction_schedule else None + if profile is None or not profile.prediction_schedule: + return None + return DELAY_SCHEDULE_BP, _PREDICTION_SCHEDULE_V.get(profile.calibration, DELAY_SCHEDULE_V) def _command_delay_setting(settings, key, default: float) -> float: @@ -67,7 +92,9 @@ def _command_delay_setting(settings, key, default: float) -> float: def command_delay(settings, speed: float, *, calibration: str = "clarity_a020") -> float: if calibration in FIXED_COMMAND_DELAY_LOW: # Do not let persisted Clarity command-delay settings silently retune another car. - return float(np.interp(speed, COMMAND_DELAY_BP, (FIXED_COMMAND_DELAY_LOW[calibration], DEFAULT_COMMAND_DELAY_HIGH))) + low = FIXED_COMMAND_DELAY_LOW[calibration] + high = low if calibration in CIVIC_FLAT_COMMAND_DELAY_CALIBRATIONS else DEFAULT_COMMAND_DELAY_HIGH + return float(np.interp(speed, COMMAND_DELAY_BP, (low, high))) low = _command_delay_setting(settings, NrdrParamKey.NRDR_YAW_COMMAND_DELAY_LOW, DEFAULT_COMMAND_DELAY_LOW) high = _command_delay_setting(settings, NrdrParamKey.NRDR_YAW_COMMAND_DELAY_HIGH, DEFAULT_COMMAND_DELAY_HIGH) return float(np.interp(speed, COMMAND_DELAY_BP, (low, high))) diff --git a/openpilot/nrdr/tests/test_civic_firmware_profiles.py b/openpilot/nrdr/tests/test_civic_firmware_profiles.py index 877e8ce1b88..4f832671117 100644 --- a/openpilot/nrdr/tests/test_civic_firmware_profiles.py +++ b/openpilot/nrdr/tests/test_civic_firmware_profiles.py @@ -114,14 +114,25 @@ def test_c020_feedforward_respects_source_caps_and_lane_change_bypass(speed): assert control.ff_weight == 0. and control.pid.f == 0. -@pytest.mark.parametrize("profile", [CIVIC_PROFILE, TEG_PROFILE]) -def test_civic_keeps_shared_prediction_delay_but_not_saved_clarity_command_delay(profile): - assert prediction_delay_schedule(profile) is None - assert prediction_delay_schedule(CLARITY_PROFILE) is not None +@pytest.mark.parametrize("profile,schedule", [ + (CIVIC_PROFILE, (.28, .28, .28, .26, .26)), + (TEG_PROFILE, (.20, .20, .20, .20, .20)), +]) +def test_civic_has_its_own_prediction_delay_and_not_saved_clarity_command_delay(profile, schedule): + bp, values = prediction_delay_schedule(profile) + assert bp == prediction_delay_schedule(CLARITY_PROFILE)[0] and values == schedule + # the command delay holds above 15 m/s: the Civics ran as early there as in town settings = {"NrdrLateralController": 1, "NrdrYawCommandDelayLow": .3, "NrdrYawCommandDelayHigh": .3} assert [command_delay(settings, speed, calibration=profile.calibration) for speed in (0., 10., 12.5, 15., 30.)] == \ - pytest.approx([.175, .175, .10, .025, .025]) - assert all(tuning_write_allowed(settings, key) for key in ("LagdToggle", "LagdToggleDelay")) + pytest.approx([.175] * 5) + # lagd is locked out once the schedule tells the model the delay + assert not any(tuning_write_allowed(settings, key, firmware_prediction_schedule=profile.prediction_schedule) + for key in ("LagdToggle", "LagdToggleDelay")) + + +def test_the_insight_keeps_its_command_delay_fade(): + assert [command_delay(None, speed, calibration="insight_txm_a040") for speed in (0., 15., 30.)] == \ + pytest.approx([.175, .025, .025]) @pytest.mark.parametrize("key", ["LagdToggle", "LagdToggleDelay"]) diff --git a/openpilot/nrdr/tests/test_cleanup_policy.py b/openpilot/nrdr/tests/test_cleanup_policy.py index 1a03e0211b5..cbace10834d 100644 --- a/openpilot/nrdr/tests/test_cleanup_policy.py +++ b/openpilot/nrdr/tests/test_cleanup_policy.py @@ -68,7 +68,7 @@ def test_shared_geometry_is_not_locked_by_controller_selection(key): def test_only_measured_firmware_schedule_locks_live_manual_delay(key): from openpilot.nrdr.ui.sunnylink_schema import apply_sunnylink_metadata params = MemoryParams(NrdrLateralController=1, **{key: .31}) - assert tuning_write_allowed(params, key) # Civic keeps live/manual delay + assert tuning_write_allowed(params, key) # no prediction schedule: live/manual delay assert not tuning_write_allowed(params, key, firmware_prediction_schedule=True) item = apply_sunnylink_metadata({"key": key}) assert item["enablement"][-1] == {"type": "not", "condition": {"type": "all", "conditions": [ diff --git a/openpilot/nrdr/tests/test_model_source.py b/openpilot/nrdr/tests/test_model_source.py new file mode 100644 index 00000000000..abe2d1b49da --- /dev/null +++ b/openpilot/nrdr/tests/test_model_source.py @@ -0,0 +1,102 @@ +"""Where the model's steering comes from, and the command delay the Firmware Controller takes from it.""" +import ast +import re +from pathlib import Path +from types import SimpleNamespace as NS + +import numpy as np +import pytest + +from opendbc.sunnypilot.car.honda.values_ext import HondaFlagsSP +from opendbc.car.structs import car as car_structs +from opendbc.car.vehicle_model import VehicleModel +from openpilot.nrdr.features.lateral import model_source as ms +from openpilot.nrdr.features.lateral.latcontrol_fw import LatControlFirmware +from openpilot.nrdr.features.lateral.yaw_control_timing import command_delay + +ROOT = Path(__file__).resolve().parents[2] + + +@pytest.mark.parametrize("output,mlsim,source", [ + ({"action": np.zeros((1, 2)), "plan": np.zeros((1, 33, 15))}, True, ms.ACTION_HEAD), + ({"plan": np.zeros((1, 33, 15))}, True, ms.PLAN), # Pop v2, TCP v3 + ({"plan": np.zeros((1, 33, 15)), "desired_curvature": np.zeros((1, 1))}, True, ms.PLAN), + ({"plan": np.zeros((1, 33, 15)), "desired_curvature": np.zeros((1, 1))}, False, ms.CURVATURE_HEAD), +]) +def test_the_source_is_the_one_get_action_from_model_steers_with(output, mlsim, source): + assert ms.lateral_source(output, mlsim) == source + + +def test_the_delay_input_names_how_the_told_delay_reaches_the_model(): + assert ms.delay_input({"action_t": 0, "desire_pulse": 0}) == "actionT" + assert ms.delay_input({"lateral_control_params": 0}) == "lateralControlParams" + assert ms.delay_input({"desire_pulse": 0, "traffic_convention": 0}) == "none" + + +@pytest.mark.parametrize("action_time,delay", [(0.0, None), (-1.0, None), (0.155, 0.145), (0.275, 0.025), (0.3, 0.0), (0.45, 0.0)]) +def test_the_plan_floor_command_delay_lands_the_plan_at_the_told_time(action_time, delay): + assert ms.plan_floor_command_delay(action_time) == (None if delay is None else pytest.approx(delay)) + + +def test_the_slew_starts_at_its_first_target_and_moves_at_its_rate(): + slew = ms.CommandDelaySlew(0.01) + assert slew.update(0.175) == 0.175 + out = [slew.update(0.025) for _ in range(60)] + assert out[0] == pytest.approx(0.175 - 0.003) and out[-1] == pytest.approx(0.025) + + +def _controller(fingerprint, firmware): + cp = NS(brand="honda", carFingerprint=fingerprint, steerRatio=16.5, wheelbase=2.75, steerLimitTimer=.8, + carFw=[NS(ecu="eps", fwVersion=firmware)], lateralTuning=NS(which=lambda: "pid")) + return LatControlFirmware(cp, NS(flags=HondaFlagsSP.EPS_MODIFIED.value), None, .01) + + +def _delays(control, source, action_time, frames, speed=8.): + vm = VehicleModel(car_structs.CarParams.new_message( + mass=1840, rotationalInertia=3000, wheelbase=2.75, centerToFront=1.08, + steerRatioRear=0, steerRatio=16.5, tireStiffnessFront=200000, tireStiffnessRear=200000)) + cs = NS(vEgo=speed, steeringAngleDeg=0., steeringRateDeg=0., steeringPressed=False, steeringTorque=0.) + model = NS(meta=NS(laneChangeState=NS(raw=0)), action=NS(lateralSource=source, lateralActionTime=action_time)) + out = [] + for _ in range(frames): + control.update_model_v2(model) + out.append(control.update(True, cs, vm, NS(roll=0., angleOffsetDeg=0.), False, .001, None, False, .2)[2].commandDelay) + return out + + +@pytest.mark.parametrize("fingerprint,firmware,calibration,action_time", [ + ("HONDA_CLARITY", b"39990-TRW-A020", "clarity_a020", 0.155), # 0.08 told at 7 m/s + ("HONDA_CIVIC", b"39990-TEG-A010", "civic_teg_a010", 0.275), # 0.20 told + ("HONDA_CIVIC_BOSCH", b"39990-TBA-C020", "civic_bosch_c020", 0.355), # 0.28 told: above the floor +]) +def test_a_plan_model_on_a_scheduled_profile_only_undoes_the_floor(fingerprint, firmware, calibration, action_time): + control = _controller(fingerprint, firmware) + head = _delays(control, "actionHead", action_time, 5) + assert head[-1] == pytest.approx(command_delay(None, 8., calibration=calibration), abs=1e-6) + plan = _delays(control, "plan", action_time, 300) + assert plan[-1] == pytest.approx(max(0.3 - action_time, 0.), abs=1e-6) + assert max(abs(a - b) for a, b in zip([head[-1], *plan], plan, strict=False)) <= 0.003 + 1e-6 # slewed, no step + + +def test_an_unknown_source_or_an_older_model_message_keeps_the_calibrations_delay(): + control = _controller("HONDA_CIVIC", b"39990-TEG-A010") + assert _delays(control, "unknown", 0.275, 3)[-1] == pytest.approx(0.175) + control.update_model_v2(NS(meta=NS(laneChangeState=NS(raw=0)))) # no action field (an older publisher) + assert control._command_delay(8.) == pytest.approx(0.175) + + +def test_the_schema_names_every_source_and_delay_input(): + schema = (ROOT / "cereal" / "log.capnp").read_text(encoding="utf-8") + action = re.search(r"\n struct Action \{(?P.*?)\n \}", schema[schema.index("struct ModelDataV2 {"):], re.DOTALL) + body = action.group("body") + assert "lateralSource @4 :LateralSource;" in body and "delayInput @5 :DelayInput;" in body + for name in ("unknown", ms.PLAN, ms.ACTION_HEAD, ms.CURVATURE_HEAD, "none", "actionT", "lateralControlParams"): + assert re.search(rf"\b{name} @\d+;", body), name + + +@pytest.mark.parametrize("publisher", ["selfdrive/modeld/modeld.py", "sunnypilot/modeld_v2/modeld.py"]) +def test_both_publishers_attach_the_source_and_delay_input(publisher): + tree = ast.parse((ROOT / publisher).read_text(encoding="utf-8")) + call, = [n for n in ast.walk(tree) if isinstance(n, ast.Call) and getattr(n.func, "id", None) == "set_lateral_action_timing"] + keywords = {k.arg: k.value for k in call.keywords} + assert keywords["lateral_source"].func.id == "lateral_source" and keywords["delay_input"].func.id == "delay_input" diff --git a/openpilot/nrdr/tests/test_model_timing_metadata.py b/openpilot/nrdr/tests/test_model_timing_metadata.py index d50d6abffa8..5e2deaf466a 100644 --- a/openpilot/nrdr/tests/test_model_timing_metadata.py +++ b/openpilot/nrdr/tests/test_model_timing_metadata.py @@ -115,7 +115,10 @@ def test_production_helper_attaches_atomic_timing_without_changing_model_action( baseline_action = (action.desiredCurvature, action.desiredAcceleration, action.shouldStop) helper(model, compatibility, action_time) + assert (action.lateralSource, action.delayInput) == ("unknown", "unknown") # a caller that does not know yet + helper(model, compatibility, action_time, lateral_source="plan", delay_input="none") + assert (action.lateralSource, action.delayInput) == ("plan", "none") assert model.valid is valid assert model.logMonoTime == 987654321 assert (action.desiredCurvature, action.desiredAcceleration, action.shouldStop) == baseline_action diff --git a/openpilot/nrdr/tests/test_starting_calibrations.py b/openpilot/nrdr/tests/test_starting_calibrations.py index 543ce93fb1b..531f6f0cf9b 100644 --- a/openpilot/nrdr/tests/test_starting_calibrations.py +++ b/openpilot/nrdr/tests/test_starting_calibrations.py @@ -5,7 +5,7 @@ from openpilot.nrdr.features.lateral.controller_selection import ( A030_PROFILE, C120_PROFILE, CRV_A040_PROFILE, CRV_A220_PROFILE, TGG_PROFILE, ) -from openpilot.nrdr.features.lateral.yaw_control_timing import command_delay +from openpilot.nrdr.features.lateral.yaw_control_timing import command_delay, prediction_delay_schedule STARTING_PROFILES = (C120_PROFILE, TGG_PROFILE, A030_PROFILE, CRV_A040_PROFILE, CRV_A220_PROFILE) # Corrected-PTM starting calibrations. Golden R5 for the same 0xE4 at 20 m/s, from JamesL787 ns-bosch-radar-testing @@ -36,7 +36,9 @@ def test_starting_calibrations_match_the_source_tree_and_their_build(name): cal, rwd, e4, clamp = STARTING[name] profile = next(p for p in STARTING_PROFILES if p.calibration == name) - assert profile.required_rwd == rwd and profile.provisional and not profile.prediction_schedule + assert profile.required_rwd == rwd and profile.provisional + # the Civics take their measured sibling's delay schedule; the CR-Vs keep live/manual delay + assert profile.prediction_schedule == name.startswith("civic_") assert cal.e4_per_output == e4 and cal.key_clamp == clamp for e4_value, expected in zip(STARTING_E4, STARTING_GOLDEN_R5[name], strict=True): assert core.r5_from_output(e4_value / cal.e4_per_output, 20., cal) == pytest.approx(expected, abs=0.5) @@ -60,6 +62,20 @@ def test_starting_calibration_vehicle_side_and_command_delay(): assert command_delay({"NrdrYawCommandDelayLow": 0.3}, 5.0, calibration=name) == pytest.approx(.175) for name in ("crv_tla_a040", "crv_tla_a220"): assert command_delay({"NrdrYawCommandDelayLow": 0.3}, 5.0, calibration=name) == pytest.approx(.145) + # above 15 m/s the Civics hold theirs (as the C020 and TEG, measured); the CR-Vs, unmeasured, keep the old fade + for name in ("civic_tba_c120", "civic_tgg_a120", "civic_tba_a030"): + assert command_delay({"NrdrYawCommandDelayHigh": 0.3}, 30.0, calibration=name) == pytest.approx(.175) + for name in ("crv_tla_a040", "crv_tla_a220"): + assert command_delay({"NrdrYawCommandDelayHigh": 0.3}, 30.0, calibration=name) == pytest.approx(.025) + + +@pytest.mark.parametrize("name,sibling", [("civic_tba_a030", "civic_teg_a010"), ("civic_tba_c120", "civic_bosch_c020"), + ("civic_tgg_a120", "civic_bosch_c020")]) +def test_starting_civics_are_told_their_siblings_measured_lag(name, sibling): + from openpilot.nrdr.features.lateral.controller_selection import _PROFILES + profiles = {p.calibration: p for p in _PROFILES.values()} + assert prediction_delay_schedule(profiles[name]) == prediction_delay_schedule(profiles[sibling]) + assert prediction_delay_schedule(profiles[name]) != prediction_delay_schedule(profiles["clarity_a020"]) # The CR-V's key clamp is 1774 (the C120's 1663), though its row ends at key 1663, so R5 is flat from there to the clamp. assert core.key_ceiling(20., core.CRV_TLA_A040) == 1774 and core.key_ceiling(20., core.CIVIC_TBA_C120) == 1663 assert core.CIVIC_TBA_A030.r5_v == core.CIVIC_TEG_A010.r5_v and core.CIVIC_TBA_A030.r6_angle_gain == core.CIVIC_TEG_A010.r6_angle_gain diff --git a/openpilot/nrdr/tests/test_vfn_controller_runtime.py b/openpilot/nrdr/tests/test_vfn_controller_runtime.py index 683a4ca0de6..0965c501eb4 100644 --- a/openpilot/nrdr/tests/test_vfn_controller_runtime.py +++ b/openpilot/nrdr/tests/test_vfn_controller_runtime.py @@ -66,7 +66,7 @@ def test_civic_wrapper_uses_its_own_calibration_and_the_c020_trims_without_mutat assert control.core.ff.cal is (CIVIC_TEG_A010 if teg else CIVIC_BOSCH_C020) assert control.core.ff.load_coefficients == (CIVIC_TEG_EPS_LOAD if teg else CIVIC_EPS_LOAD) assert control.core.p_scale == CIVIC_P_SCALE and control.core.i_scale == CIVIC_I_SCALE - assert control.delay_schedule is None + assert control.delay_schedule[1] == ((.20,) * 5 if teg else (.28, .28, .28, .26, .26)) # its own measured lag assert not control.firmware_profile.provisional # User PIF/yaw settings cannot leak into either firmware profile. control.set_live_tuning_snapshot(ParamSnapshot(1, {"NrdrDeviceYawCorrection": True, "LatFScaleHighway": 500, @@ -75,7 +75,7 @@ def test_civic_wrapper_uses_its_own_calibration_and_the_c020_trims_without_mutat for _ in range(100): output, _, logged = tick(control, state=state) assert math.isfinite(output) and abs(output) <= control.steer_max - assert logged.commandDelay == pytest.approx(.025) + assert logged.commandDelay == pytest.approx(.175) # held flat above 15 m/s if state in (2, 3): assert logged.f == 0. and logged.epsFfWeight == 0. @@ -97,7 +97,7 @@ def test_insight_wrapper_uses_its_own_calibration_the_c020_trims_and_the_clarity @pytest.mark.parametrize("changes", SUPPORTED_CIVICS) -@pytest.mark.parametrize("speed,delay", [(5., .175), (12.5, .10), (20., .025)]) +@pytest.mark.parametrize("speed,delay", [(5., .175), (12.5, .175), (20., .175)]) def test_civic_wrapper_uses_its_command_delay_and_preserves_shared_steer_ratio(changes, speed, delay): control = controller(**changes) cp, _ = car(**changes) diff --git a/openpilot/selfdrive/modeld/modeld.py b/openpilot/selfdrive/modeld/modeld.py index 6dde6787a20..2a7fb011ac7 100755 --- a/openpilot/selfdrive/modeld/modeld.py +++ b/openpilot/selfdrive/modeld/modeld.py @@ -41,6 +41,7 @@ from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController from openpilot.nrdr.features.services import admit_default_artifact +from openpilot.nrdr.features.lateral.model_source import delay_input, lateral_source SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -78,10 +79,14 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log. shouldStop=bool(stop)) -def set_lateral_action_timing(modelv2_send, mdv2sp_send, lat_action_t: float) -> None: - """Attach one source identity and horizon before either model event is sent.""" +def set_lateral_action_timing(modelv2_send, mdv2sp_send, lat_action_t: float, + lateral_source: str = "unknown", delay_input: str = "unknown") -> None: + """Attach one source identity and horizon before either model event is sent, and where the steering came from + (nrdr.features.lateral.model_source) so the Firmware Controller can size its command delay.""" action_time = float(lat_action_t) modelv2_send.modelV2.action.lateralActionTime = action_time + modelv2_send.modelV2.action.lateralSource = lateral_source + modelv2_send.modelV2.action.delayInput = delay_input mdv2sp_send.valid = modelv2_send.valid mdv2sp_send.modelDataV2SP.modelMonoTime = modelv2_send.logMonoTime mdv2sp_send.modelDataV2SP.lateralActionTime = action_time @@ -450,7 +455,8 @@ def load_big(): publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, extrinsics_calibration_seen) modelv2_send.modelV2.big = model.chestnut - set_lateral_action_timing(modelv2_send, mdv2sp_send, lat_action_t) + set_lateral_action_timing(modelv2_send, mdv2sp_send, lat_action_t, + lateral_source=lateral_source(model_output, mlsim=True), delay_input=delay_input(model.npy)) desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index b81f040c40c..4dc5b6d9fca 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -47,6 +47,7 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController from openpilot.nrdr.features.services import admit_selected_artifact, admit_default_artifact +from openpilot.nrdr.features.lateral.model_source import delay_input, lateral_source PROCESS_NAME = "openpilot.selfdrive.modeld.modeld_tinygrad" BIG_MODEL_TIMEOUT = 60 @@ -482,7 +483,8 @@ def load_big(): publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, meta_constants) modelv2_send.modelV2.big = model.chestnut - set_lateral_action_timing(modelv2_send, mdv2sp_send, lat_action_t) + set_lateral_action_timing(modelv2_send, mdv2sp_send, lat_action_t, + lateral_source=lateral_source(model_output, model.mlsim), delay_input=delay_input(model.numpy_inputs)) drivingdata_send.drivingModelData.action.lateralActionTime = modelv2_send.modelV2.action.lateralActionTime desire_state = modelv2_send.modelV2.meta.desireState diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index abdc770e0bb..fdc72b632ce 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -4163,7 +4163,7 @@ "key": "LagdToggle", "widget": "toggle", "title": "Live Learning Steer Delay", - "description": "Learn steering delay. Clarity Firmware Controller uses its measured speed schedule instead; Civic Firmware Controller uses this live/manual delay path normally.", + "description": "Learn steering delay. The Firmware Controller uses its measured speed schedule instead.", "enablement": [ { "type": "not", @@ -4205,8 +4205,8 @@ "key": "LagdToggleDelay", "widget": "option", "title": "Adjust Software Delay", - "description": "Additional software delay when live learning is off. Clarity Firmware Controller uses a measured speed schedule; Civic Firmware Controller uses this value normally.", - "details": "Software delay is added to the vehicle's actuator delay. Clarity Firmware Controller instead uses 0.15/0.08/0.10/0.20/0.30 seconds at 3.5/7/12/20/30 m/s, smoothly interpolated. Saved settings are preserved for other controllers. Civic has no measured prediction schedule yet. Firmware command delay and model-specific smoothing are separate. Default additional software delay is 0.2 seconds.", + "description": "Additional software delay when live learning is off. The Firmware Controller uses a measured speed schedule instead.", + "details": "Software delay is added to the vehicle's actuator delay. Clarity Firmware Controller instead uses 0.15/0.08/0.10/0.20/0.30 seconds at 3.5/7/12/20/30 m/s, smoothly interpolated. The Civics use their own (TEG-A010 0.20 seconds; C020 0.28 seconds, 0.26 on the highway). Saved settings are preserved for other controllers. Firmware command delay and model-specific smoothing are separate. Default additional software delay is 0.2 seconds.", "min": 0.05, "max": 1.0, "step": 0.01, diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml index 0a2c8515ba3..c0b08dc5884 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml @@ -34,12 +34,12 @@ sections: - key: LagdToggle widget: toggle title: Live Learning Steer Delay - description: Learn steering delay. Clarity Firmware Controller uses its measured speed schedule instead; Civic Firmware Controller uses this live/manual delay path normally. + description: Learn steering delay. The Firmware Controller uses its measured speed schedule instead. - key: LagdToggleDelay widget: option title: Adjust Software Delay - description: Additional software delay when live learning is off. Clarity Firmware Controller uses a measured speed schedule; Civic Firmware Controller uses this value normally. - details: Software delay is added to the vehicle's actuator delay. Clarity Firmware Controller instead uses 0.15/0.08/0.10/0.20/0.30 seconds at 3.5/7/12/20/30 m/s, smoothly interpolated. Saved settings are preserved for other controllers. Civic has no measured prediction schedule yet. Firmware command delay and model-specific smoothing are separate. Default additional software delay is 0.2 seconds. + description: Additional software delay when live learning is off. The Firmware Controller uses a measured speed schedule instead. + details: Software delay is added to the vehicle's actuator delay. Clarity Firmware Controller instead uses 0.15/0.08/0.10/0.20/0.30 seconds at 3.5/7/12/20/30 m/s, smoothly interpolated. The Civics use their own (TEG-A010 0.20 seconds; C020 0.28 seconds, 0.26 on the highway). Saved settings are preserved for other controllers. Firmware command delay and model-specific smoothing are separate. Default additional software delay is 0.2 seconds. min: 0.05 max: 1.0 step: 0.01 From 82e57e378ce9bed1ad3c4e17c65eebdb9a3cf024 Mon Sep 17 00:00:00 2001 From: JamesL787 Date: Sun, 18 Oct 2026 13:34:03 -0400 Subject: [PATCH 3/3] lateral: the Clarity's command delay defaults to 0.125 s above 15 m/s The high end was 0.025 s (the source's none plus the port's 0.025 s compensation), set from 5-12 m/s turns only. By speed band on routes 3f1 / 3f5 (an action-head model, no command delay above 15 m/s) the Clarity ran 0.12 / 0.14 / 0.10 s early at 17 / 16 / 25 m/s, so the default is 0.10 + 0.025. A saved NrdrYawCommandDelayHigh is kept. The Insight keeps its own 0.025 s; plan models take the floor formula either way. Co-Authored-By: Claude Opus 5.5 --- openpilot/nrdr/docs/firmware_controller_profiles.md | 5 +++-- openpilot/nrdr/features/lateral/yaw_control_timing.py | 10 ++++++++-- openpilot/nrdr/params/generated/params_keys.inc | 2 +- openpilot/nrdr/params/specs.py | 2 +- openpilot/nrdr/tests/test_param_catalog.py | 2 +- openpilot/nrdr/tests/test_vfn_controller_runtime.py | 2 +- openpilot/nrdr/tests/test_yaw_control_timing.py | 4 ++-- 7 files changed, 17 insertions(+), 10 deletions(-) diff --git a/openpilot/nrdr/docs/firmware_controller_profiles.md b/openpilot/nrdr/docs/firmware_controller_profiles.md index fc71dccc07c..69c44ce372a 100644 --- a/openpilot/nrdr/docs/firmware_controller_profiles.md +++ b/openpilot/nrdr/docs/firmware_controller_profiles.md @@ -62,8 +62,9 @@ fixed 0.075 seconds. lagd told the TEG 0.37-0.39 seconds, which ran it early. Model-specific smoothing remains separate, as in the source implementation. Command delay is a separate interpolated curvature history buffer, not a blocking -sleep. It fades between 10 and 15 m/s. Clarity retains its saved endpoints, default -0.145 / 0.025 seconds. The Insight uses fixed 0.175 / 0.025 seconds and the Civics +sleep. It moves between its ends from 10 to 15 m/s. Clarity retains its saved endpoints, +default 0.145 / 0.125 seconds (the high end was 0.025 until the Clarity, on an action-head +model, measured 0.10-0.14 seconds early above 15 m/s without it). The Insight uses fixed 0.175 / 0.025 seconds and the Civics 0.175 seconds at every speed, since they ran as early on the highway as in town (source 0.150 plus the existing port's 0.025-second model-action compensation); saved Clarity command-delay settings do not retune them. These are model-dependent diff --git a/openpilot/nrdr/features/lateral/yaw_control_timing.py b/openpilot/nrdr/features/lateral/yaw_control_timing.py index c6fcaf71951..bf62b4935f7 100644 --- a/openpilot/nrdr/features/lateral/yaw_control_timing.py +++ b/openpilot/nrdr/features/lateral/yaw_control_timing.py @@ -46,9 +46,15 @@ # model's plan 0.12-0.14 s early on 5-12 m/s turns. 0.12 s fixed that; the defaults add 0.025 s that # stands in for the source branch's model-action interpolation, which this port does not carry. # Other models aim differently, so both ends are settings; the delay fades between them on speed. +# The high end was 0.025 (none from the source): those drives measured 5-12 m/s turns only. By speed band +# (JamesL787, 2026-10-18) the Clarity ran 0.12 / 0.14 / 0.10 s early at 17 / 16 / 25 m/s with no command delay +# (routes 3f1 / 3f5, a Mountain-Dew-style action head; 0.10 s is the 1667-frame highway band), so 0.10 + 0.025. COMMAND_DELAY_BP = (10.0, 15.0) # m/s DEFAULT_COMMAND_DELAY_LOW = 0.145 # s -DEFAULT_COMMAND_DELAY_HIGH = 0.025 # s +DEFAULT_COMMAND_DELAY_HIGH = 0.125 # s +# The other fixed calibrations' high end, unchanged: the Insight's Cinque route needed ~0.05 s at 12-20 m/s, and no +# CR-V timing has been measured. +FIXED_COMMAND_DELAY_HIGH = 0.025 # s CIVIC_COMMAND_DELAY_LOW = 0.175 # 0.15 source + 0.025 port compensation # The TEG-A010 keeps it: with 0.175 applied, plan timing on its owner's routes 12e/134/137/139 (5-12 m/s turns) # measured the car 0.05 s early (+0.02/-0.06/-0.06/-0.08), so there is no case for less; mostly highway drives. @@ -93,7 +99,7 @@ def command_delay(settings, speed: float, *, calibration: str = "clarity_a020") if calibration in FIXED_COMMAND_DELAY_LOW: # Do not let persisted Clarity command-delay settings silently retune another car. low = FIXED_COMMAND_DELAY_LOW[calibration] - high = low if calibration in CIVIC_FLAT_COMMAND_DELAY_CALIBRATIONS else DEFAULT_COMMAND_DELAY_HIGH + high = low if calibration in CIVIC_FLAT_COMMAND_DELAY_CALIBRATIONS else FIXED_COMMAND_DELAY_HIGH return float(np.interp(speed, COMMAND_DELAY_BP, (low, high))) low = _command_delay_setting(settings, NrdrParamKey.NRDR_YAW_COMMAND_DELAY_LOW, DEFAULT_COMMAND_DELAY_LOW) high = _command_delay_setting(settings, NrdrParamKey.NRDR_YAW_COMMAND_DELAY_HIGH, DEFAULT_COMMAND_DELAY_HIGH) diff --git a/openpilot/nrdr/params/generated/params_keys.inc b/openpilot/nrdr/params/generated/params_keys.inc index 7e8ed3805e6..523c42fee86 100644 --- a/openpilot/nrdr/params/generated/params_keys.inc +++ b/openpilot/nrdr/params/generated/params_keys.inc @@ -149,4 +149,4 @@ {"HondaCivicRadarTryout", {PERSISTENT | BACKUP, BOOL, "0"}}, {"NrdrYawDelaySchedule", {PERSISTENT | BACKUP, BOOL, "1"}}, {"NrdrYawCommandDelayLow", {PERSISTENT | BACKUP, FLOAT, "0.145"}}, - {"NrdrYawCommandDelayHigh", {PERSISTENT | BACKUP, FLOAT, "0.025"}}, + {"NrdrYawCommandDelayHigh", {PERSISTENT | BACKUP, FLOAT, "0.125"}}, diff --git a/openpilot/nrdr/params/specs.py b/openpilot/nrdr/params/specs.py index bd85d159a58..0098b47a763 100644 --- a/openpilot/nrdr/params/specs.py +++ b/openpilot/nrdr/params/specs.py @@ -256,7 +256,7 @@ def _added(key: str, param_type: ParamType, flags: tuple[ParamFlag, ...], defaul _added("NrdrYawDelaySchedule", ParamType.BOOL, PB, "1", ParamLifecycle.TOMBSTONE, ParamOwner.LATERAL), # Command delay remains distinct from model/actuator delay, live in seconds. _added("NrdrYawCommandDelayLow", ParamType.FLOAT, PB, "0.145", owner=ParamOwner.LATERAL), - _added("NrdrYawCommandDelayHigh", ParamType.FLOAT, PB, "0.025", owner=ParamOwner.LATERAL), + _added("NrdrYawCommandDelayHigh", ParamType.FLOAT, PB, "0.125", owner=ParamOwner.LATERAL), ) diff --git a/openpilot/nrdr/tests/test_param_catalog.py b/openpilot/nrdr/tests/test_param_catalog.py index c7a6508232a..da3fe78376e 100644 --- a/openpilot/nrdr/tests/test_param_catalog.py +++ b/openpilot/nrdr/tests/test_param_catalog.py @@ -25,7 +25,7 @@ ) -REGISTRY_METADATA_SHA256 = "c6bc7e9fb72a6a6341306de15c36a28dc74f5a9483498c4b4a1ee626e429407d" +REGISTRY_METADATA_SHA256 = "1fed2234100e595815d77a0d280f7a08ce0290ca6a4eceec2707047b7b252c24" class TestParamCatalog(unittest.TestCase): diff --git a/openpilot/nrdr/tests/test_vfn_controller_runtime.py b/openpilot/nrdr/tests/test_vfn_controller_runtime.py index 0965c501eb4..70a7911b994 100644 --- a/openpilot/nrdr/tests/test_vfn_controller_runtime.py +++ b/openpilot/nrdr/tests/test_vfn_controller_runtime.py @@ -173,7 +173,7 @@ def test_lane_change_shaping_reduces_request_without_reversing_it(curvature): assert abs(shaped) < abs(curvature) -@pytest.mark.parametrize("speed, delay", [(5., .145), (12.5, .085), (20., .025)]) +@pytest.mark.parametrize("speed, delay", [(5., .145), (12.5, .135), (20., .125)]) def test_default_command_delay_holds_the_request_back_on_speed(speed, delay): control = controller() for _ in range(50): diff --git a/openpilot/nrdr/tests/test_yaw_control_timing.py b/openpilot/nrdr/tests/test_yaw_control_timing.py index 7fc522d3568..88e2858e1e9 100644 --- a/openpilot/nrdr/tests/test_yaw_control_timing.py +++ b/openpilot/nrdr/tests/test_yaw_control_timing.py @@ -17,13 +17,13 @@ def test_reference_schedule_is_the_refit_table(): @pytest.mark.parametrize("settings, speed, expected", [ - (None, 0., .145), (None, 12.5, .085), (None, 30., .025), + (None, 0., .145), (None, 12.5, .135), (None, 30., .125), (ParamSnapshot(1, {}), 5., .145), (ParamSnapshot(1, {"NrdrYawCommandDelayLow": .2, "NrdrYawCommandDelayHigh": .05}), 12.5, .125), (ParamSnapshot(1, {"NrdrYawCommandDelayLow": 9., "NrdrYawCommandDelayHigh": -1.}), 5., timing.MAX_COMMAND_DELAY), (ParamSnapshot(1, {"NrdrYawCommandDelayLow": 9., "NrdrYawCommandDelayHigh": -1.}), 20., 0.), (ParamSnapshot(1, {"NrdrYawCommandDelayLow": math.nan, "NrdrYawCommandDelayHigh": b"x"}), 5., .145), - (ParamSnapshot(1, {"NrdrYawCommandDelayLow": math.nan, "NrdrYawCommandDelayHigh": b"x"}), 20., .025), + (ParamSnapshot(1, {"NrdrYawCommandDelayLow": math.nan, "NrdrYawCommandDelayHigh": b"x"}), 20., .125), ]) def test_command_delay_defaults_fade_and_bounds(settings, speed, expected): assert timing.command_delay(settings, speed) == pytest.approx(expected)