From 11c9105a60ca7ce24016195d48c05ef8e535c813 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 12 Aug 2024 01:12:42 -0500 Subject: [PATCH] carState: add invalid sensors field (#33266) * add to CS * add vehicleSensorsInvalid to CarState * fix ord * clean up old-commit-hash: 7ec998634097f062afe8fb584e34e4c8a1d9fb54 --- cereal/car.capnp | 1 + selfdrive/car/ford/carstate.py | 4 +--- selfdrive/car/ford/interface.py | 2 -- selfdrive/car/interfaces.py | 2 ++ selfdrive/car/toyota/carstate.py | 4 ++++ selfdrive/car/toyota/interface.py | 5 ----- 6 files changed, 8 insertions(+), 10 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index 3df23d9921..cbfb330c22 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -196,6 +196,7 @@ struct CarState { accFaulted @42 :Bool; carFaultedNonCritical @47 :Bool; # some ECU is faulted, but car remains controllable espActive @51 :Bool; + vehicleSensorsInvalid @52 :Bool; # invalid steering angle readings, etc. # cruise state cruiseState @10 :CruiseState; diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index ed707a5f78..5fe105482e 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -17,8 +17,6 @@ class CarState(CarStateBase): if CP.transmissionType == TransmissionType.automatic: self.shifter_values = can_define.dv["PowertrainData_10"]["TrnRng_D_Rq"] - self.vehicle_sensors_valid = False - self.prev_distance_button = 0 self.distance_button = 0 @@ -27,7 +25,7 @@ class CarState(CarStateBase): # Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement # The vehicle usually recovers out of this state within a minute of normal driving - self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3 + ret.vehicleSensorsInvalid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] != 3 # car speed ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index b872be212b..5768d73e4b 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -73,8 +73,6 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic]) - if not self.CS.vehicle_sensors_valid: - events.add(car.CarEvent.EventName.vehicleSensorsInvalid) ret.events = events.to_msg() diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 4707ade718..3d4bf543e0 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -300,6 +300,8 @@ class CarInterfaceBase(ABC): events.add(EventName.preEnableStandstill) if cs_out.gasPressed: events.add(EventName.gasPressedOverride) + if cs_out.vehicleSensorsInvalid: + events.add(EventName.vehicleSensorsInvalid) # Handle button presses for b in cs_out.buttonEvents: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 6037585c90..7c2d0e6ce6 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -112,6 +112,10 @@ class CarState(CarStateBase): 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 PERM_STEER_FAULTS + # Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until + # the more accurate angle sensor signal is initialized + ret.vehicleSensorsInvalid = not self.accurate_steer_angle_seen + 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 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 5b2a075ce8..5e356bfd49 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -154,11 +154,6 @@ class CarInterface(CarInterfaceBase): # events events = self.create_common_events(ret) - # Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until - # the more accurate angle sensor signal is initialized - if self.CP.steerControlType == SteerControlType.angle and not self.CS.accurate_steer_angle_seen: - events.add(EventName.vehicleSensorsInvalid) - if self.CP.openpilotLongitudinalControl: if ret.cruiseState.standstill and not ret.brakePressed: events.add(EventName.resumeRequired)