Restore 2022 RAV4 radar CAN filter and lateral ownership

opendbc_repo/opendbc/car/toyota/radar_filter.py

@@ -1,31 +1,36 @@
-"""Dom's bus-0 radar CAN-filter path for the angle-control RAV4 (2023–25)."""
+"""Dom's bus-0 radar CAN-filter path for the RAV4 (2022 torque and 2023–25 angle control)."""
 from opendbc.car import structs
 from opendbc.car.toyota.values import CAR, ToyotaFlags
 
 LONG_WORD = 0x1449
 STOCK_WORD = 0x1649
+PROFILES = {
+  CAR.TOYOTA_RAV4_TSS2_2022: (structs.CarParams.SteerControlType.torque, 0x1049, 0x1249),
+  CAR.TOYOTA_RAV4_TSS2_2023: (structs.CarParams.SteerControlType.angle, LONG_WORD, STOCK_WORD),
+}
 
 
 def hardware_supported(candidate, fingerprint):
   # The filter's advertisement is a hardware discriminator, not an actuation
   # permission or a copy of ACC_CONTROL to decode under a different checksum.
-  return (candidate == CAR.TOYOTA_RAV4_TSS2_2023 and fingerprint.get(0, {}).get(0x2AA) == 8 and
+  return (candidate in PROFILES and fingerprint.get(0, {}).get(0x2AA) == 8 and
           not any(0x2AA in frames for bus, frames in fingerprint.items() if bus != 0) and
           not any(addr in frames for frames in fingerprint.values() for addr in (0x201, 0x2FF)))
 
 
 def configured(cp):
-  return bool(cp is not None and cp.brand == 'toyota' and cp.carFingerprint == CAR.TOYOTA_RAV4_TSS2_2023 and
+  profile = PROFILES.get(cp.carFingerprint) if cp is not None else None
+  return bool(profile is not None and cp.brand == 'toyota' and
               cp.flags & ToyotaFlags.RADAR_CAN_FILTER and not cp.flags & ToyotaFlags.DISABLE_RADAR and
-              cp.steerControlType == structs.CarParams.SteerControlType.angle and len(cp.safetyConfigs) == 1 and
+              cp.steerControlType == profile[0] and len(cp.safetyConfigs) == 1 and
               cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.toyota and
-              cp.safetyConfigs[0].safetyParam == (LONG_WORD if cp.openpilotLongitudinalControl else STOCK_WORD))
+              cp.safetyConfigs[0].safetyParam == (profile[1] if cp.openpilotLongitudinalControl else profile[2]))
 
 
 def prepare_stock(cp):
   if not configured(cp):
     return False
   cp.openpilotLongitudinalControl = False
-  cp.safetyConfigs[0].safetyParam = STOCK_WORD
+  cp.safetyConfigs[0].safetyParam = PROFILES[cp.carFingerprint][2]
   cp.autoResumeSng = False
   return True

openpilot/starpilot/car/toyota/aol.py

@@ -45,7 +45,21 @@ def lexus_is_qualified(cp, *, marked_only=False):
   return config.safetyModel == structs.CarParams.SafetyModel.toyota and config.safetyParam == 0x424D
 
 
+def radar_filter_qualified(cp, *, marked_only=False):
+  from opendbc.car.toyota.radar_filter import configured
+  if (not configured(cp) or cp.carFingerprint != CAR.TOYOTA_RAV4_TSS2_2022 or
+      not cp.pcmCruise or cp.notCar or cp.passive or cp.dashcamOnly or
+      cp.alternativeExperience not in ((32,) if marked_only else (0, 32))):
+    return False
+  required = ToyotaFlags.TSS2 | ToyotaFlags.NO_DSU | ToyotaFlags.RADAR_ACC | ToyotaFlags.RAISED_ACCEL_LIMIT | ToyotaFlags.RADAR_CAN_FILTER
+  allowed = required | ToyotaFlags.HYBRID | ToyotaFlags.HAS_BSM | ToyotaFlags.ARS510_RADAR | ToyotaFlags.ARS510_FUSED
+  return bool(cp.flags & required == required and not int(cp.flags) & ~int(allowed) and
+              (not cp.flags & ToyotaFlags.ARS510_FUSED or cp.flags & ToyotaFlags.ARS510_RADAR))
+
+
 def qualified(cp, *, marked_only=False):
+  if radar_filter_qualified(cp, marked_only=marked_only):
+    return True
   if lexus_is_qualified(cp, marked_only=marked_only):
     return True
   from opendbc.car.toyota import pedal

openpilot/starpilot/car/toyota/tests/test_rav4_2022_filter.py

@@ -0,0 +1,38 @@
+import pytest
+from opendbc.car import gen_empty_fingerprint
+from opendbc.car.toyota.interface import CarInterface
+from opendbc.car.toyota.values import CAR, ToyotaFlags
+from openpilot.starpilot.car.toyota.aol import radar_filter_qualified
+
+
+def factory(alpha):
+  fp = gen_empty_fingerprint()
+  fp[0][0x2AA] = 8
+  return CarInterface.get_params(CAR.TOYOTA_RAV4_TSS2_2022, fp, [], alpha, False, False)
+
+
+@pytest.mark.parametrize('alpha', [False, True])
+def test_2022_filter_factory_preserves_current_tune(alpha):
+  cp = factory(alpha)
+  plain = CarInterface.get_params(CAR.TOYOTA_RAV4_TSS2_2022, gen_empty_fingerprint(), [], alpha, False, False)
+  expected = plain.to_dict()
+  expected['flags'] = (expected['flags'] & ~int(ToyotaFlags.DISABLE_RADAR)) | int(ToyotaFlags.RADAR_CAN_FILTER)
+  expected['safetyConfigs'][0]['safetyParam'] = 0x1049 if alpha else 0x1249
+  assert cp.to_dict() == expected
+  assert radar_filter_qualified(cp)
+
+
+@pytest.mark.parametrize('field,value', [('passive', True), ('dashcamOnly', True), ('notCar', True),
+                                        ('pcmCruise', False), ('alternativeExperience', 128),
+                                        ('flags', 0), ('flags', 1 << 18)])
+def test_2022_filter_aol_rejects_invalid_configuration(field, value):
+  cp = factory(True)
+  setattr(cp, field, value)
+  assert not radar_filter_qualified(cp)
+
+
+@pytest.mark.parametrize('word', [73, 585, 0x1048, 0x104A, 0x1449, 0x1649])
+def test_2022_filter_aol_rejects_wrong_native_profile(word):
+  cp = factory(True)
+  cp.safetyConfigs[0].safetyParam = word
+  assert not radar_filter_qualified(cp)

openpilot/starpilot/car/toyota/tests/test_rav4_2022_filter_callers.py

@@ -0,0 +1,12 @@
+"""2022 torque RAV4 exact filter owner, with saved AOL ON/OFF."""
+import pytest
+from opendbc.car.toyota.values import CAR
+from openpilot.starpilot.car.toyota.tests import test_rav4_angle_filter_callers as helpers
+
+source_matched_native = helpers.source_matched_native
+daemon_gc_scope = helpers.daemon_gc_scope
+
+@pytest.mark.parametrize('alpha,disable,radar_mode', [(False, False, 0), (True, False, 0), (True, False, 1), (True, False, 2), (True, True, 1)])
+@pytest.mark.parametrize('aol', [False, True])
+def test_2022_actual_callers(alpha, disable, radar_mode, aol, tmp_path):
+  helpers.run_case(CAR.TOYOTA_RAV4_TSS2_2022, alpha, disable, radar_mode, tmp_path, aol=aol)

openpilot/starpilot/car/toyota/tests/test_rav4_angle_filter.py

@@ -49,7 +49,7 @@ def test_filter_detection_rejects_wrong_topology(fault):
   elif fault in ('pedal', 'sdsu'):
     fp[0][0x201 if fault == 'pedal' else 0x2FF] = 8
   else:
-    candidate = CAR.TOYOTA_RAV4_TSS2_2022
+    candidate = CAR.TOYOTA_CHR_TSS2
   cp = factory(True, fp, candidate)
   assert not cp.flags & (1 << 18)
   assert cp.flags & ToyotaFlags.DISABLE_RADAR

openpilot/starpilot/car/toyota/tests/test_rav4_angle_filter_callers.py

@@ -30,15 +30,18 @@ daemon_gc_scope = helpers.daemon_gc_scope
 
 @pytest.mark.parametrize('alpha,disable,radar_mode', [(False, False, 0), (True, False, 0), (True, False, 1), (True, False, 2), (True, True, 1)])
 def test_rav4_filter_actual_callers(alpha, disable, radar_mode, tmp_path):
+  run_case(CAR.TOYOTA_RAV4_TSS2_2023, alpha, disable, radar_mode, tmp_path)
+
+
+def run_case(candidate, alpha, disable, radar_mode, tmp_path, *, aol=True):
   helpers.SYNTHETIC_LAST.clear()
   helpers.NATIVE_LAST.clear()
-  candidate = CAR.TOYOTA_RAV4_TSS2_2023
   safety = libsafety_py.libsafety
   trace = []
   with OpenpilotPrefix(), ExitStack() as stack:
     params = Params()
     for key, value in [('OpenpilotEnabledToggle', True), ('AlphaLongitudinalEnabled', alpha),
-                       ('DisableOpenpilotLongitudinal', disable), ('AlwaysOnLateral', True), ('SafeMode', False)]:
+                       ('DisableOpenpilotLongitudinal', disable), ('AlwaysOnLateral', aol), ('SafeMode', False)]:
       params.put_bool(key, value, block=True)
     params.put('ToyotaRadarMode', radar_mode, block=True)
     fw = helpers.firmware(candidate)
@@ -53,15 +56,18 @@ def test_rav4_filter_actual_callers(alpha, disable, radar_mode, tmp_path):
       host = card_module.Car()
     long = alpha and not disable
     assert not host.CP.flags & ToyotaFlags.DISABLE_RADAR
-    assert host.CP.safetyConfigs[0].safetyParam == (0x1449 if long else 0x1649)
+    angle = candidate == CAR.TOYOTA_RAV4_TSS2_2023
+    enrolled = not angle and aol
+    assert host.CP.safetyConfigs[0].safetyParam == ((0x1449 if long else 0x1649) if angle else (0x1049 if long else 0x1249))
     assert host.CP.openpilotLongitudinalControl == long
     # This port keeps the existing angle-control AOL enrollment boundary.
-    assert host.CP.alternativeExperience == 0
+    assert host.CP.alternativeExperience == (0 if angle else 32)
     controls, sd = Controls(), sd_module.SelfdriveD(CP=host.CP)
     assert controls.CP.to_dict() == host.CP.to_dict()
     safety.init_tests()
     safety.set_alternative_experience(host.CP.alternativeExperience)
     assert safety.set_safety_hooks(host.CP.safetyConfigs[0].safetyModel.raw, host.CP.safetyConfigs[0].safetyParam) == 0
+    safety.set_aol_test_heartbeat(True)
     packer = CANPacker(DBC[candidate][Bus.pt])
     published, previous = {}, {}
 
@@ -91,7 +97,7 @@ def test_rav4_filter_actual_callers(alpha, disable, radar_mode, tmp_path):
         fault = 420 <= tick < 440
         main = not 500 <= tick < 520
         cruise = tick >= 150 and not any(a <= tick < b for a, b in ((260, 290), (310, 340), (360, 390), (420, 450), (500, 530)))
-        frames = helpers.physical(packer, cruise=cruise, main=main, brake=brake, gas=gas, gear=16 if reverse else 0, eps=9 if fault else 1)
+        frames = helpers.physical(packer, cruise=cruise, main=main, brake=brake, gas=gas, stopped=brake, gear=16 if reverse else 0, eps=9 if fault else 1)
         # LTA requires initialized measured angle and its own EPS status.
         frames = [f for f in frames if f[0] not in (0x260, 0x262)]
         frames += [packer.make_can_msg('STEER_TORQUE_SENSOR', 0, {'STEER_ANGLE': .0573}),
@@ -120,6 +126,9 @@ def test_rav4_filter_actual_callers(alpha, disable, radar_mode, tmp_path):
               patch.object(sd.sm, 'update', side_effect=lambda _, peers=peers: helpers.deliver(sd.sm, peers)),
               patch.object(sd.pm, 'send', side_effect=lambda n, m, events=events: events.update({n: m.as_reader()}))):
           sd.step()
+        wanted = sd.aol_axis_decision
+        safety.aol_set_host_request(int(wanted.desired_lateral) | int(wanted.desired_longitudinal) << 1)
+        native = helpers.native_events(host, safety, now, sd.aol_session_id)
         current = {**previous, **events}
         helpers.deliver(controls.sm, helpers.control_inputs(controls, published, state, current, native, now, .00005))
         command, lac_log = controls.state_control()
@@ -136,6 +145,9 @@ def test_rav4_filter_actual_callers(alpha, disable, radar_mode, tmp_path):
           if frame.address == 0x191:
             value = decode(host.CP, (frame.address, bytes(frame.dat), frame.src), 'STEERING_LTA')
             steering |= bool(value['STEER_REQUEST'] and abs(value['STEER_ANGLE_CMD']) > 0)
+          elif frame.address == 0x2E4 and not angle:
+            value = decode(host.CP, (frame.address, bytes(frame.dat), frame.src), 'STEERING_LKA')
+            steering |= bool(value['STEER_REQUEST'] and value['STEER_TORQUE_CMD'])
           elif frame.address == 0x343:
             value = decode(host.CP, (frame.address, bytes(frame.dat), frame.src), 'ACC_CONTROL')
             acceleration |= value['ACCEL_CMD'] > 0
@@ -145,8 +157,12 @@ def test_rav4_filter_actual_callers(alpha, disable, radar_mode, tmp_path):
           assert cs.canValid and not cs.canTimeout and host.ci_initialized and sd.initialized, trace[-1]
           assert cc.latActive and cc.longActive == long, trace[-1]
           active = True
-        if any(a <= tick < b for a, b in ((270, 279), (370, 379), (430, 439), (510, 519))):
+        if any(a <= tick < b for a, b in ((370, 379), (430, 439), (510, 519))):
           assert not cc.latActive and not cc.longActive, trace[-1]
+        if any(a <= tick < b for a, b in ((270, 279), (320, 329))):
+          assert cc.latActive == enrolled and not cc.longActive, trace[-1]
+        if enrolled and 442 <= tick < 449:
+          assert cc.latActive and not cc.longActive, trace[-1]
         previous = current
       assert active and steering
       assert acceleration == long

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "baa8ff4667d67c7739a1417763e8f842e0c53c85195c9509d733907bfdbbf3e7", "schema_version": 1}
+{"protected_inventory_sha256": "e9af39a992a51f2ebd7998a963134538d01d2e54aa92d91f924cb4be745e172a", "schema_version": 1}