Yield HKG direct-angle fault recovery until driver release

opendbc_repo/opendbc/car/hyundai/carcontroller.py

@@ -118,6 +118,7 @@ class CarController(CarControllerBase):
     self.angle_vm = VehicleModel(CP) if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else None
     self.apply_angle_last = 0.0
     self.angle_gain_last = 0.0
+    self.angle_fault_driver_override = False
     self.angle_tx_recovery = (AngleTxRecovery(0xCB, self.CAN.ECAN)
       if pe_long_qualified(CP) or ev9_long_qualified(CP) else AngleTxRecovery(0x110, self.CAN.ACAN)
       if CP.carFingerprint in (CAR.HYUNDAI_IONIQ_5_PE, CAR.KIA_EV9) and
@@ -436,6 +437,8 @@ class CarController(CarControllerBase):
 
     # steering control
     if ev9_long:
+      self.angle_fault_driver_override = bool(CS.out.steeringPressed and (
+        self.angle_fault_driver_override or CS.out.steerFaultTemporary or CS.out.steerFaultPermanent))
       drive = CS.out.gearShifter == structs.CarState.GearShifter.drive
       measured = CS.angle_steering_angle
       if self.angle_tx_recovery is not None and self.angle_tx_recovery.take_rejection(self.frame):
@@ -450,7 +453,8 @@ class CarController(CarControllerBase):
       ev9_lateral_allowed = lateral_request_allowed(CC, CS.out, CS.angle_steering_fault,
                                                    independent=(ev9_long_aol_qualified(self.CP, marked_only=True) or
                                                                 pe_long_aol_qualified(self.CP, marked_only=True)))
-      active = bool(ev9_lateral_allowed and CC.latActive and abs(measured) <= bound and abs(self.apply_angle_last) <= bound)
+      active = bool(ev9_lateral_allowed and CC.latActive and not self.angle_fault_driver_override and
+                    abs(measured) <= bound and abs(self.apply_angle_last) <= bound)
       self.ev9_angle_filter.update_alpha(float(np.interp(CS.out.vEgo, [5.0, 10.0, 20.0], [0.2, 0.1, 0.0])))
       desired = self.ev9_angle_filter.update(float(np.clip(CC.actuators.steeringAngleDeg,
                                                          -(self.params.ANGLE_LIMITS.STEER_ANGLE_MAX if pe_long else EV9_OP_LONG_ANGLE_TARGET_MAX),

opendbc_repo/opendbc/car/hyundai/tests/test_angle_fault_recovery.py

@@ -58,6 +58,8 @@ def test_actual_controller_fault_and_full_lateral_withdrawal_restart_gain_from_z
   cs = ci.CS
   cs.out.vEgoRaw = cs.out.vEgo = 5.
   cs.out.steeringAngleDeg = cs.angle_steering_angle = 20.
+  cs.out.steeringPressed = False
+  cs.out.steeringTorque = 0.
   command = structs.CarControl(enabled=True, latActive=True)
   command.actuators.steeringAngleDeg = 50.
   ci.CC.apply_angle_last = 20.
@@ -124,3 +126,59 @@ def test_long_controller_consumes_current_rejection_without_retaining_filtered_h
     _, frames = ci.apply(command.as_reader(), now[0])
     assert abs(ci.CC.apply_angle_last + 20.) <= 5.
     assert ci.CC.angle_gain_last == .004
+
+
+@pytest.mark.parametrize('identity', (CAR.HYUNDAI_IONIQ_5_PE, CAR.KIA_EV9))
+@pytest.mark.parametrize('long', (True,))
+@pytest.mark.parametrize('aol', (False, True))
+def test_fault_recovery_yields_until_driver_releases_steering(identity, long, aol):
+  ci, _ = controller(identity, long, aol)
+  cs = ci.CS
+  cs.out.vEgoRaw = cs.out.vEgo = 3.
+  cs.out.steeringAngleDeg = cs.angle_steering_angle = 175.
+  cs.out.steeringPressed = True
+  cs.out.steeringTorque = 500.
+  command = structs.CarControl(enabled=not aol, latActive=True)
+  command.actuators.steeringAngleDeg = 360.
+  ci.CC.apply_angle_last = 175.
+  ci.CC.angle_gain_last = .1
+  address, gain_index = (0xCB, 6) if long else (0x110, 12)
+  active_index, active_mask = (3, 15) if long else (9, 3)
+  for frame in range(80):
+    cs.out.steerFaultTemporary = cs.angle_steering_fault = frame % 40 < 16
+    cs.out.steeringAngleDeg = cs.angle_steering_angle = 175. - frame * 3.
+    ci.CC.frame = frame + 21
+    _, output = ci.apply(command.as_reader(), 200_000_000 + frame * 10_000_000)
+    payload = next((data for addr, data, _ in output if addr == address), None)
+    if payload is not None:
+      assert (payload[active_index] >> 4 & active_mask) == 1, 'FAULT_RECOVERY_FIGHTS_DRIVER'
+      assert payload[gain_index] == 0
+    assert ci.CC.apply_angle_last == pytest.approx(cs.out.steeringAngleDeg)
+  cs.out.steerFaultTemporary = cs.angle_steering_fault = False
+  cs.out.steeringPressed = False
+  cs.out.steeringTorque = 0.
+  ci.CC.frame += 1
+  _, output = ci.apply(command.as_reader(), 1_000_000_000)
+  payload = next(data for addr, data, _ in output if addr == address)
+  assert (payload[active_index] >> 4 & active_mask) == 2
+  assert payload[gain_index] == 1
+  assert abs(ci.CC.apply_angle_last - cs.out.steeringAngleDeg) <= 5.
+
+
+@pytest.mark.parametrize('identity', (CAR.HYUNDAI_IONIQ_5_PE, CAR.KIA_EV9))
+@pytest.mark.parametrize('long', (False, True))
+def test_driver_pressure_without_fault_keeps_existing_angle_blending(identity, long):
+  ci, _ = controller(identity, long, True)
+  cs = ci.CS
+  cs.out.vEgoRaw = cs.out.vEgo = 3.
+  cs.out.steeringAngleDeg = cs.angle_steering_angle = 20.
+  cs.out.steeringPressed = True
+  cs.out.steeringTorque = 500.
+  command = structs.CarControl(enabled=False, latActive=True)
+  command.actuators.steeringAngleDeg = 40.
+  ci.CC.apply_angle_last = 20.
+  ci.CC.frame = 21
+  _, output = ci.apply(command.as_reader(), 200_000_000)
+  address, active_index, active_mask = (0xCB, 3, 15) if long else (0x110, 9, 3)
+  payload = next(data for addr, data, _ in output if addr == address)
+  assert (payload[active_index] >> 4 & active_mask) == 2

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "d5f92beb655186ab530750a45131653bcd6550756e0bf0e140d125e4fde627c9", "schema_version": 1}
+{"protected_inventory_sha256": "793e8493716dd27302959d43c361f2d918661f2e5ab7d2ecd343a861afa97360", "schema_version": 1}