From fd2b79b0ab07600b49495b5d64bbe06cd1afc10a Mon Sep 17 00:00:00 2001 From: Carlin Hefner Date: Thu, 1 Aug 2024 14:31:51 -0600 Subject: [PATCH 1/3] Add Tesla vision speed limit detection --- selfdrive/car/tesla/carstate.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index e54e220c68..06c3bcdc2c 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.vl["DAS_status"]["DAS_visionOnlySpeedLimit"] # TODO: give user toggle between vision and fused + 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")] @@ -100,6 +103,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 = [ From b0e972d1eff0f185585b181e64b6d524ff838fe9 Mon Sep 17 00:00:00 2001 From: Carlin Hefner Date: Thu, 1 Aug 2024 14:52:38 -0600 Subject: [PATCH 2/3] Use correct bus --- selfdrive/car/tesla/carstate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 06c3bcdc2c..ca10ca30aa 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -63,7 +63,7 @@ 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.vl["DAS_status"]["DAS_visionOnlySpeedLimit"] # TODO: give user toggle between vision and fused + speed_limit = cp_cam.vl["DAS_status"]["DAS_visionOnlySpeedLimit"] # TODO: give user toggle between vision and fused ret.cruiseState.speedLimit = self._calculate_speed_limit(speed_limit, speed_units) # Gear From 83baeefd62cd84a82f5e568dc293aa73a376d477 Mon Sep 17 00:00:00 2001 From: Carlin Hefner Date: Thu, 1 Aug 2024 15:08:07 -0600 Subject: [PATCH 3/3] Use fused speed limit --- selfdrive/car/tesla/carstate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index ca10ca30aa..0d5cc1fc00 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -63,7 +63,7 @@ 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_visionOnlySpeedLimit"] # TODO: give user toggle between vision and fused + speed_limit = cp_cam.vl["DAS_status"]["DAS_fusedSpeedLimit"] ret.cruiseState.speedLimit = self._calculate_speed_limit(speed_limit, speed_units) # Gear