Added speed sign recognition on CAN-FD Ford

selfdrive/car/ford/carstate.py

@@ -24,6 +24,7 @@ class CarState(CarStateBase):
     self.prev_lkas_enabled = None
     self.buttonStates = BUTTON_STATES.copy()
     self.buttonStatesPrev = BUTTON_STATES.copy()
+    self._vLimit = 0
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -75,6 +76,11 @@ class CarState(CarStateBase):
     ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0
     ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3
     ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2)
+
+    # SpeedLimit
+    self.update_traffic_signals(self.CP, cp, cp_cam)
+    ret.cruiseState.speedLimit = self._vLimit
+
     if not self.CP.openpilotLongitudinalControl:
       ret.accFaulted = ret.accFaulted or cp_cam.vl["ACCDATA"]["CmbbDeny_B_Actl"] == 1
 
@@ -127,6 +133,21 @@ class CarState(CarStateBase):
 
     return ret
 
+
+  def update_traffic_signals(self, CP, cp, cp_cam):
+    if CP.carFingerprint in CANFD_CAR:
+      # we'll start with CANFD - not sure if and how to check if this message is in non CANFD
+      origVLimit = cp_cam.vl["Traffic_RecognitnData"]["TsrVLim1MsgTxt_D_Rq"]
+      origVLimitUnit = cp_cam.vl["Traffic_RecognitnData"]["TsrVlUnitMsgTxt_D_Rq"]
+
+      # convert to m/s from whatever came in
+      speed_factor = CV.MPH_TO_MS if origVLimitUnit == 2 else CV.KPH_TO_MS if origVLimitUnit == 1 else 0              
+
+      vLimit = origVLimit if origVLimit not in (0, 255) else 0
+
+      # speed in m/s
+      self._vLimit = vLimit * speed_factor
+
   @staticmethod
   def get_can_parser(CP):
     messages = [
@@ -161,6 +182,11 @@ class CarState(CarStateBase):
         ("BCM_Lamp_Stat_FD1", 1),
       ]
 
+    if CP.carFingerprint in CANFD_CAR:
+      messages += [
+        ("Traffic_RecognitnData", 1),
+      ]
+
     if CP.enableBsm and CP.carFingerprint not in CANFD_CAR:
       messages += [
         ("Side_Detect_L_Stat", 5),