diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 6945d8d976..3ee254cca7 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -63,6 +63,9 @@ def update(self, cp, cp_cam, cp_adas): ret.cruiseState.available = ((cruise_state == "STANDBY") or ret.cruiseState.enabled) ret.cruiseState.standstill = False # This needs to be false, since we can resume from stop without sending anything special + speed_limit = cp_cam.vl["DAS_status"]["DAS_fusedSpeedLimit"] + ret.cruiseState.speedLimit = self._calculate_speed_limit(speed_limit, speed_units) + # Gear ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_systemStatus"]["DI_gear"].get(int(cp.vl["DI_systemStatus"]["DI_gear"]), "DI_GEAR_INVALID")] @@ -105,6 +108,15 @@ def update(self, cp, cp_cam, cp_adas): return ret + def _calculate_speed_limit(self, speed_limit, speed_unit): + if speed_limit in [0, 155]: + return 0 + if speed_unit == "KPH": + return speed_limit * CV.KPH_TO_MS + elif speed_unit == "MPH": + return speed_limit * CV.MPH_TO_MS + return 0 + @staticmethod def get_can_parser(CP): messages = [