diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 9281ca3746..fbcacd74b1 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -10,10 +10,14 @@ from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR -# Steering fault definitions: +SteerControlType = car.CarParams.SteerControlType + +# These steering fault definitions seem to be common across LKA (torque) and LTA (angle): # - high steer rate fault: goes to 21 or 25 for 1 frame, then 9 for 2 seconds -# - lka msg drop out: goes to 9 then 11 for a combined total of 2 seconds, then 3 -# - initializing: catch-all +# - lka/lta msg drop out: goes to 9 then 11 for a combined total of 2 seconds, then 3. +# if using the other control command, goes directly to 3 after 1.5 seconds +# - initializing: LTA can report 0 as long as STEER_TORQUE_SENSOR->STEER_ANGLE_INITIALIZING is 1, +# and is a catch-all for LKA TEMP_STEER_FAULTS = (0, 9, 11, 21, 25) @@ -95,12 +99,16 @@ class CarState(CarStateBase): # we could use the override bit from dbc, but it's triggered at too high torque values ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD - # Check EPS LKA fault status + # Check EPS LKA/LTA fault status ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in TEMP_STEER_FAULTS # 3 is a fault from the lka command message not being received by the EPS (recoverable) # 17 is a fault from a prolonged high torque delta between cmd and user (permanent) ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] in (3, 17) + if self.CP.steerControlType == SteerControlType.angle: + ret.steerFaultTemporary = ret.steerFaultTemporary or cp.vl["EPS_STATUS"]["LTA_STATE"] in TEMP_STEER_FAULTS + ret.steerFaultPermanent = ret.steerFaultPermanent or cp.vl["EPS_STATUS"]["LTA_STATE"] in (3,) + if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: # TODO: find the bit likely in DSU_CRUISE that describes an ACC fault. one may also exist in CLUTCH ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0 @@ -190,6 +198,10 @@ class CarState(CarStateBase): ("AUTO_HIGH_BEAM", "LIGHT_STALK"), ] + # Check LTA state if using LTA angle control + if CP.steerControlType == SteerControlType.angle: + signals.append(("LTA_STATE", "EPS_STATUS")) + checks = [ ("GEAR_PACKET", 1), ("LIGHT_STALK", 1), diff --git a/selfdrive/car/toyota/tests/test_toyota.py b/selfdrive/car/toyota/tests/test_toyota.py index 5648f75fe7..3d5eca1e0a 100755 --- a/selfdrive/car/toyota/tests/test_toyota.py +++ b/selfdrive/car/toyota/tests/test_toyota.py @@ -1,13 +1,20 @@ #!/usr/bin/env python3 import unittest -from selfdrive.car.toyota.values import TSS2_CAR, ANGLE_CONTROL_CAR +from selfdrive.car.toyota.values import DBC, TSS2_CAR, ANGLE_CONTROL_CAR class TestToyotaInterfaces(unittest.TestCase): def test_angle_car_set(self): self.assertTrue(len(ANGLE_CONTROL_CAR - TSS2_CAR) == 0) + def test_tss2_dbc(self): + # We make some assumptions about TSS2 platforms, + # like looking up certain signals only in this DBC + for car, dbc in DBC.items(): + if car in TSS2_CAR: + self.assertEqual(dbc["pt"], "toyota_nodsu_pt_generated") + if __name__ == "__main__": unittest.main()