Merge branch 'speed-limit-detection' into tesla-port-dev

carleeno · Aug 6, 2024 · 534de90 · 534de90
2 parents c8989aa + 83baeef
commit 534de90
Showing 1 changed file with 12 additions and 0 deletions.
diff --git 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 = [