diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index d6265fd657..5c06da9680 100644 --- a/selfdrive/car/ford/carstate.py +++ b/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),