diff --git a/cereal b/cereal index feef4530af..8902e7a778 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit feef4530affd31b3c1816eebb52e8a684d8c7329 +Subproject commit 8902e7a77815ca08e795b3006274d02379da7ca6 diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 9b8e966228..f27e9bb981 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -16,9 +16,15 @@ class CarState(CarStateBase): if CP.transmissionType == TransmissionType.automatic: self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"] + self.vehicle_sensors_valid = False + def update(self, cp, cp_cam): ret = car.CarState.new_message() + # 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 + # car speed ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) @@ -106,6 +112,7 @@ class CarState(CarStateBase): ("AccStopMde_D_Rq", "EngBrakeData"), # PCM ACC standstill ("AccEnbl_B_RqDrv", "Cluster_Info1_FD1"), # PCM ACC enable ("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg) + ("StePinCompAnEst_D_Qf", "SteeringPinion_Data"), # PSCM estimated steering angle (quality flag) # Calculates steering angle (and offset) from pinion # angle and driving measurements. # StePinRelInit_An_Sns is the pinion angle, initialised diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 01c5dd377b..df41a8db9d 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -76,6 +76,9 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam) 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() return ret diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 89fc8d8357..70506ca9ac 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -945,4 +945,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("LKAS Disabled"), }, + EventName.vehicleSensorsInvalid: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Vehicle Sensors Invalid"), + ET.PERMANENT: NormalPermanentAlert("Vehicle Sensors Calibrating", "Drive to Calibrate"), + ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"), + }, + }