Ford: add alert for steering wheel angle not ready (#28140)

* add event for steering angle calibrating

* how about this

how about this

* rename

* more openpilot-y

* add comment to code

* move to top

* better wording

* more general

* fix

* ?

* above

* bump cereal to master

* add more comms

cereal

@@ -1 +1 @@
-Subproject commit feef4530affd31b3c1816eebb52e8a684d8c7329
+Subproject commit 8902e7a77815ca08e795b3006274d02379da7ca6

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

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

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"),
+  },
+
 }