diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index abfba2aba2..898ce6ae7c 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/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), diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_angle_fault_recovery.py b/opendbc_repo/opendbc/car/hyundai/tests/test_angle_fault_recovery.py index 57c6178b45..d1ec6e3894 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_angle_fault_recovery.py +++ b/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 diff --git a/prebuilt.json b/prebuilt.json index 565af6fabf..320c3ff311 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "d5f92beb655186ab530750a45131653bcd6550756e0bf0e140d125e4fde627c9", "schema_version": 1} +{"protected_inventory_sha256": "793e8493716dd27302959d43c361f2d918661f2e5ab7d2ecd343a861afa97360", "schema_version": 1}