MADS: Hyundai: Independent control with LFA button (#362)

* MADS: HKG CAN-FD: Disallow cruise buttons to engage when pcmCruiseSpeed is off

* handle states properly

* this is better

* make sure main button is pressed

* oops flipped main

* same behavior

* handle cruise main button universally

* both

* not needed

* oops

* wtf how did i miss

selfdrive/car/hyundai/carcontroller.py

@@ -228,7 +228,7 @@ class CarController(CarControllerBase):
         if hda2:
           can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
         if self.frame % 2 == 0:
-          can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled and CS.out.cruiseState.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
+          can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CS, CC.enabled and CS.out.cruiseState.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
                                                            set_speed_in_units, hud_control))
           self.accel_last = accel
       else:

selfdrive/car/hyundai/carstate.py

@@ -62,6 +62,11 @@ class CarState(CarStateBase):
     self.escc_aeb_dec_cmd = 0
     self._speed_limit_clu = 0
 
+  def get_main_enabled(self, ret) -> bool:
+    if self.prev_main_buttons != 1 and self.main_buttons[-1] == 1:
+      self.mainEnabled = not self.mainEnabled
+    return ret.cruiseState.available and self.mainEnabled
+
   def update(self, cp, cp_cam):
     if self.CP.carFingerprint in CANFD_CAR:
       return self.update_canfd(cp, cp_cam)
@@ -112,7 +117,7 @@ class CarState(CarStateBase):
     # cruise state
     if self.CP.openpilotLongitudinalControl:
       # These are not used for engage/disengage since openpilot keeps track of state using the buttons
-      ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0 and self.mainEnabled
+      ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0
       ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
       ret.cruiseState.standstill = False
       ret.cruiseState.nonAdaptive = False
@@ -207,17 +212,12 @@ class CarState(CarStateBase):
     self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
     self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
     if self.CP.openpilotLongitudinalControl:
-      if self.prev_main_buttons != 1:
-        if self.main_buttons[-1] == 1:
-          self.mainEnabled = not self.mainEnabled
-      ret.cruiseState.available = ret.cruiseState.available and self.mainEnabled
+      ret.cruiseState.available = self.get_main_enabled(ret)
     self.prev_mads_enabled = self.mads_enabled
     self.prev_lfa_enabled = self.lfa_enabled
     if self.CP.spFlags & HyundaiFlagsSP.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
-
     if self.CP.spFlags & HyundaiFlagsSP.SP_NAV_MSG or self.CP.spFlags & HyundaiFlagsSP.SP_LKAS12:
       self._update_traffic_signals(cp, cp_cam)
       ret.cruiseState.speedLimit = self._calculate_speed_limit() * speed_conv
@@ -282,6 +282,7 @@ class CarState(CarStateBase):
       ret.cruiseState.standstill = False
     else:
       cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
+      ret.cruiseState.available = cp_cruise_info.vl["SCC_CONTROL"]["MainMode_ACC"] == 1
       ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2)
       ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1
       ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor
@@ -298,6 +299,8 @@ class CarState(CarStateBase):
     self.prev_main_buttons = self.main_buttons[-1]
     self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
     self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
+    if self.CP.openpilotLongitudinalControl:
+      ret.cruiseState.available = self.get_main_enabled(ret)
     self.prev_mads_enabled = self.mads_enabled
     self.prev_lfa_enabled = self.lfa_enabled
     self.lfa_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"]

selfdrive/car/hyundai/hyundaicanfd.py

@@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled, lat_active, lateral_paused, blin
   return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
 
 
-def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control):
+def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control):
   jerk = 5
   jn = jerk / 50
   if not enabled or gas_override:
@@ -132,7 +132,7 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov
 
   values = {
     "ACCMode": 0 if not enabled else (2 if gas_override else 1),
-    "MainMode_ACC": 1,
+    "MainMode_ACC": 1 if CS.mainEnabled else 0,
     "StopReq": 1 if stopping else 0,
     "aReqValue": a_val,
     "aReqRaw": a_raw,

selfdrive/car/hyundai/interface.py

@@ -210,22 +210,25 @@ class CarInterface(CarInterfaceBase):
       *create_button_events(self.CS.main_buttons[-1], self.CS.prev_main_buttons, {1: ButtonType.altButton3}),
     ]
 
+    self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
+
     self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
                                                             self.CS.button_events, c.vCruise)
 
-    self.CS.mads_enabled = False if not self.mads_main_toggle else self.CS.mads_enabled
-
     if ret.cruiseState.available:
       if not self.CP.pcmCruiseSpeed:
         if any(b.type in (ButtonType.altButton3, ButtonType.cancel) and not b.pressed for b in self.CS.button_events):
           self.CS.accEnabled = True
-      if self.enable_mads:
-        if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
-          self.CS.madsEnabled = True
-        if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
-          self.CS.madsEnabled = not self.CS.madsEnabled
-        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
-    else:
+
+    if self.enable_mads:
+      if not self.CS.prev_mads_enabled and self.CS.mads_enabled and \
+        any(b.type == ButtonType.altButton3 for b in self.CS.button_events):
+        self.CS.madsEnabled = True
+      if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
+        self.CS.madsEnabled = not self.CS.madsEnabled
+      self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+
+    if not ret.cruiseState.available and self.CS.out.cruiseState.available:
       self.CS.madsEnabled = False
 
     if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed:

selfdrive/car/interfaces.py

@@ -470,7 +470,7 @@ class CarInterfaceBase(ABC):
         events.add(EventName.spReverseGear)
       elif cs_out.vEgo >= 5:
         events.add(EventName.reverseGear)
-    if not cs_out.cruiseState.available:
+    if not cs_out.cruiseState.available and cs_out.cruiseState.enabled:
       events.add(EventName.wrongCarMode)
     if cs_out.espDisabled:
       events.add(EventName.espDisabled)