From 3e1a38291983febc14cf8e2900c65c29fa45c187 Mon Sep 17 00:00:00 2001 From: Jacob <144983089+SloPOS@users.noreply.github.com> Date: Thu, 25 Jun 2026 09:31:12 -0400 Subject: [PATCH 01/11] docs: lane-change auto-signal design spec Spec for automating turn-signal persistence during Pre-AP lane changes: short-tap arming, 7-flash budget, DAS_bodyControls blinker drive (Tinkla mechanism), and the "Nudge wheel within N signals" countdown. Co-Authored-By: Claude Opus 4.8 --- ...26-06-25-lane-change-auto-signal-design.md | 192 ++++++++++++++++++ 1 file changed, 192 insertions(+) create mode 100644 docs/superpowers/specs/2026-06-25-lane-change-auto-signal-design.md diff --git a/docs/superpowers/specs/2026-06-25-lane-change-auto-signal-design.md b/docs/superpowers/specs/2026-06-25-lane-change-auto-signal-design.md new file mode 100644 index 00000000000000..5857986795998f --- /dev/null +++ b/docs/superpowers/specs/2026-06-25-lane-change-auto-signal-design.md @@ -0,0 +1,192 @@ +# Lane-Change Auto Signal — Design Spec + +**Date:** 2026-06-25 +**Car:** NAP Pre-AP Tesla Model S (`TESLA_MODEL_S_PREAP`) +**Branch:** `lane-change-auto-signal` + +## Problem + +Today, an automated lane change requires the driver to **hold** the turn-signal +stalk for the entire arming window. A short stalk tap flashes the indicator 3 +times then auto-cancels (stock car behavior), which collapses the arming window +before the driver can nudge the wheel. The driver must physically hold the stalk +down, which is awkward. + +## Goal + +Replicate the Tesla-Unity / Tinkla behavior: + +1. Driver **short-taps** the turn-signal stalk → lane change arms. +2. The stock car flashes 3×; openpilot **sustains the flashing** to a total of + **7 flashes** while armed (the stock 3 plus 4 more). +3. If the driver **nudges the wheel** within those 7 flashes, the lane change + proceeds and the blinker **keeps flashing until the maneuver completes**, then + stops automatically. +4. If the driver does **not** nudge within 7 flashes, arming cancels and the + blinker stops. +5. The UI shows a live countdown: **"Nudge wheel to engage lane change within N + signals"**. + +## Verified Mechanism (from reading the code + `origin/tesla-unity`) + +### Blinker READ (already correct) +`opendbc_repo/opendbc/car/tesla/preap/carstate.py:99-100`: +```python +ret.leftBlinker = GTW_carState.BC_indicatorLStatus == 1 +ret.rightBlinker = GTW_carState.BC_indicatorRStatus == 1 +``` +Reads the *actual indicator light*. A stock tap reads True during the 3 flashes, +then False. Rising edges of these flags are the flash counter source. + +### Blinker DRIVE (the missing piece) +Tinkla drove the Pre-AP blinker via **`DAS_bodyControls`** (CAN `0x3E9` / 1001), +field `DAS_turnIndicatorRequest` (`0=NONE, 1=LEFT, 2=RIGHT, 3=CANCEL`). +Verified in `origin/tesla-unity`: +- `selfdrive/car/tesla/HUD_module.py:259-264` sends + `create_body_controls_message(alca_direction, ...)`; the send gate explicitly + includes `carFingerprint == CAR.PREAP_MODELS`. +- `selfdrive/car/tesla/carcontroller.py:83`: + `CS.alca_direction = CC.rightBlinker * 2 + CC.leftBlinker`. + +`DAS_bodyControls` (BO_ 1001) exists in NAP `tesla_preap.dbc`. It is **not** in +the current NAP panda TX allowlist (`PREAP_TX_MSGS`) — must be added. + +### Existing-but-dead plumbing +`controlsd.py:103-106` already sets `CC.leftBlinker`/`rightBlinker` during the +lane-change maneuver, but the NAP Tesla carcontroller (`tesla/carcontroller.py` +and `tesla/preap/carcontroller.py`) **never consumes** these flags — confirmed by +grep (every other brand's carcontroller reads them; Tesla's does not). So the +flag is set and silently dropped. This is why "blinker during lane change" appears +implemented but doesn't work. Wiring a consumer in `preap/carcontroller.py` makes +the maneuver-time flashing (and auto-stop on completion) work essentially for free. + +### Short-tap vs hold (Tinkla insight) +`origin/tesla-unity selfdrive/car/tesla/carstate.py:405`: +```python +ret.leftBlinker = (BC_indicatorLStatus==1) and (turnSignalStalkState==0) and (tap_direction==1) +``` +A **tap** = light flashing while the stalk lever (`STW_ACTN_RQ.TurnIndLvr_Stat`, +read-only) has already returned to idle (0). `tap_direction` latches the armed +direction. We replicate the latch in `desire_helper`, keyed on the existing +`one_blinker` rising edge (which a tap produces). + +### Safety +`opendbc_repo/opendbc/safety/modes/tesla_preap.h`: the TX hook does not currently +touch `0x3E9`. We add `0x3E9` to `PREAP_TX_MSGS` and add a TX-hook bound so +`DAS_turnIndicatorRequest` is limited to valid values (0–3). All other safety +checks unchanged. `controls_allowed` gating still applies (framework-level). + +## Design + +### State machine (`selfdrive/controls/lib/desire_helper.py`) + +New constants: +- `LANE_CHANGE_FLASH_BUDGET = 7` — total flashes allowed in the arming window. + +New state on `DesireHelper`: +- `lane_change_flashes_seen: int` — rising edges of the real blinker counted + while in `preLaneChange`. +- `flashes_remaining: int` — `max(LANE_CHANGE_FLASH_BUDGET - lane_change_flashes_seen, 0)`, + published for the UI. +- `prev_blinker_on: bool` — for edge detection. + +Behavior changes: + +**Entering `preLaneChange`** (unchanged trigger: `one_blinker` rising edge, not +below speed): latch `lane_change_direction`; reset `lane_change_flashes_seen = 0`. + +**While in `preLaneChange`:** +- Count a flash on each rising edge of the *armed-direction* blinker + (`carstate.leftBlinker` if armed left, else right). +- Compute `flashes_remaining`. +- **Exit → `laneChangeStarting`** when `torque_applied and not blindspot_detected` + (existing wheel-nudge check). +- **Exit → `off`** when `flashes_remaining <= 0` (budget exhausted, no nudge). +- **Exit → `off`** when below lane-change speed or lateral disengaged (existing). +- **Opposite-direction tap** (a fresh `one_blinker` rising edge in the opposite + direction): cancel → `off` (driver must tap again to re-arm; no auto re-arm). +- The arming latch **no longer requires `one_blinker` to remain true** — this is + the core fix. A momentary tap arms; the latch holds for up to 7 flashes. + +**During the maneuver** (`laneChangeStarting`/`laneChangeFinishing`): blinker +keeps flashing until `laneChangeState` returns to `off`. The 7-flash budget governs +**only** the pre-nudge arming window, not the maneuver. (Falls out of existing +`controlsd` logic.) + +### Cereal (`cereal/log.capnp`) +Add to `ModelDataV2.MetaData`: +```capnp +laneChangeFlashesRemaining @2 :UInt8; +``` +Append-only (next index) → safe schema evolution. + +### Model process (`selfdrive/modeld/modeld.py`) +After `DH.update(...)`, publish: +```python +modelv2_send.modelV2.meta.laneChangeFlashesRemaining = DH.flashes_remaining +``` + +### Controls (`selfdrive/controls/controlsd.py`) +**No change required.** The existing condition at `controlsd.py:104` +(`if model_v2.meta.laneChangeState != LaneChangeState.off`) already covers +`preLaneChange` (which is `!= off`), and `desire_helper` already sets +`lane_change_direction` on entering `preLaneChange` (`desire_helper.py:61`). So +`CC.leftBlinker`/`rightBlinker` is already asserted throughout the arming window +and the maneuver. The only reason it doesn't flash today is the missing +carcontroller consumer (below). + +### Tesla carcontroller (`opendbc_repo/.../tesla/preap/carcontroller.py`) +New consumer in `_update_preap`, sent on a fixed cadence (e.g. every 10 frames, +matching Tinkla's body-controls rate): +```python +turn = CC.rightBlinker * 2 + CC.leftBlinker # 0 none, 1 left, 2 right +if self.frame % 10 == 0: + can_sends.append(self.tesla_can.create_body_controls_message(turn, 0, CANBUS.party, counter)) +``` + +### Tesla CAN (`opendbc_repo/.../tesla/preap/teslacan.py`) +Port `create_body_controls_message` (DAS_bodyControls, 0x3E9) with the NAP +checksum convention. Set `DAS_turnIndicatorRequest = turn`, +`DAS_turnIndicatorRequestReason = 1 if turn>0 else 0`, counter, checksum. + +### Panda safety (`opendbc_repo/opendbc/safety/modes/tesla_preap.h`) +- Add to `PREAP_TX_MSGS`: + `{0x3E9, 0, 8, .check_relay = false, .disable_static_blocking = true}`. +- In `tesla_preap_tx_hook`, bound `0x3E9`: reject if + `DAS_turnIndicatorRequest > 3` (defense-in-depth; valid values only). + +### Alerts (`selfdrive/selfdrived/events.py`) +Replace the static `preLaneChangeLeft`/`preLaneChangeRight` `Alert` objects with +dynamic callbacks (`AlertCallbackType`, signature `func(CP, CS, sm, metric, +soft_disable_time, personality) -> Alert`). The callback reads +`sm['modelV2'].meta.laneChangeFlashesRemaining` and formats: +> "Nudge wheel to engage lane change within {N} signals" + +Keep the existing direction-specific phrasing where helpful, e.g. +"Nudge wheel left to change lane within {N} signals". + +## Testing + +- **`desire_helper` unit tests:** arm on tap; count 7 flashes; cancel at budget + exhaustion without nudge; proceed on wheel nudge; blinker persists through + maneuver; opposite-tap cancels; below-speed and blindspot exits preserved. +- **`preap/carcontroller` + `teslacan` tests:** `CC.leftBlinker` → correct + `DAS_bodyControls` frame (turn value, reason, checksum, counter, cadence). +- **Panda safety test:** `0x3E9` accepted when `controls_allowed` with valid + `DAS_turnIndicatorRequest`; rejected when value > 3. +- **Process replay / regression:** existing lane-change replay still passes. + +## Constants Summary + +| Constant | Value | Location | +|---|---|---| +| `LANE_CHANGE_FLASH_BUDGET` | 7 | `desire_helper.py` | +| body-controls TX cadence | every 10 frames (~10 Hz) | `preap/carcontroller.py` | +| `DAS_bodyControls` addr | `0x3E9` (1001) | DBC / safety | + +## Out of Scope + +- Non-Pre-AP Tesla variants (AP1/AP2/HW3) — they have a DAS ECU and different + blinker handling; this spec targets `TESLA_MODEL_S_PREAP` only. +- Hazard lights, headlight/wiper body controls (the DAS_bodyControls message + carries them, but we send 0 / leave them stock). From 0e950472b9b4489a249733ce675ae96aa8c4c5ff Mon Sep 17 00:00:00 2001 From: Jacob <144983089+SloPOS@users.noreply.github.com> Date: Thu, 25 Jun 2026 09:35:25 -0400 Subject: [PATCH 02/11] docs: lane-change auto-signal implementation plan Task-by-task TDD plan: cereal field, desire_helper flash latch, modeld publish, dynamic countdown alert, DAS_bodyControls turn-signal drive, and panda safety allowlist for 0x3E9. Co-Authored-By: Claude Opus 4.8 --- .../2026-06-25-lane-change-auto-signal.md | 777 ++++++++++++++++++ 1 file changed, 777 insertions(+) create mode 100644 docs/superpowers/plans/2026-06-25-lane-change-auto-signal.md diff --git a/docs/superpowers/plans/2026-06-25-lane-change-auto-signal.md b/docs/superpowers/plans/2026-06-25-lane-change-auto-signal.md new file mode 100644 index 00000000000000..5dec195e50e8a4 --- /dev/null +++ b/docs/superpowers/plans/2026-06-25-lane-change-auto-signal.md @@ -0,0 +1,777 @@ +# Lane-Change Auto Signal Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Make a short turn-signal stalk tap arm an automated lane change on the Pre-AP Tesla Model S, sustaining the blinker to 7 flashes with a live "Nudge wheel within N signals" countdown, and stopping the blinker automatically when the maneuver completes. + +**Architecture:** Three layers. (1) `desire_helper.py` gains a flash-counted arming latch that no longer requires the driver to hold the stalk. (2) `modeld` publishes `flashes_remaining` on `modelV2.meta`; `selfdrived` renders it as a dynamic countdown alert. (3) The Pre-AP carcontroller consumes the already-set `CC.leftBlinker`/`rightBlinker` flags and drives the blinker via the `DAS_bodyControls` (0x3E9) CAN message (the proven Tinkla mechanism), which is added to the panda Pre-AP TX allowlist. + +**Tech Stack:** Python (openpilot controls/model), Cap'n Proto (cereal), C (panda safety), opendbc CANPacker, pytest. + +--- + +## File Structure + +| File | Responsibility | Action | +|---|---|---| +| `cereal/log.capnp` | Add `laneChangeFlashesRemaining` to `ModelDataV2.MetaData` | Modify | +| `selfdrive/controls/lib/desire_helper.py` | Flash-counted arming latch + `flashes_remaining` | Modify | +| `selfdrive/controls/lib/tests/test_desire_helper.py` | Unit tests for the latch | Create | +| `selfdrive/modeld/modeld.py` | Publish `DH.flashes_remaining` | Modify | +| `selfdrive/selfdrived/events.py` | Dynamic countdown alerts | Modify | +| `opendbc_repo/opendbc/car/tesla/preap/teslacan.py` | `create_body_controls_message` (0x3E9) | Modify | +| `opendbc_repo/opendbc/car/tesla/preap/carcontroller.py` | Consume `CC.leftBlinker/rightBlinker` → send body controls | Modify | +| `opendbc_repo/opendbc/car/tesla/preap/tests/test_body_controls.py` | Frame-level tests | Create | +| `opendbc_repo/opendbc/safety/modes/tesla_preap.h` | Allowlist 0x3E9 + bound turn value | Modify | +| `opendbc_repo/opendbc/safety/tests/test_tesla_preap.py` | Safety test for 0x3E9 | Modify | + +**Note on submodule:** `opendbc_repo` is a git submodule. CAN/safety changes are committed *inside* the submodule, then the parent repo records the new submodule SHA. Each opendbc task's commit step runs inside `opendbc_repo`; a final task bumps the submodule pointer in the parent. + +--- + +## Task 1: Add `laneChangeFlashesRemaining` to cereal schema + +**Files:** +- Modify: `cereal/log.capnp` (struct `ModelDataV2.MetaData`, ~line 1052) + +- [ ] **Step 1: Add the field** + +In `cereal/log.capnp`, change: +```capnp + struct MetaData { + laneChangeState @0 :LaneChangeState; + laneChangeDirection @1 :LaneChangeDirection; + } +``` +to: +```capnp + struct MetaData { + laneChangeState @0 :LaneChangeState; + laneChangeDirection @1 :LaneChangeDirection; + laneChangeFlashesRemaining @2 :UInt8; + } +``` + +- [ ] **Step 2: Verify the schema compiles** + +Run: `python -c "from cereal import log; m = log.ModelDataV2.new_message(); m.meta.laneChangeFlashesRemaining = 7; print(m.meta.laneChangeFlashesRemaining)"` +Expected: prints `7` with no schema error. + +- [ ] **Step 3: Commit** + +```bash +git add cereal/log.capnp +git commit -m "cereal: add laneChangeFlashesRemaining to ModelDataV2.MetaData" +``` + +--- + +## Task 2: Flash-counted arming latch in `desire_helper.py` + +**Files:** +- Modify: `selfdrive/controls/lib/desire_helper.py` +- Create: `selfdrive/controls/lib/tests/test_desire_helper.py` + +Behavior (from spec): arm `preLaneChange` on a `one_blinker` rising edge; count rising edges of the armed-direction blinker; expose `flashes_remaining = max(7 - seen, 0)`; the latch does NOT require `one_blinker` to stay true; exit to `off` when the budget is exhausted with no nudge, or on opposite-direction tap, or on the existing safety exits; exit to `laneChangeStarting` on wheel nudge. + +- [ ] **Step 1: Create the test directory init (if missing)** + +Run: `python -c "import os; os.makedirs('selfdrive/controls/lib/tests', exist_ok=True); open('selfdrive/controls/lib/tests/__init__.py','a').close(); print('ok')"` +Expected: prints `ok`. + +- [ ] **Step 2: Write the failing tests** + +Create `selfdrive/controls/lib/tests/test_desire_helper.py`: +```python +from cereal import log +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.desire_helper import ( + DesireHelper, LaneChangeState, LaneChangeDirection, LANE_CHANGE_FLASH_BUDGET, +) +from openpilot.common.constants import CV + + +class FakeCarState: + def __init__(self, v_ego=30.0, left=False, right=False, + steering_pressed=False, steering_torque=0.0, + left_blindspot=False, right_blindspot=False): + self.vEgo = v_ego + self.leftBlinker = left + self.rightBlinker = right + self.steeringPressed = steering_pressed + self.steeringTorque = steering_torque + self.leftBlindspot = left_blindspot + self.rightBlindspot = right_blindspot + + +def _pulse_blinker(dh, cs, on): + """Drive one blinker on/off transition through update() to count a flash.""" + cs.leftBlinker = on and (dh.lane_change_direction == LaneChangeDirection.left) + cs.rightBlinker = on and (dh.lane_change_direction == LaneChangeDirection.right) + dh.update(cs, lateral_active=True, lane_change_prob=0.0) + + +def test_tap_arms_pre_lane_change(): + dh = DesireHelper() + # rising edge of one_blinker (a tap) + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + assert dh.lane_change_state == LaneChangeState.preLaneChange + assert dh.lane_change_direction == LaneChangeDirection.left + + +def test_latch_survives_blinker_off(): + # Core fix: after the tap, blinker physically goes off but arming persists. + dh = DesireHelper() + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + # blinker now off (stock 3-flash ended), no wheel nudge + dh.update(FakeCarState(left=False), True, 0.0) + assert dh.lane_change_state == LaneChangeState.preLaneChange + + +def test_flashes_remaining_counts_down(): + dh = DesireHelper() + dh.update(FakeCarState(left=False), True, 0.0) + cs = FakeCarState(left=True) + dh.update(cs, True, 0.0) # enter preLaneChange, first rising edge = 1 flash + assert dh.flashes_remaining == LANE_CHANGE_FLASH_BUDGET - 1 + # toggle blinker off then on -> second flash + _pulse_blinker(dh, cs, False) + _pulse_blinker(dh, cs, True) + assert dh.flashes_remaining == LANE_CHANGE_FLASH_BUDGET - 2 + + +def test_cancels_when_budget_exhausted_without_nudge(): + dh = DesireHelper() + dh.update(FakeCarState(left=False), True, 0.0) + cs = FakeCarState(left=True) + dh.update(cs, True, 0.0) + # Generate 7 flashes with no wheel nudge + for _ in range(LANE_CHANGE_FLASH_BUDGET): + _pulse_blinker(dh, cs, False) + _pulse_blinker(dh, cs, True) + assert dh.lane_change_state == LaneChangeState.off + assert dh.flashes_remaining == 0 + + +def test_wheel_nudge_starts_lane_change(): + dh = DesireHelper() + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + # driver nudges wheel left (positive torque, pressed) while armed left + nudge = FakeCarState(left=True, steering_pressed=True, steering_torque=1.0) + dh.update(nudge, True, 0.0) + assert dh.lane_change_state == LaneChangeState.laneChangeStarting + + +def test_opposite_tap_cancels(): + dh = DesireHelper() + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) # armed left + # opposite tap: right blinker rising edge + dh.update(FakeCarState(left=False, right=False), True, 0.0) # blinker off + dh.update(FakeCarState(right=True), True, 0.0) # right rising edge + assert dh.lane_change_state == LaneChangeState.off + + +def test_below_speed_does_not_arm(): + dh = DesireHelper() + slow = 10 * CV.MPH_TO_MS + dh.update(FakeCarState(v_ego=slow, left=False), True, 0.0) + dh.update(FakeCarState(v_ego=slow, left=True), True, 0.0) + assert dh.lane_change_state == LaneChangeState.off +``` + +- [ ] **Step 3: Run tests to verify they fail** + +Run: `python -m pytest selfdrive/controls/lib/tests/test_desire_helper.py -v` +Expected: FAIL — `ImportError: cannot import name 'LANE_CHANGE_FLASH_BUDGET'` (and others). + +- [ ] **Step 4: Rewrite `desire_helper.py` with the latch** + +Replace the entire body of `selfdrive/controls/lib/desire_helper.py` with: +```python +from cereal import log +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL + +LaneChangeState = log.LaneChangeState +LaneChangeDirection = log.LaneChangeDirection + +LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS +LANE_CHANGE_TIME_MAX = 10. + +# Total indicator flashes allowed in the pre-nudge arming window (stock 3 + 4 +# openpilot-sustained). When the count reaches this with no wheel nudge, arming +# cancels. The flash count also drives the "Nudge wheel within N signals" UI. +LANE_CHANGE_FLASH_BUDGET = 7 + +DESIRES = { + LaneChangeDirection.none: { + LaneChangeState.off: log.Desire.none, + LaneChangeState.preLaneChange: log.Desire.none, + LaneChangeState.laneChangeStarting: log.Desire.none, + LaneChangeState.laneChangeFinishing: log.Desire.none, + }, + LaneChangeDirection.left: { + LaneChangeState.off: log.Desire.none, + LaneChangeState.preLaneChange: log.Desire.none, + LaneChangeState.laneChangeStarting: log.Desire.laneChangeLeft, + LaneChangeState.laneChangeFinishing: log.Desire.laneChangeLeft, + }, + LaneChangeDirection.right: { + LaneChangeState.off: log.Desire.none, + LaneChangeState.preLaneChange: log.Desire.none, + LaneChangeState.laneChangeStarting: log.Desire.laneChangeRight, + LaneChangeState.laneChangeFinishing: log.Desire.laneChangeRight, + }, +} + + +class DesireHelper: + def __init__(self): + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + self.lane_change_timer = 0.0 + self.lane_change_ll_prob = 1.0 + self.keep_pulse_timer = 0.0 + self.prev_one_blinker = False + self.desire = log.Desire.none + + # Flash-counted arming latch + self.lane_change_flashes_seen = 0 + self.flashes_remaining = LANE_CHANGE_FLASH_BUDGET + self.prev_blinker_on = False + + @staticmethod + def get_lane_change_direction(CS): + return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right + + def _armed_blinker_on(self, carstate): + if self.lane_change_direction == LaneChangeDirection.left: + return carstate.leftBlinker + if self.lane_change_direction == LaneChangeDirection.right: + return carstate.rightBlinker + return False + + def update(self, carstate, lateral_active, lane_change_prob): + v_ego = carstate.vEgo + one_blinker = carstate.leftBlinker != carstate.rightBlinker + below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + + if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + else: + # LaneChangeState.off + if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + self.lane_change_state = LaneChangeState.preLaneChange + self.lane_change_ll_prob = 1.0 + self.lane_change_direction = self.get_lane_change_direction(carstate) + # Reset the flash latch for a fresh arming window. + self.lane_change_flashes_seen = 0 + self.prev_blinker_on = False + + # LaneChangeState.preLaneChange + elif self.lane_change_state == LaneChangeState.preLaneChange: + torque_applied = carstate.steeringPressed and \ + ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or + (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) + + blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or + (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) + + # Opposite-direction tap cancels arming (driver must re-tap to re-arm). + opposite_tap = one_blinker and not self.prev_one_blinker and \ + self.get_lane_change_direction(carstate) != self.lane_change_direction + + # Count a flash on each rising edge of the armed-direction indicator. + blinker_on = self._armed_blinker_on(carstate) + if blinker_on and not self.prev_blinker_on: + self.lane_change_flashes_seen += 1 + self.prev_blinker_on = blinker_on + + flashes_left = max(LANE_CHANGE_FLASH_BUDGET - self.lane_change_flashes_seen, 0) + + if below_lane_change_speed or opposite_tap: + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + elif torque_applied and not blindspot_detected: + self.lane_change_state = LaneChangeState.laneChangeStarting + elif flashes_left <= 0: + # Budget exhausted with no wheel nudge — cancel. + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + + # LaneChangeState.laneChangeStarting + elif self.lane_change_state == LaneChangeState.laneChangeStarting: + self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0) + if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: + self.lane_change_state = LaneChangeState.laneChangeFinishing + + # LaneChangeState.laneChangeFinishing + elif self.lane_change_state == LaneChangeState.laneChangeFinishing: + self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) + if self.lane_change_ll_prob > 0.99: + self.lane_change_direction = LaneChangeDirection.none + if one_blinker: + self.lane_change_state = LaneChangeState.preLaneChange + else: + self.lane_change_state = LaneChangeState.off + + # Publish remaining flashes (only meaningful in preLaneChange; full otherwise). + if self.lane_change_state == LaneChangeState.preLaneChange: + self.flashes_remaining = max(LANE_CHANGE_FLASH_BUDGET - self.lane_change_flashes_seen, 0) + else: + self.flashes_remaining = LANE_CHANGE_FLASH_BUDGET + self.lane_change_flashes_seen = 0 + self.prev_blinker_on = False + + if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange): + self.lane_change_timer = 0.0 + else: + self.lane_change_timer += DT_MDL + + self.prev_one_blinker = one_blinker + + self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + + # Send keep pulse once per second during LaneChangeState.preLaneChange + if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting): + self.keep_pulse_timer = 0.0 + elif self.lane_change_state == LaneChangeState.preLaneChange: + self.keep_pulse_timer += DT_MDL + if self.keep_pulse_timer > 1.0: + self.keep_pulse_timer = 0.0 + elif self.desire in (log.Desire.keepLeft, log.Desire.keepRight): + self.desire = log.Desire.none +``` + +- [ ] **Step 5: Run tests to verify they pass** + +Run: `python -m pytest selfdrive/controls/lib/tests/test_desire_helper.py -v` +Expected: PASS (all 7 tests). + +- [ ] **Step 6: Commit** + +```bash +git add selfdrive/controls/lib/desire_helper.py selfdrive/controls/lib/tests/ +git commit -m "desire_helper: flash-counted lane-change arming latch" +``` + +--- + +## Task 3: Publish `flashes_remaining` from modeld + +**Files:** +- Modify: `selfdrive/modeld/modeld.py` (~line 402-405) + +- [ ] **Step 1: Add the publish lines** + +In `selfdrive/modeld/modeld.py`, after the existing block: +```python + modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state + modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction +``` +add: +```python + modelv2_send.modelV2.meta.laneChangeFlashesRemaining = DH.flashes_remaining +``` + +- [ ] **Step 2: Verify it imports/parses** + +Run: `python -c "import ast; ast.parse(open('selfdrive/modeld/modeld.py').read()); print('ok')"` +Expected: prints `ok`. + +- [ ] **Step 3: Commit** + +```bash +git add selfdrive/modeld/modeld.py +git commit -m "modeld: publish laneChangeFlashesRemaining" +``` + +--- + +## Task 4: Dynamic countdown alert in events.py + +**Files:** +- Modify: `selfdrive/selfdrived/events.py` (alert callbacks region ~line 245; event dict ~line 578) + +- [ ] **Step 1: Add alert callback functions** + +In `selfdrive/selfdrived/events.py`, near the other alert-callback functions (after `below_steer_speed_alert`, ~line 255), add: +```python +def pre_lane_change_left_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + n = sm['modelV2'].meta.laneChangeFlashesRemaining + return Alert( + f"Nudge wheel left to change lane within {n} signals", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1) + + +def pre_lane_change_right_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + n = sm['modelV2'].meta.laneChangeFlashesRemaining + return Alert( + f"Nudge wheel right to change lane within {n} signals", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1) +``` + +- [ ] **Step 2: Wire the callbacks into the event dict** + +In `selfdrive/selfdrived/events.py`, replace: +```python + EventName.preLaneChangeLeft: { + ET.WARNING: Alert( + "Steer Left to Start Lane Change Once Safe", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.preLaneChangeRight: { + ET.WARNING: Alert( + "Steer Right to Start Lane Change Once Safe", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + }, +``` +with: +```python + EventName.preLaneChangeLeft: { + ET.WARNING: pre_lane_change_left_alert, + }, + + EventName.preLaneChangeRight: { + ET.WARNING: pre_lane_change_right_alert, + }, +``` + +- [ ] **Step 3: Verify it parses and the callback type is accepted** + +Run: `python -c "from openpilot.selfdrive.selfdrived import events; print('ok')"` +Expected: prints `ok` (no import/type error). + +- [ ] **Step 4: Commit** + +```bash +git add selfdrive/selfdrived/events.py +git commit -m "events: dynamic 'Nudge wheel within N signals' countdown alert" +``` + +--- + +## Task 5: `create_body_controls_message` in opendbc (Pre-AP teslacan) + +**Files (inside `opendbc_repo`):** +- Modify: `opendbc/car/tesla/preap/teslacan.py` +- Create: `opendbc/car/tesla/preap/tests/test_body_controls.py` + +All commands in this task run from `opendbc_repo/`. + +- [ ] **Step 1: Write the failing test** + +Create `opendbc_repo/opendbc/car/tesla/preap/tests/test_body_controls.py`: +```python +"""Frame-level invariants for NAP Pre-AP DAS_bodyControls (turn signal drive).""" +from opendbc.can import CANPacker +from opendbc.car.tesla.preap.teslacan import TeslaCANPreAP +from opendbc.car.tesla.values import CANBUS + + +def _tc(): + packer = CANPacker("tesla_preap") + return TeslaCANPreAP({CANBUS.party: packer, CANBUS.autopilot_party: packer}) + + +def test_addr_is_body_controls(): + tc = _tc() + addr, _, _ = tc.create_body_controls_message(1, 0, CANBUS.party, 1) + assert addr == 0x3E9 # DAS_bodyControls / 1001 + + +def test_turn_left_sets_indicator_left(): + tc = _tc() + packer = CANPacker("tesla_preap") + addr, dat, _ = tc.create_body_controls_message(1, 0, CANBUS.party, 1) + vals = packer.unpack(addr, dat) if hasattr(packer, "unpack") else None + # DAS_turnIndicatorRequest is at bit 8 (byte 1, bits 0-1) + assert dat[1] & 0x03 == 1 + + +def test_turn_right_sets_indicator_right(): + tc = _tc() + _, dat, _ = tc.create_body_controls_message(2, 0, CANBUS.party, 1) + assert dat[1] & 0x03 == 2 + + +def test_turn_none_sets_indicator_none(): + tc = _tc() + _, dat, _ = tc.create_body_controls_message(0, 0, CANBUS.party, 1) + assert dat[1] & 0x03 == 0 + + +def test_reason_set_when_turning(): + tc = _tc() + # DAS_turnIndicatorRequestReason at bit 16 (byte 2, bits 0-3) + _, dat_on, _ = tc.create_body_controls_message(1, 0, CANBUS.party, 1) + _, dat_off, _ = tc.create_body_controls_message(0, 0, CANBUS.party, 1) + assert dat_on[2] & 0x0F == 1 + assert dat_off[2] & 0x0F == 0 +``` + +- [ ] **Step 2: Run test to verify it fails** + +Run (from `opendbc_repo/`): `python -m pytest opendbc/car/tesla/preap/tests/test_body_controls.py -v` +Expected: FAIL — `AttributeError: 'TeslaCANPreAP' object has no attribute 'create_body_controls_message'`. + +- [ ] **Step 3: Implement `create_body_controls_message`** + +In `opendbc_repo/opendbc/car/tesla/preap/teslacan.py`, add this method to the `TeslaCANPreAP` class (after `create_action_request`): +```python + def create_body_controls_message(self, turn, hazard, bus, counter): + """Build DAS_bodyControls (0x3E9) to drive the turn indicator. + + turn: 0=none, 1=left, 2=right (matches CC.rightBlinker*2 + CC.leftBlinker). + Reason=1 (DAS_ACTIVE_NAV_LANE_CHANGE) when turning, 0 otherwise. This is the + proven Tinkla/Tesla-Unity Pre-AP blinker mechanism. + """ + values = { + "DAS_headlightRequest": 0, + "DAS_hazardLightRequest": hazard, + "DAS_wiperSpeed": 0, + "DAS_turnIndicatorRequest": turn, + "DAS_highLowBeamDecision": 0, + "DAS_highLowBeamOffReason": 0, + "DAS_turnIndicatorRequestReason": 1 if turn > 0 else 0, + "DAS_bodyControlsCounter": counter, + "DAS_bodyControlsChecksum": 0, + } + data = self.packers[CANBUS.party].make_can_msg("DAS_bodyControls", bus, values)[1] + values["DAS_bodyControlsChecksum"] = self.checksum(0x3E9, data[:7]) + return self.packers[CANBUS.party].make_can_msg("DAS_bodyControls", bus, values) +``` + +- [ ] **Step 4: Run test to verify it passes** + +Run (from `opendbc_repo/`): `python -m pytest opendbc/car/tesla/preap/tests/test_body_controls.py -v` +Expected: PASS (all 5 tests). + +- [ ] **Step 5: Commit (inside submodule)** + +```bash +cd opendbc_repo +git add opendbc/car/tesla/preap/teslacan.py opendbc/car/tesla/preap/tests/test_body_controls.py +git commit -m "tesla preap: create_body_controls_message for turn-signal drive" +cd .. +``` + +--- + +## Task 6: Consume blinker flags in Pre-AP carcontroller + +**Files (inside `opendbc_repo`):** +- Modify: `opendbc/car/tesla/preap/carcontroller.py` (the `_update_preap` method in `tesla/carcontroller.py` calls into this — confirm path) — actually modify `opendbc/car/tesla/carcontroller.py` `_update_preap`. + +> **Path note:** The Pre-AP carcontroller update lives in `opendbc/car/tesla/carcontroller.py::CarController._update_preap`. `preap/carcontroller.py` holds `PreAPLongController`. The blinker send goes in `_update_preap`, using `self.tesla_can.create_body_controls_message` (the `tesla_can` is a `TeslaCANPreAP` instance per `init_preap_can`). + +- [ ] **Step 1: Add the blinker send in `_update_preap`** + +In `opendbc_repo/opendbc/car/tesla/carcontroller.py`, inside `_update_preap`, after the `self.stock_cc.update(...)` block and before `new_actuators = actuators.as_builder()`, add: +```python + # Turn-signal drive: keep the indicator flashing during the lane-change + # arming window and maneuver. CC.leftBlinker/rightBlinker are set by + # controlsd whenever laneChangeState != off. 0=none, 1=left, 2=right. + if self.frame % 10 == 0: + turn = int(CC.rightBlinker) * 2 + int(CC.leftBlinker) + cntr = (self.frame // 10) % 16 + can_sends.append(self.tesla_can.create_body_controls_message(turn, 0, CANBUS.party, cntr)) +``` + +- [ ] **Step 2: Verify it parses** + +Run (from `opendbc_repo/`): `python -c "import ast; ast.parse(open('opendbc/car/tesla/carcontroller.py').read()); print('ok')"` +Expected: prints `ok`. + +- [ ] **Step 3: Add a carcontroller integration test** + +The existing `test_stock_cc_spoofer.py` constructs the spoofer directly with +`SimpleNamespace`/`MagicMock` stubs rather than a full `CarController`. Follow that +lighter style: assert the builder contract and the send cadence logic. Append to +`opendbc_repo/opendbc/car/tesla/preap/tests/test_body_controls.py`: +```python +def test_turn_value_encoding_matches_cc_convention(): + # turn = rightBlinker*2 + leftBlinker, as used in _update_preap. + assert (int(True) * 2 + int(False)) == 2 # right only + assert (int(False) * 2 + int(True)) == 1 # left only + assert (int(False) * 2 + int(False)) == 0 # none + + +def test_body_controls_frame_addr_and_bus(): + tc = _tc() + addr, _, bus = tc.create_body_controls_message(1, 0, CANBUS.party, 3) + assert addr == 0x3E9 + assert bus == CANBUS.party +``` + +> **Why not build the full `CarController` here:** it needs DBC names + packers and +> is exercised end-to-end by process-replay (Task 8 regression). This unit test +> pins the builder contract and the `turn` encoding. If you prefer a fuller test, +> the construction pattern is in `test_stock_cc_spoofer.py` — but the lighter test +> is sufficient and matches the file's established style. + +- [ ] **Step 4: Run tests** + +Run (from `opendbc_repo/`): `python -m pytest opendbc/car/tesla/preap/tests/test_body_controls.py -v` +Expected: PASS. + +- [ ] **Step 5: Commit (inside submodule)** + +```bash +cd opendbc_repo +git add opendbc/car/tesla/carcontroller.py opendbc/car/tesla/preap/tests/test_body_controls.py +git commit -m "tesla preap: drive turn signal via DAS_bodyControls from CC blinker flags" +cd .. +``` + +--- + +## Task 7: Panda safety — allowlist 0x3E9 and bound turn value + +**Files (inside `opendbc_repo`):** +- Modify: `opendbc/safety/modes/tesla_preap.h` +- Modify: `opendbc/safety/tests/test_tesla_preap.py` + +- [ ] **Step 1: Add the failing safety test** + +In `opendbc_repo/opendbc/safety/tests/test_tesla_preap.py`, add `[0x3E9, 0]` to the `TX_MSGS` list: +```python + TX_MSGS = [ + [0x488, 0], # DAS_steeringControl + [0x2B9, 0], # DAS_control + [0x214, 0], # EPB_epasControl + [0x551, 0], # Pedal + [0x551, 2], # Pedal + [0x45, 0], # STW_ACTN_RQ (stalk spoof) + [0x3E9, 0], # DAS_bodyControls (turn signal) + ] +``` +(Match the existing entries already present; only the `0x3E9` line is new. If `0x214`/`0x551` rows differ, keep the file's existing rows and just append the `0x3E9` row.) + +Then add a test method inside the `TeslaPreAPTestMixin` class (the harness exposes +`self.packer.make_can_msg_safety(name, bus, values)` and `self._tx(msg)` from +`opendbc/safety/tests/common.py`): +```python + def test_body_controls_turn_indicator_allowed(self): + # Valid turn-indicator requests (0-3) are allowed when controls_allowed. + self.safety.set_controls_allowed(True) + for turn in range(4): + msg = self.packer.make_can_msg_safety("DAS_bodyControls", 0, + {"DAS_turnIndicatorRequest": turn}) + self.assertTrue(self._tx(msg), f"turn={turn} should be allowed") + + def test_body_controls_blocked_when_not_allowed(self): + # Like other actuation, blocked when controls are not allowed. + self.safety.set_controls_allowed(False) + msg = self.packer.make_can_msg_safety("DAS_bodyControls", 0, + {"DAS_turnIndicatorRequest": 1}) + self.assertFalse(self._tx(msg)) +``` + +> **controls_allowed note:** the second test assumes the framework gates 0x3E9 on +> `controls_allowed` (as it does for other Pre-AP TX). If `DAS_bodyControls` should +> be allowed even when disengaged (so the indicator can be cleared on disengage), +> drop `test_body_controls_blocked_when_not_allowed` and instead assert it is always +> allowed. Decide based on desired UX: blinker should stop on disengage, and +> `controlsd` already sets `CC.*Blinker=False` then, so gating on controls_allowed +> is the safe default — keep both tests. + +- [ ] **Step 2: Run test to verify it fails** + +Run (from `opendbc_repo/`): `python -m pytest opendbc/safety/tests/test_tesla_preap.py -v -k body_controls` +Expected: FAIL — 0x3E9 not in TX allowlist (tx rejected). + +> If the safety C must be rebuilt first, run `scons -j8 opendbc/safety` (or the repo's documented safety build) before pytest. + +- [ ] **Step 3: Add 0x3E9 to the TX allowlist** + +In `opendbc_repo/opendbc/safety/modes/tesla_preap.h`, in `PREAP_TX_MSGS`, add after the `0x45` line: +```c + {0x3E9, 0, 8, .check_relay = false, .disable_static_blocking = true}, // DAS_bodyControls (turn signal) +``` + +- [ ] **Step 4: Bound the turn-indicator value in the TX hook** + +In `tesla_preap_tx_hook`, before the `if (violation)` block, add: +```c + // DAS_bodyControls (0x3E9): only allow valid turn-indicator requests (0-3). + if (msg->addr == 0x3E9U) { + int turn_req = (msg->data[1] & 0x03U); // DAS_turnIndicatorRequest at bit 8 + if (turn_req > 3) { + violation = true; // unreachable for 2 bits, but explicit per defense-in-depth + } + } +``` + +> The 2-bit field can't exceed 3, so this is documentation-of-intent + a guard if the field width ever changes. Keeping it makes the safety reviewer's job easy. + +- [ ] **Step 5: Rebuild safety and run the test** + +Run (from `opendbc_repo/`): `scons -j8 opendbc/safety && python -m pytest opendbc/safety/tests/test_tesla_preap.py -v -k body_controls` +Expected: PASS. + +- [ ] **Step 6: Run the full Pre-AP safety suite (regression)** + +Run (from `opendbc_repo/`): `python -m pytest opendbc/safety/tests/test_tesla_preap.py -v` +Expected: PASS (all, including pre-existing tests). + +- [ ] **Step 7: Commit (inside submodule)** + +```bash +cd opendbc_repo +git add opendbc/safety/modes/tesla_preap.h opendbc/safety/tests/test_tesla_preap.py +git commit -m "safety tesla_preap: allow DAS_bodyControls (0x3E9) turn-signal TX" +cd .. +``` + +--- + +## Task 8: Bump submodule pointer + full regression + +**Files:** +- Modify: parent repo's `opendbc_repo` submodule reference + +- [ ] **Step 1: Record the new submodule SHA in the parent** + +```bash +git add opendbc_repo +git status # confirm: "modified: opendbc_repo (new commits)" +``` + +- [ ] **Step 2: Run the desire_helper + events tests once more from parent** + +Run: `python -m pytest selfdrive/controls/lib/tests/test_desire_helper.py -v` +Expected: PASS. + +- [ ] **Step 3: Sanity-check the cereal field round-trips through modeld's message** + +Run: `python -c "from cereal import log; m=log.ModelDataV2.new_message(); m.meta.laneChangeFlashesRemaining=7; assert m.meta.laneChangeFlashesRemaining==7; print('ok')"` +Expected: prints `ok`. + +- [ ] **Step 4: Commit the submodule bump** + +```bash +git commit -m "bump opendbc_repo: lane-change auto-signal (DAS_bodyControls turn drive)" +``` + +--- + +## Self-Review Notes + +- **Spec coverage:** tap-arm (Task 2), 7-flash sustain via DAS_bodyControls (Tasks 5-6), auto-stop on completion (Task 6 — `turn` follows `CC.*Blinker` which controlsd drops at `off`), countdown UI (Tasks 1,3,4), timeout cancel (Task 2), opposite-tap cancel (Task 2), safety allowlist (Task 7). All covered. +- **Type consistency:** `flashes_remaining` (int, 0-7) defined in Task 2, published in Task 3, read in Task 4, schema `UInt8` in Task 1 — consistent. `turn` value convention `right*2+left` consistent across Tasks 5/6 and the test. +- **Known soft spots flagged inline:** the carcontroller integration test (Task 6) and the safety test helper (Task 7) note where to mirror existing repo harness patterns if the simple form doesn't match. These are real verification steps, not placeholders — the executing agent confirms the harness on the spot. From 584916199a44d84b7a40a041b6e595641850cb41 Mon Sep 17 00:00:00 2001 From: Jacob <144983089+SloPOS@users.noreply.github.com> Date: Thu, 25 Jun 2026 09:54:40 -0400 Subject: [PATCH 03/11] cereal: add laneChangeFlashesRemaining to ModelDataV2.MetaData New UInt8 field (@10) carries the remaining-flashes countdown from desire_helper to the UI alert layer. Co-Authored-By: Claude Opus 4.8 --- cereal/log.capnp | 1 + 1 file changed, 1 insertion(+) diff --git a/cereal/log.capnp b/cereal/log.capnp index 327cdfb5388f92..760e2c04ed6b6c 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1145,6 +1145,7 @@ struct ModelDataV2 { hardBrakePredicted @7 :Bool; laneChangeState @8 :LaneChangeState; laneChangeDirection @9 :LaneChangeDirection; + laneChangeFlashesRemaining @10 :UInt8; # deprecated From d73cd29403f5581c8eb4887bac2bf1287114748d Mon Sep 17 00:00:00 2001 From: Jacob <144983089+SloPOS@users.noreply.github.com> Date: Thu, 25 Jun 2026 10:00:44 -0400 Subject: [PATCH 04/11] desire_helper: flash-counted lane-change arming latch Short stalk tap arms preLaneChange and the latch now persists without the driver holding the stalk. Counts indicator flashes (rising edges of the armed-direction blinker) up to LANE_CHANGE_FLASH_BUDGET=7, exposing flashes_remaining for the UI countdown. Exits: wheel nudge -> start, budget exhausted -> cancel, opposite tap -> cancel. Co-Authored-By: Claude Opus 4.8 --- selfdrive/controls/lib/desire_helper.py | 53 +++++++++- selfdrive/controls/lib/tests/__init__.py | 0 .../controls/lib/tests/test_desire_helper.py | 96 +++++++++++++++++++ 3 files changed, 144 insertions(+), 5 deletions(-) create mode 100644 selfdrive/controls/lib/tests/__init__.py create mode 100644 selfdrive/controls/lib/tests/test_desire_helper.py diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index ee4567f1e988e3..dc852232821920 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -8,6 +8,11 @@ LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. +# Total indicator flashes allowed in the pre-nudge arming window (stock 3 + 4 +# openpilot-sustained). When the count reaches this with no wheel nudge, arming +# cancels. The flash count also drives the "Nudge wheel within N signals" UI. +LANE_CHANGE_FLASH_BUDGET = 7 + DESIRES = { LaneChangeDirection.none: { LaneChangeState.off: log.Desire.none, @@ -40,10 +45,22 @@ def __init__(self): self.prev_one_blinker = False self.desire = log.Desire.none + # Flash-counted arming latch + self.lane_change_flashes_seen = 0 + self.flashes_remaining = LANE_CHANGE_FLASH_BUDGET + self.prev_blinker_on = False + @staticmethod def get_lane_change_direction(CS): return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right + def _armed_blinker_on(self, carstate): + if self.lane_change_direction == LaneChangeDirection.left: + return carstate.leftBlinker + if self.lane_change_direction == LaneChangeDirection.right: + return carstate.rightBlinker + return False + def update(self, carstate, lateral_active, lane_change_prob): v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker @@ -59,12 +76,14 @@ def update(self, carstate, lateral_active, lane_change_prob): self.lane_change_ll_prob = 1.0 # Initialize lane change direction to prevent UI alert flicker self.lane_change_direction = self.get_lane_change_direction(carstate) + # Start a fresh arming window. Entry is triggered by the blinker turning + # on, so that first flash counts and prev_blinker_on latches True to + # avoid double-counting it on the next tick. + self.lane_change_flashes_seen = 1 + self.prev_blinker_on = True # LaneChangeState.preLaneChange elif self.lane_change_state == LaneChangeState.preLaneChange: - # Update lane change direction - self.lane_change_direction = self.get_lane_change_direction(carstate) - torque_applied = carstate.steeringPressed and \ ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) @@ -72,11 +91,27 @@ def update(self, carstate, lateral_active, lane_change_prob): blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) - if not one_blinker or below_lane_change_speed: + # Opposite-direction tap cancels arming (driver must re-tap to re-arm). + opposite_tap = one_blinker and not self.prev_one_blinker and \ + self.get_lane_change_direction(carstate) != self.lane_change_direction + + # Count a flash on each rising edge of the armed-direction indicator. + blinker_on = self._armed_blinker_on(carstate) + if blinker_on and not self.prev_blinker_on: + self.lane_change_flashes_seen += 1 + self.prev_blinker_on = blinker_on + + flashes_left = max(LANE_CHANGE_FLASH_BUDGET - self.lane_change_flashes_seen, 0) + + if below_lane_change_speed or opposite_tap: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none elif torque_applied and not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting + elif flashes_left <= 0: + # Budget exhausted with no wheel nudge — cancel. + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none # LaneChangeState.laneChangeStarting elif self.lane_change_state == LaneChangeState.laneChangeStarting: @@ -99,6 +134,14 @@ def update(self, carstate, lateral_active, lane_change_prob): else: self.lane_change_state = LaneChangeState.off + # Publish remaining flashes (only meaningful in preLaneChange; full otherwise). + if self.lane_change_state == LaneChangeState.preLaneChange: + self.flashes_remaining = max(LANE_CHANGE_FLASH_BUDGET - self.lane_change_flashes_seen, 0) + else: + self.flashes_remaining = LANE_CHANGE_FLASH_BUDGET + self.lane_change_flashes_seen = 0 + self.prev_blinker_on = False + if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange): self.lane_change_timer = 0.0 else: @@ -108,7 +151,7 @@ def update(self, carstate, lateral_active, lane_change_prob): self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] - # Send keep pulse once per second during LaneChangeStart.preLaneChange + # Send keep pulse once per second during LaneChangeState.preLaneChange if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting): self.keep_pulse_timer = 0.0 elif self.lane_change_state == LaneChangeState.preLaneChange: diff --git a/selfdrive/controls/lib/tests/__init__.py b/selfdrive/controls/lib/tests/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/controls/lib/tests/test_desire_helper.py b/selfdrive/controls/lib/tests/test_desire_helper.py new file mode 100644 index 00000000000000..13a82d1bd5d177 --- /dev/null +++ b/selfdrive/controls/lib/tests/test_desire_helper.py @@ -0,0 +1,96 @@ +from openpilot.selfdrive.controls.lib.desire_helper import ( + DesireHelper, LaneChangeState, LaneChangeDirection, LANE_CHANGE_FLASH_BUDGET, +) +from openpilot.common.constants import CV + + +class FakeCarState: + def __init__(self, v_ego=30.0, left=False, right=False, + steering_pressed=False, steering_torque=0.0, + left_blindspot=False, right_blindspot=False): + self.vEgo = v_ego + self.leftBlinker = left + self.rightBlinker = right + self.steeringPressed = steering_pressed + self.steeringTorque = steering_torque + self.leftBlindspot = left_blindspot + self.rightBlindspot = right_blindspot + + +def _pulse_blinker(dh, cs, on): + """Drive one blinker on/off transition through update() to count a flash.""" + cs.leftBlinker = on and (dh.lane_change_direction == LaneChangeDirection.left) + cs.rightBlinker = on and (dh.lane_change_direction == LaneChangeDirection.right) + dh.update(cs, lateral_active=True, lane_change_prob=0.0) + + +def test_tap_arms_pre_lane_change(): + dh = DesireHelper() + # rising edge of one_blinker (a tap) + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + assert dh.lane_change_state == LaneChangeState.preLaneChange + assert dh.lane_change_direction == LaneChangeDirection.left + + +def test_latch_survives_blinker_off(): + # Core fix: after the tap, blinker physically goes off but arming persists. + dh = DesireHelper() + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + # blinker now off (stock 3-flash ended), no wheel nudge + dh.update(FakeCarState(left=False), True, 0.0) + assert dh.lane_change_state == LaneChangeState.preLaneChange + + +def test_flashes_remaining_counts_down(): + dh = DesireHelper() + dh.update(FakeCarState(left=False), True, 0.0) + cs = FakeCarState(left=True) + dh.update(cs, True, 0.0) # enter preLaneChange, first rising edge = 1 flash + assert dh.flashes_remaining == LANE_CHANGE_FLASH_BUDGET - 1 + # toggle blinker off then on -> second flash + _pulse_blinker(dh, cs, False) + _pulse_blinker(dh, cs, True) + assert dh.flashes_remaining == LANE_CHANGE_FLASH_BUDGET - 2 + + +def test_cancels_when_budget_exhausted_without_nudge(): + dh = DesireHelper() + dh.update(FakeCarState(left=False), True, 0.0) + cs = FakeCarState(left=True) + dh.update(cs, True, 0.0) + # Generate 7 flashes with no wheel nudge + for _ in range(LANE_CHANGE_FLASH_BUDGET): + _pulse_blinker(dh, cs, False) + _pulse_blinker(dh, cs, True) + assert dh.lane_change_state == LaneChangeState.off + assert dh.flashes_remaining == LANE_CHANGE_FLASH_BUDGET + + +def test_wheel_nudge_starts_lane_change(): + dh = DesireHelper() + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + # driver nudges wheel left (positive torque, pressed) while armed left + nudge = FakeCarState(left=True, steering_pressed=True, steering_torque=1.0) + dh.update(nudge, True, 0.0) + assert dh.lane_change_state == LaneChangeState.laneChangeStarting + + +def test_opposite_tap_cancels(): + dh = DesireHelper() + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) # armed left + # opposite tap: right blinker rising edge + dh.update(FakeCarState(left=False, right=False), True, 0.0) # blinker off + dh.update(FakeCarState(right=True), True, 0.0) # right rising edge + assert dh.lane_change_state == LaneChangeState.off + + +def test_below_speed_does_not_arm(): + dh = DesireHelper() + slow = 10 * CV.MPH_TO_MS + dh.update(FakeCarState(v_ego=slow, left=False), True, 0.0) + dh.update(FakeCarState(v_ego=slow, left=True), True, 0.0) + assert dh.lane_change_state == LaneChangeState.off From 2e2959e62e1463ce043ff74eb12a8ed3f5fe53be Mon Sep 17 00:00:00 2001 From: Jacob <144983089+SloPOS@users.noreply.github.com> Date: Thu, 25 Jun 2026 10:01:49 -0400 Subject: [PATCH 05/11] modeld: publish laneChangeFlashesRemaining on modelV2.meta Carries the DesireHelper flash countdown to selfdrived for the UI alert. Co-Authored-By: Claude Opus 4.8 --- selfdrive/modeld/modeld.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index bff59366d6ec4c..eb5d37a3332542 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -401,6 +401,7 @@ def main(demo=False): DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction + modelv2_send.modelV2.meta.laneChangeFlashesRemaining = DH.flashes_remaining drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction From 9e2b955d72f1128b364809db49b332c42e49a9ae Mon Sep 17 00:00:00 2001 From: Jacob <144983089+SloPOS@users.noreply.github.com> Date: Thu, 25 Jun 2026 10:06:31 -0400 Subject: [PATCH 06/11] events: dynamic 'Nudge wheel within N signals' lane-change countdown Replace the static preLaneChange alerts with callbacks that read modelV2.meta.laneChangeFlashesRemaining and render a live countdown. Co-Authored-By: Claude Opus 4.8 --- selfdrive/selfdrived/events.py | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 62475ab55a8cdc..f3380d8c1975c3 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -254,6 +254,24 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4) +def pre_lane_change_left_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + n = sm['modelV2'].meta.laneChangeFlashesRemaining + return Alert( + f"Nudge wheel left to change lane within {n} signals", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1) + + +def pre_lane_change_right_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + n = sm['modelV2'].meta.laneChangeFlashesRemaining + return Alert( + f"Nudge wheel right to change lane within {n} signals", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1) + + def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: first_word = 'Recalibrating' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating' return Alert( @@ -576,19 +594,11 @@ def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messagin }, EventName.preLaneChangeLeft: { - ET.WARNING: Alert( - "Steer Left to Start Lane Change Once Safe", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + ET.WARNING: pre_lane_change_left_alert, }, EventName.preLaneChangeRight: { - ET.WARNING: Alert( - "Steer Right to Start Lane Change Once Safe", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + ET.WARNING: pre_lane_change_right_alert, }, EventName.laneChangeBlocked: { From 80a2330bed75377f3b4f9b462a11ea59211c52b8 Mon Sep 17 00:00:00 2001 From: Jacob <144983089+SloPOS@users.noreply.github.com> Date: Thu, 25 Jun 2026 10:17:43 -0400 Subject: [PATCH 07/11] bump opendbc_repo: lane-change auto-signal (DAS_bodyControls turn drive) Pulls in the Pre-AP turn-signal drive: create_body_controls_message, carcontroller consumer of CC blinker flags, and the 0x3E9 safety allowlist. Co-Authored-By: Claude Opus 4.8 --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index f867b3a4817b01..ce2da3020364b3 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit f867b3a4817b01df7f69b7cf2057c4ff59371f6e +Subproject commit ce2da3020364b317b713b15c9df6439e00319c35 From 3eeebfe53d578b88448af6353d1e69e824c7a22f Mon Sep 17 00:00:00 2001 From: Jacob <144983089+SloPOS@users.noreply.github.com> Date: Thu, 25 Jun 2026 10:36:13 -0400 Subject: [PATCH 08/11] gitmodules: point opendbc at SloPOS fork lane-change-auto-signal branch So the auto-signal opendbc commits (DAS_bodyControls turn drive + 0x3E9 safety) resolve from a SloPOS-owned repo for anyone cloning this fork. Co-Authored-By: Claude Opus 4.8 --- .gitmodules | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index 7920ee32f04483..32d8321eb87749 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,8 +4,8 @@ branch = master-nap [submodule "opendbc"] path = opendbc_repo - url = https://github.com/NotAutopilot/opendbc.git - branch = nap-dev + url = https://github.com/SloPOS/opendbc.git + branch = lane-change-auto-signal [submodule "msgq"] path = msgq_repo url = ../../commaai/msgq.git From f8aa434499f950a304072cacd3a9e49da618ef33 Mon Sep 17 00:00:00 2001 From: Jacob <144983089+SloPOS@users.noreply.github.com> Date: Fri, 26 Jun 2026 10:37:55 -0400 Subject: [PATCH 09/11] desire_helper: time-based arming + multi lane-change queue MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace the flash-counted budget with a 7s time budget (LANE_CHANGE_ARM_TIME) so the countdown advances reliably — Pre-AP BC_indicatorLStatus does not see the op-driven DAS_bodyControls flashes, so a flash counter froze. Add a same-direction tap queue (cap 3): each queued change needs its own wheel nudge; between changes the signal stays on and a fresh 7s window opens. Full reset to off when the queue empties, the window times out, the driver taps the opposite direction, or on blindspot/below-speed/disengage — fixes the stuck countdown toast after a completed lane change. cereal: rename laneChangeFlashesRemaining -> laneChangeSignalsRemaining and add laneChangeRemaining (queued changes left). Co-Authored-By: Claude Opus 4.8 --- cereal/log.capnp | 3 +- selfdrive/controls/lib/desire_helper.py | 101 +++++----- .../controls/lib/tests/test_desire_helper.py | 184 +++++++++++++----- 3 files changed, 199 insertions(+), 89 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 760e2c04ed6b6c..ad2efb410d64a0 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1145,7 +1145,8 @@ struct ModelDataV2 { hardBrakePredicted @7 :Bool; laneChangeState @8 :LaneChangeState; laneChangeDirection @9 :LaneChangeDirection; - laneChangeFlashesRemaining @10 :UInt8; + laneChangeSignalsRemaining @10 :UInt8; + laneChangeRemaining @11 :UInt8; # deprecated diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index dc852232821920..eed5655bbab3d3 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -1,3 +1,5 @@ +import math + from cereal import log from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL @@ -8,10 +10,15 @@ LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. -# Total indicator flashes allowed in the pre-nudge arming window (stock 3 + 4 -# openpilot-sustained). When the count reaches this with no wheel nudge, arming -# cancels. The flash count also drives the "Nudge wheel within N signals" UI. -LANE_CHANGE_FLASH_BUDGET = 7 +# Time (seconds) the lane change stays armed waiting for a wheel nudge. The +# countdown shown in the UI is the ceil of the time remaining. Time-based rather +# than flash-based because on Pre-AP the indicator-light feedback +# (BC_indicatorLStatus) does not reflect openpilot-driven DAS_bodyControls +# flashes, so a flash counter never advances on its own. +LANE_CHANGE_ARM_TIME = 7.0 + +# Cap on how many same-direction lane changes can be queued from repeated taps. +MAX_QUEUED_LANE_CHANGES = 3 DESIRES = { LaneChangeDirection.none: { @@ -45,21 +52,27 @@ def __init__(self): self.prev_one_blinker = False self.desire = log.Desire.none - # Flash-counted arming latch - self.lane_change_flashes_seen = 0 - self.flashes_remaining = LANE_CHANGE_FLASH_BUDGET - self.prev_blinker_on = False + # Time-based arming latch + self.arm_timer = 0.0 + self.signals_remaining = math.ceil(LANE_CHANGE_ARM_TIME) + + # Multi-lane-change queue. queued_changes counts the lane changes still to + # perform (the one currently armed/in-progress plus any waiting). It is set + # from same-direction taps during the arming window (capped). lane_changes_ + # remaining is the UI value: how many are left *beyond* the current one. + self.queued_changes = 0 + self.lane_changes_remaining = 0 @staticmethod def get_lane_change_direction(CS): return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right - def _armed_blinker_on(self, carstate): - if self.lane_change_direction == LaneChangeDirection.left: - return carstate.leftBlinker - if self.lane_change_direction == LaneChangeDirection.right: - return carstate.rightBlinker - return False + def _reset(self): + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + self.arm_timer = 0.0 + self.queued_changes = 0 + self.lane_changes_remaining = 0 def update(self, carstate, lateral_active, lane_change_prob): v_ego = carstate.vEgo @@ -67,8 +80,7 @@ def update(self, carstate, lateral_active, lane_change_prob): below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: - self.lane_change_state = LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none + self._reset() else: # LaneChangeState.off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: @@ -76,11 +88,9 @@ def update(self, carstate, lateral_active, lane_change_prob): self.lane_change_ll_prob = 1.0 # Initialize lane change direction to prevent UI alert flicker self.lane_change_direction = self.get_lane_change_direction(carstate) - # Start a fresh arming window. Entry is triggered by the blinker turning - # on, so that first flash counts and prev_blinker_on latches True to - # avoid double-counting it on the next tick. - self.lane_change_flashes_seen = 1 - self.prev_blinker_on = True + # Start a fresh arming window. The first tap queues one change. + self.arm_timer = 0.0 + self.queued_changes = 1 # LaneChangeState.preLaneChange elif self.lane_change_state == LaneChangeState.preLaneChange: @@ -91,27 +101,24 @@ def update(self, carstate, lateral_active, lane_change_prob): blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) - # Opposite-direction tap cancels arming (driver must re-tap to re-arm). - opposite_tap = one_blinker and not self.prev_one_blinker and \ - self.get_lane_change_direction(carstate) != self.lane_change_direction + new_tap = one_blinker and not self.prev_one_blinker + same_dir_tap = new_tap and self.get_lane_change_direction(carstate) == self.lane_change_direction + opposite_tap = new_tap and self.get_lane_change_direction(carstate) != self.lane_change_direction - # Count a flash on each rising edge of the armed-direction indicator. - blinker_on = self._armed_blinker_on(carstate) - if blinker_on and not self.prev_blinker_on: - self.lane_change_flashes_seen += 1 - self.prev_blinker_on = blinker_on + # Same-direction tap queues another change (capped). + if same_dir_tap: + self.queued_changes = min(self.queued_changes + 1, MAX_QUEUED_LANE_CHANGES) - flashes_left = max(LANE_CHANGE_FLASH_BUDGET - self.lane_change_flashes_seen, 0) + # Count down the arming window. + self.arm_timer += DT_MDL if below_lane_change_speed or opposite_tap: - self.lane_change_state = LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none + self._reset() elif torque_applied and not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting - elif flashes_left <= 0: - # Budget exhausted with no wheel nudge — cancel. - self.lane_change_state = LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none + elif self.arm_timer > LANE_CHANGE_ARM_TIME: + # Window expired with no wheel nudge — cancel everything. + self._reset() # LaneChangeState.laneChangeStarting elif self.lane_change_state == LaneChangeState.laneChangeStarting: @@ -128,19 +135,25 @@ def update(self, carstate, lateral_active, lane_change_prob): self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) if self.lane_change_ll_prob > 0.99: - self.lane_change_direction = LaneChangeDirection.none - if one_blinker: + # One change just completed. + self.queued_changes = max(self.queued_changes - 1, 0) + if self.queued_changes > 0: + # More queued: keep the same direction and signal on, re-arm a fresh + # window and wait for the next wheel nudge. self.lane_change_state = LaneChangeState.preLaneChange + self.lane_change_ll_prob = 1.0 + self.arm_timer = 0.0 else: - self.lane_change_state = LaneChangeState.off + # Nothing left — full reset so the toast clears and no ALC re-arms. + self._reset() - # Publish remaining flashes (only meaningful in preLaneChange; full otherwise). + # Derived UI values. if self.lane_change_state == LaneChangeState.preLaneChange: - self.flashes_remaining = max(LANE_CHANGE_FLASH_BUDGET - self.lane_change_flashes_seen, 0) + self.signals_remaining = max(math.ceil(LANE_CHANGE_ARM_TIME - self.arm_timer), 0) else: - self.flashes_remaining = LANE_CHANGE_FLASH_BUDGET - self.lane_change_flashes_seen = 0 - self.prev_blinker_on = False + self.signals_remaining = math.ceil(LANE_CHANGE_ARM_TIME) + # How many lane changes remain beyond the one currently armed/in progress. + self.lane_changes_remaining = max(self.queued_changes - 1, 0) if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange): self.lane_change_timer = 0.0 diff --git a/selfdrive/controls/lib/tests/test_desire_helper.py b/selfdrive/controls/lib/tests/test_desire_helper.py index 13a82d1bd5d177..d5d3e0187125dc 100644 --- a/selfdrive/controls/lib/tests/test_desire_helper.py +++ b/selfdrive/controls/lib/tests/test_desire_helper.py @@ -1,5 +1,9 @@ +import math + +from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.lib.desire_helper import ( - DesireHelper, LaneChangeState, LaneChangeDirection, LANE_CHANGE_FLASH_BUDGET, + DesireHelper, LaneChangeState, LaneChangeDirection, + LANE_CHANGE_ARM_TIME, MAX_QUEUED_LANE_CHANGES, ) from openpilot.common.constants import CV @@ -17,80 +21,172 @@ def __init__(self, v_ego=30.0, left=False, right=False, self.rightBlindspot = right_blindspot -def _pulse_blinker(dh, cs, on): - """Drive one blinker on/off transition through update() to count a flash.""" - cs.leftBlinker = on and (dh.lane_change_direction == LaneChangeDirection.left) - cs.rightBlinker = on and (dh.lane_change_direction == LaneChangeDirection.right) - dh.update(cs, lateral_active=True, lane_change_prob=0.0) +def _tick(dh, cs, n=1, lane_change_prob=0.0): + for _ in range(n): + dh.update(cs, lateral_active=True, lane_change_prob=lane_change_prob) -def test_tap_arms_pre_lane_change(): - dh = DesireHelper() - # rising edge of one_blinker (a tap) +def _arm_left(dh): + """Drive a single left tap (rising edge of one_blinker) to arm preLaneChange.""" dh.update(FakeCarState(left=False), True, 0.0) dh.update(FakeCarState(left=True), True, 0.0) + + +def _nudge_left(): + return FakeCarState(left=True, steering_pressed=True, steering_torque=1.0) + + +# ---- arming ------------------------------------------------------------- + +def test_tap_arms_pre_lane_change(): + dh = DesireHelper() + _arm_left(dh) assert dh.lane_change_state == LaneChangeState.preLaneChange assert dh.lane_change_direction == LaneChangeDirection.left def test_latch_survives_blinker_off(): - # Core fix: after the tap, blinker physically goes off but arming persists. dh = DesireHelper() - dh.update(FakeCarState(left=False), True, 0.0) - dh.update(FakeCarState(left=True), True, 0.0) - # blinker now off (stock 3-flash ended), no wheel nudge + _arm_left(dh) + # blinker physically off, no nudge — still armed (well within 7s) dh.update(FakeCarState(left=False), True, 0.0) assert dh.lane_change_state == LaneChangeState.preLaneChange -def test_flashes_remaining_counts_down(): +def test_below_speed_does_not_arm(): dh = DesireHelper() - dh.update(FakeCarState(left=False), True, 0.0) - cs = FakeCarState(left=True) - dh.update(cs, True, 0.0) # enter preLaneChange, first rising edge = 1 flash - assert dh.flashes_remaining == LANE_CHANGE_FLASH_BUDGET - 1 - # toggle blinker off then on -> second flash - _pulse_blinker(dh, cs, False) - _pulse_blinker(dh, cs, True) - assert dh.flashes_remaining == LANE_CHANGE_FLASH_BUDGET - 2 + slow = 10 * CV.MPH_TO_MS + dh.update(FakeCarState(v_ego=slow, left=False), True, 0.0) + dh.update(FakeCarState(v_ego=slow, left=True), True, 0.0) + assert dh.lane_change_state == LaneChangeState.off + +# ---- time-based countdown ---------------------------------------------- -def test_cancels_when_budget_exhausted_without_nudge(): +def test_signals_remaining_starts_at_budget(): dh = DesireHelper() - dh.update(FakeCarState(left=False), True, 0.0) - cs = FakeCarState(left=True) - dh.update(cs, True, 0.0) - # Generate 7 flashes with no wheel nudge - for _ in range(LANE_CHANGE_FLASH_BUDGET): - _pulse_blinker(dh, cs, False) - _pulse_blinker(dh, cs, True) + _arm_left(dh) + assert dh.signals_remaining == math.ceil(LANE_CHANGE_ARM_TIME) + + +def test_signals_remaining_counts_down_with_time(): + dh = DesireHelper() + _arm_left(dh) + start = dh.signals_remaining + # advance ~2 seconds of ticks, blinker off the whole time (no edges) + _tick(dh, FakeCarState(left=False), n=int(2.0 / DT_MDL)) + assert dh.signals_remaining < start + assert dh.signals_remaining > 0 + + +def test_cancels_after_time_budget_without_nudge(): + dh = DesireHelper() + _arm_left(dh) + # advance past the 7s budget with no wheel nudge + _tick(dh, FakeCarState(left=False), n=int(LANE_CHANGE_ARM_TIME / DT_MDL) + 5) assert dh.lane_change_state == LaneChangeState.off - assert dh.flashes_remaining == LANE_CHANGE_FLASH_BUDGET + assert dh.lane_change_direction == LaneChangeDirection.none +# ---- nudge / exits ------------------------------------------------------ + def test_wheel_nudge_starts_lane_change(): dh = DesireHelper() - dh.update(FakeCarState(left=False), True, 0.0) - dh.update(FakeCarState(left=True), True, 0.0) - # driver nudges wheel left (positive torque, pressed) while armed left - nudge = FakeCarState(left=True, steering_pressed=True, steering_torque=1.0) - dh.update(nudge, True, 0.0) + _arm_left(dh) + dh.update(_nudge_left(), True, 0.0) assert dh.lane_change_state == LaneChangeState.laneChangeStarting def test_opposite_tap_cancels(): dh = DesireHelper() - dh.update(FakeCarState(left=False), True, 0.0) - dh.update(FakeCarState(left=True), True, 0.0) # armed left - # opposite tap: right blinker rising edge + _arm_left(dh) dh.update(FakeCarState(left=False, right=False), True, 0.0) # blinker off - dh.update(FakeCarState(right=True), True, 0.0) # right rising edge + dh.update(FakeCarState(right=True), True, 0.0) # opposite rising edge assert dh.lane_change_state == LaneChangeState.off + assert dh.lane_change_direction == LaneChangeDirection.none -def test_below_speed_does_not_arm(): +# ---- full reset after single change (stuck-toast fix) ------------------- + +def _complete_maneuver(dh, cs_after): + """Drive laneChangeStarting -> Finishing -> done. cs_after is the carstate + presented once the maneuver finishes (controls which post-state we land in).""" + # In starting: lane_change_prob low + ll_prob fades to push to finishing + _tick(dh, _nudge_left(), n=int(0.6 / DT_MDL), lane_change_prob=0.0) + assert dh.lane_change_state == LaneChangeState.laneChangeFinishing + # In finishing: ll_prob fades back in over ~1s + for _ in range(int(1.5 / DT_MDL)): + dh.update(cs_after, True, 0.0) + if dh.lane_change_state != LaneChangeState.laneChangeFinishing: + break + + +def test_single_change_resets_to_off_after_completion(): dh = DesireHelper() - slow = 10 * CV.MPH_TO_MS - dh.update(FakeCarState(v_ego=slow, left=False), True, 0.0) - dh.update(FakeCarState(v_ego=slow, left=True), True, 0.0) + _arm_left(dh) + dh.update(_nudge_left(), True, 0.0) + assert dh.lane_change_state == LaneChangeState.laneChangeStarting + # After maneuver, blinker is off (op stops driving it). Expect full reset. + _complete_maneuver(dh, FakeCarState(left=False)) + assert dh.lane_change_state == LaneChangeState.off + assert dh.lane_change_direction == LaneChangeDirection.none + assert dh.lane_changes_remaining == 0 + + +# ---- multi-lane-change queue ------------------------------------------- + +def test_double_tap_queues_two_changes(): + dh = DesireHelper() + # first tap arms + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + # second same-direction tap within window: blinker off then on again + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + assert dh.queued_changes == 2 + # one in progress + one remaining + assert dh.lane_changes_remaining == 1 + + +def test_queue_capped_at_three(): + dh = DesireHelper() + dh.update(FakeCarState(left=False), True, 0.0) + for _ in range(5): # 5 taps, cap is 3 + dh.update(FakeCarState(left=True), True, 0.0) + dh.update(FakeCarState(left=False), True, 0.0) + assert dh.queued_changes == MAX_QUEUED_LANE_CHANGES == 3 + + +def test_second_change_rearms_after_first_completes(): + dh = DesireHelper() + # queue two + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + assert dh.queued_changes == 2 + # nudge + complete first; signal stays on (left=True) between changes + dh.update(_nudge_left(), True, 0.0) + _complete_maneuver(dh, FakeCarState(left=True)) + # Should re-arm in same direction, NOT reset to off + assert dh.lane_change_state == LaneChangeState.preLaneChange + assert dh.lane_change_direction == LaneChangeDirection.left + assert dh.lane_changes_remaining == 0 # one left, now in the waiting window + assert dh.queued_changes == 1 + + +def test_midsequence_timeout_full_resets(): + dh = DesireHelper() + # queue two + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + dh.update(FakeCarState(left=False), True, 0.0) + dh.update(FakeCarState(left=True), True, 0.0) + dh.update(_nudge_left(), True, 0.0) + _complete_maneuver(dh, FakeCarState(left=True)) + assert dh.lane_change_state == LaneChangeState.preLaneChange + # now let the 2nd window time out with no nudge + _tick(dh, FakeCarState(left=True), n=int(LANE_CHANGE_ARM_TIME / DT_MDL) + 5) assert dh.lane_change_state == LaneChangeState.off + assert dh.lane_change_direction == LaneChangeDirection.none + assert dh.queued_changes == 0 From 0243166ab9e4e5bee3765320b19faeb76f3efa35 Mon Sep 17 00:00:00 2001 From: Jacob <144983089+SloPOS@users.noreply.github.com> Date: Fri, 26 Jun 2026 10:39:03 -0400 Subject: [PATCH 10/11] modeld/events: publish & display signals-remaining + queued lane changes modeld publishes the renamed laneChangeSignalsRemaining (time countdown) and laneChangeRemaining (queued changes). The preLaneChange alert shows the countdown and a second line "N more lane changes queued" when applicable. Co-Authored-By: Claude Opus 4.8 --- selfdrive/modeld/modeld.py | 3 ++- selfdrive/selfdrived/events.py | 23 +++++++++++++---------- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index eb5d37a3332542..d35bcfe934753a 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -401,7 +401,8 @@ def main(demo=False): DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction - modelv2_send.modelV2.meta.laneChangeFlashesRemaining = DH.flashes_remaining + modelv2_send.modelV2.meta.laneChangeSignalsRemaining = DH.signals_remaining + modelv2_send.modelV2.meta.laneChangeRemaining = DH.lane_changes_remaining drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index f3380d8c1975c3..0292ec341ce020 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -254,22 +254,25 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4) -def pre_lane_change_left_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - n = sm['modelV2'].meta.laneChangeFlashesRemaining +def _pre_lane_change_alert(side: str, sm: messaging.SubMaster) -> Alert: + meta = sm['modelV2'].meta + n = meta.laneChangeSignalsRemaining + remaining = meta.laneChangeRemaining + # Second line shows how many more lane changes are queued beyond this one. + line2 = f"{remaining} more lane change{'s' if remaining != 1 else ''} queued" if remaining > 0 else "" return Alert( - f"Nudge wheel left to change lane within {n} signals", - "", + f"Nudge wheel {side} to change lane within {n} signals", + line2, AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.none, .1) +def pre_lane_change_left_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + return _pre_lane_change_alert("left", sm) + + def pre_lane_change_right_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - n = sm['modelV2'].meta.laneChangeFlashesRemaining - return Alert( - f"Nudge wheel right to change lane within {n} signals", - "", - AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .1) + return _pre_lane_change_alert("right", sm) def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: From 288f90c27b7e5b08e980e6565716fefadf808920 Mon Sep 17 00:00:00 2001 From: Jacob <144983089+SloPOS@users.noreply.github.com> Date: Fri, 26 Jun 2026 10:40:41 -0400 Subject: [PATCH 11/11] =?UTF-8?q?docs:=20spec=20Rev=202=20=E2=80=94=20time?= =?UTF-8?q?=20budget,=20full=20reset,=20multi-change=20queue?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Opus 4.8 --- ...26-06-25-lane-change-auto-signal-design.md | 63 +++++++++++++++++++ 1 file changed, 63 insertions(+) diff --git a/docs/superpowers/specs/2026-06-25-lane-change-auto-signal-design.md b/docs/superpowers/specs/2026-06-25-lane-change-auto-signal-design.md index 5857986795998f..9274e8f5e19b73 100644 --- a/docs/superpowers/specs/2026-06-25-lane-change-auto-signal-design.md +++ b/docs/superpowers/specs/2026-06-25-lane-change-auto-signal-design.md @@ -27,6 +27,69 @@ Replicate the Tesla-Unity / Tinkla behavior: 5. The UI shows a live countdown: **"Nudge wheel to engage lane change within N signals"**. +--- + +## Revision 2 (2026-06-26) — time budget, full reset, multi-change queue + +On-car testing showed the original flash-counted design works to *hold* the +signal, but has three problems, all fixed here: + +1. **Countdown never advances.** On this Pre-AP build, `BC_indicatorLStatus` + reflects only the driver's physical stalk, NOT the openpilot-driven + `DAS_bodyControls` flashes. So the flash counter only ticks when the driver + moves the stalk — the countdown freezes at 7/6. + **Fix:** count down a **7-second time budget** (`LANE_CHANGE_ARM_TIME = 7.0`) + instead of flashes. The countdown is `ceil(time_remaining)` and ticks by + `DT_MDL` every frame, independent of any blinker hardware feedback. + +2. **Stuck toast after completion.** After the maneuver, `laneChangeFinishing` + re-enters `preLaneChange` on a phantom `one_blinker` (the op-driven blinker), + leaving the countdown toast stuck on screen until the driver cancels/disengages. + **Fix:** on maneuver completion with no queued changes remaining, force a + **full reset to `off`** (direction none, timers cleared, blinker dropped). + +3. **No multi-lane-change.** **New feature:** during the arming window, count + same-direction stalk taps. `queued_changes = min(taps, 3)`. Each lane change + needs its **own** wheel nudge. After one completes with more queued, the signal + **stays on**, the queue decrements, and a **fresh 7s window** opens for the next + nudge (the timer counts only while waiting for a nudge — it pauses during the + maneuver). A UI value shows "N lane changes left" (0–2). Full reset (signal off, + toast clears) on: queue exhausted, 7s expiring while waiting for a nudge, + opposite-direction tap, or blindspot/below-speed/disengage. + +### Cereal field changes (Rev 2) +- Rename `laneChangeFlashesRemaining @10` → `laneChangeSignalsRemaining @10` + (now the seconds-based countdown shown in the toast). +- Add `laneChangeRemaining @11 :UInt8` (queued lane changes left, 0–2 after the + first is in progress; up to 2 while waiting). + +### State machine (Rev 2) — `desire_helper.py` +- Constant: `LANE_CHANGE_ARM_TIME = 7.0` (replaces `LANE_CHANGE_FLASH_BUDGET`). +- New state: `arm_timer` (counts up while waiting for a nudge in `preLaneChange`), + `queued_changes` (taps seen, capped 3), `signals_remaining` (ceil of time left), + `lane_changes_remaining` (queue minus the one in progress). +- `preLaneChange` waiting: `arm_timer += DT_MDL`; cancel to full `off` when + `arm_timer > LANE_CHANGE_ARM_TIME`. Count same-direction taps into + `queued_changes` (cap 3). Opposite tap → full reset. Wheel nudge → + `laneChangeStarting` (consumes one from the queue; reset `arm_timer`). +- `laneChangeFinishing` completion: if `queued_changes` (remaining) > 0 → return + to `preLaneChange`, keep the same direction & signal on, open a fresh window + (`arm_timer = 0`). Else → **full `off` reset**. +- "Keep signal on between maneuvers": the between-change wait runs through + `preLaneChange` with the direction retained, so `laneChangeState != off` and + `controlsd` keeps asserting `CC.leftBlinker/rightBlinker`. + +### UI (Rev 2) +- Countdown alert reads `laneChangeSignalsRemaining`: "Nudge wheel {left/right} + to change lane within N signals". When `laneChangeRemaining > 0`, append the + queue count, e.g. a second line / suffix "(N more queued)". + +The carcontroller / teslacan / panda-safety layer is **unchanged** in Rev 2 — it +already drives the blinker from `CC.leftBlinker/rightBlinker`, which follow +`laneChangeState != off`. + +--- + ## Verified Mechanism (from reading the code + `origin/tesla-unity`) ### Blinker READ (already correct)