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 diff --git a/cereal/log.capnp b/cereal/log.capnp index 327cdfb5388f92..ad2efb410d64a0 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1145,6 +1145,8 @@ struct ModelDataV2 { hardBrakePredicted @7 :Bool; laneChangeState @8 :LaneChangeState; laneChangeDirection @9 :LaneChangeDirection; + laneChangeSignalsRemaining @10 :UInt8; + laneChangeRemaining @11 :UInt8; # deprecated 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. 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..9274e8f5e19b73 --- /dev/null +++ b/docs/superpowers/specs/2026-06-25-lane-change-auto-signal-design.md @@ -0,0 +1,255 @@ +# 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"**. + +--- + +## 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) +`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). 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 diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index ee4567f1e988e3..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,6 +10,16 @@ LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. +# 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: { LaneChangeState.off: log.Desire.none, @@ -40,18 +52,35 @@ def __init__(self): self.prev_one_blinker = False self.desire = log.Desire.none + # 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 _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 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 + 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: @@ -59,12 +88,12 @@ 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. The first tap queues one change. + self.arm_timer = 0.0 + self.queued_changes = 1 # 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 +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)) - if not one_blinker or below_lane_change_speed: - self.lane_change_state = LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none + 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 + + # Same-direction tap queues another change (capped). + if same_dir_tap: + self.queued_changes = min(self.queued_changes + 1, MAX_QUEUED_LANE_CHANGES) + + # Count down the arming window. + self.arm_timer += DT_MDL + + if below_lane_change_speed or opposite_tap: + self._reset() elif torque_applied and not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting + 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: @@ -93,11 +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() + + # Derived UI values. + if self.lane_change_state == LaneChangeState.preLaneChange: + self.signals_remaining = max(math.ceil(LANE_CHANGE_ARM_TIME - self.arm_timer), 0) + else: + 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 @@ -108,7 +164,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..d5d3e0187125dc --- /dev/null +++ b/selfdrive/controls/lib/tests/test_desire_helper.py @@ -0,0 +1,192 @@ +import math + +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.desire_helper import ( + DesireHelper, LaneChangeState, LaneChangeDirection, + LANE_CHANGE_ARM_TIME, MAX_QUEUED_LANE_CHANGES, +) +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 _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 _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(): + dh = DesireHelper() + _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_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 + + +# ---- time-based countdown ---------------------------------------------- + +def test_signals_remaining_starts_at_budget(): + dh = DesireHelper() + _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.lane_change_direction == LaneChangeDirection.none + + +# ---- nudge / exits ------------------------------------------------------ + +def test_wheel_nudge_starts_lane_change(): + dh = DesireHelper() + _arm_left(dh) + dh.update(_nudge_left(), True, 0.0) + assert dh.lane_change_state == LaneChangeState.laneChangeStarting + + +def test_opposite_tap_cancels(): + dh = DesireHelper() + _arm_left(dh) + dh.update(FakeCarState(left=False, right=False), True, 0.0) # blinker off + 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 + + +# ---- 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() + _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 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index bff59366d6ec4c..d35bcfe934753a 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -401,6 +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.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 62475ab55a8cdc..0292ec341ce020 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -254,6 +254,27 @@ 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_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 {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: + 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: first_word = 'Recalibrating' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating' return Alert( @@ -576,19 +597,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: {