MADS? MADS!

carleeno · Aug 2, 2024 · 81830c8 · 81830c8
1 parent 1fe12db
commit 81830c8
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 11 deletions.
diff --git a/panda b/panda
diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py
@@ -22,8 +22,6 @@ def __init__(self, CP):
   def update(self, cp, cp_cam, cp_adas):
     ret = car.CarState.new_message()
 
-    self.prev_mads_enabled = self.mads_enabled
-
     # Vehicle speed
     ret.vEgoRaw = cp.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)

diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py
@@ -39,14 +39,13 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_adas)
 
-    # TODO: Implement detection of half vs full press of stalk for MADS control
-    # Until then, we're disabling main state control to prevent it from trying to self-engage without user input
-    self.CS.mads_enabled = False  # Tesla has no "cruise main state", as cruise is automatically available
-
     if ret.cruiseState.available:
       if self.enable_mads:
-        if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
-          self.CS.madsEnabled = True
+        for b in self.CS.button_events:
+          if b.type == ButtonType.altButton1 and b.pressed:
+            self.CS.madsEnabled = True
+          elif b.type == ButtonType.altButton2 and b.pressed:
+            self.CS.madsEnabled = False
         self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
     else:
       self.CS.madsEnabled = False

diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py
@@ -64,8 +64,10 @@ class CANBUS:
   Button(car.CarState.ButtonEvent.Type.rightBlinker, "SCCM_leftStalk", "SCCM_turnIndicatorStalkStatus", [1, 2]),
   Button(car.CarState.ButtonEvent.Type.accelCruise, "VCLEFT_switchStatus", "VCLEFT_swcRightScrollTicks", list(range(1, 10))),
   Button(car.CarState.ButtonEvent.Type.decelCruise, "VCLEFT_switchStatus", "VCLEFT_swcRightScrollTicks", list(range(-9, 0))),
-  Button(car.CarState.ButtonEvent.Type.cancel, "SCCM_rightStalk", "SCCM_rightStalkStatus", [1, 2]),
-  Button(car.CarState.ButtonEvent.Type.resumeCruise, "SCCM_rightStalk", "SCCM_rightStalkStatus", [3, 4]),
+  Button(car.CarState.ButtonEvent.Type.cancel, "SCCM_rightStalk", "SCCM_rightStalkStatus", [1]),
+  Button(car.CarState.ButtonEvent.Type.resumeCruise, "SCCM_rightStalk", "SCCM_rightStalkStatus", [4]),
+  Button(car.CarState.ButtonEvent.Type.altButton1, "SCCM_rightStalk", "SCCM_rightStalkStatus", [3]),  # MADS engaged
+  Button(car.CarState.ButtonEvent.Type.altButton2, "SCCM_rightStalk", "SCCM_rightStalkStatus", [2]),  # MADS disengaged
 ]