MADS: add toggle for Cruise Main button engage (commaai#82)

* MADS: add toggle for Cruise Main button engage * add toggle
carleeno · May 31, 2023 · 356a601 · 356a601
1 parent 99de690
commit 356a601
Show file tree

Hide file tree

Showing 12 changed files with 25 additions and 9 deletions.
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
@@ -98,7 +98,7 @@ def _update(self, c):
 
     buttonEvents = []
 
-    self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
+    self.CS.mads_enabled = False if not (self.CS.control_initialized or self.CS.mads_main_toggle) else ret.cruiseState.available
 
     if ret.cruiseState.available:
       if self.enable_mads:

diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
@@ -262,7 +262,7 @@ def _update(self, c):
       if self.CS.cruise_buttons != CruiseButtons.UNPRESS and self.CS.prev_cruise_buttons != CruiseButtons.UNPRESS:
         buttonEvents.append(create_button_event(CruiseButtons.UNPRESS, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS))
 
-    self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
+    self.CS.mads_enabled = False if not (self.CS.control_initialized or self.CS.mads_main_toggle) else ret.cruiseState.available
 
     if not self.CP.pcmCruise:
       if any(b.type == ButtonType.accelCruise and b.pressed for b in buttonEvents):

diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
@@ -344,7 +344,7 @@ def _update(self, c):
     if self.CS.cruise_setting != self.CS.prev_cruise_setting:
       buttonEvents.append(create_button_event(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1}))
 
-    self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
+    self.CS.mads_enabled = False if not (self.CS.control_initialized or self.CS.mads_main_toggle) else ret.cruiseState.available
 
     self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret.cruiseState.available, self.CS.accEnabled,
                                                                           buttonEvents, c.vCruise)

diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
@@ -193,7 +193,7 @@ def update(self, cp, cp_cam):
     if self.CP.flags & HyundaiFlags.SP_CAN_LFA_BTN:
       self.lfa_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
 
-    self.mads_enabled = False if not self.control_initialized else ret.cruiseState.available
+    self.mads_enabled = False if not (self.control_initialized or self.mads_main_toggle) else ret.cruiseState.available
 
     if self.CP.flags & HyundaiFlags.SP_NAV_MSG:
       self._update_traffic_signals(self.CP, cp, cp_cam)

diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
@@ -547,6 +547,7 @@ def sp_update_params(self, CS):
       self.gac = self.param_s.get_bool("GapAdjustCruise")
       self.gac_mode = round(float(self.param_s.get("GapAdjustCruiseMode", encoding="utf8")))
       self.reverse_dm_cam = self.param_s.get_bool("ReverseDmCam")
+      CS.mads_main_toggle = self.param_s.get_bool("MadsCruiseMain")
 
 class RadarInterfaceBase(ABC):
   def __init__(self, CP):
@@ -587,6 +588,7 @@ def __init__(self, CP):
     self.control_initialized = False
     self.gap_dist_button = 0
     self.gac_tr = round(float(self.param_s.get("GapAdjustCruiseTr", encoding="utf8")))
+    self.mads_main_toggle = self.param_s.get_bool("MadsCruiseMain")
 
     # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]])
     # R = 0.3

diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
@@ -68,7 +68,7 @@ def _update(self, c):
       be.type = ButtonType.cancel
       buttonEvents.append(be)
 
-    self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
+    self.CS.mads_enabled = False if not (self.CS.control_initialized or self.CS.mads_main_toggle) else ret.cruiseState.available
 
     if ret.cruiseState.available:
       if self.enable_mads:

diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py
@@ -51,7 +51,7 @@ def _update(self, c):
     #be.type = car.CarState.ButtonEvent.Type.accelCruise
     #buttonEvents.append(be)
 
-    self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
+    self.CS.mads_enabled = False if not (self.CS.control_initialized or self.CS.mads_main_toggle) else ret.cruiseState.available
 
     if ret.cruiseState.available:
       if self.enable_mads:

diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
@@ -125,7 +125,7 @@ def _update(self, c):
 
     buttonEvents = []
 
-    self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
+    self.CS.mads_enabled = False if not (self.CS.control_initialized or self.CS.mads_main_toggle) else ret.cruiseState.available
 
     if ret.cruiseState.available:
       if self.enable_mads:

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
@@ -263,7 +263,7 @@ def _update(self, c):
     if self.CS.gap_dist_button != self.CS.prev_gap_dist_button:
       buttonEvents.append(create_button_event(self.CS.gap_dist_button, self.CS.prev_gap_dist_button, {1: ButtonType.gapAdjustCruise}))
 
-    self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
+    self.CS.mads_enabled = False if not (self.CS.control_initialized or self.CS.mads_main_toggle) else ret.cruiseState.available
 
     if ret.cruiseState.available:
       if self.enable_mads:

diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py
@@ -238,7 +238,7 @@ def _update(self, c):
         be.pressed = self.CS.buttonStates[button]
         buttonEvents.append(be)
 
-    self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available
+    self.CS.mads_enabled = False if not (self.CS.control_initialized or self.CS.mads_main_toggle) else ret.cruiseState.available
 
     self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret.cruiseState.available, self.CS.accEnabled,
                                                                           buttonEvents, c.vCruise,

diff --git a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc
@@ -165,6 +165,19 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : QWidget(parent) {
     accMadsCombo->setEnabled(offroad);
   });
   madsSubControl->addWidget(accMadsCombo);
+  madsSubControl->addWidget(horizontal_line());
+
+  // Controls: Enable M.A.D.S. with Cruise Main
+  madsCruiseMain = new ParamControl(
+    "MadsCruiseMain",
+    tr("Toggle M.A.D.S. with Cruise Main"),
+    tr("Allows M.A.D.S. engagement/disengagement with \"Cruise Main\" cruise control button from the steering wheel."),
+    "../assets/offroad/icon_openpilot.png"
+  );
+  connect(uiState(), &UIState::offroadTransition, [=](bool offroad) {
+    madsCruiseMain->setEnabled(offroad);
+  });
+  madsSubControl->addWidget(madsCruiseMain);
   connect(madsControl, &ToggleControl::toggleFlipped, [=](bool state) {
     updateToggles();
   });

diff --git a/selfdrive/ui/qt/offroad/sunnypilot_settings.h b/selfdrive/ui/qt/offroad/sunnypilot_settings.h
@@ -88,6 +88,7 @@ public slots:
   ParamControl *madsControl;
   ParamControl *disengageLateralOnBrake;
   ParamControl *accMadsCombo;
+  ParamControl *madsCruiseMain;
   ParamControl *belowSpeed;
   ParamControl *dlpControl;
   ParamControl *dlpCurve;