Mazda button spamming

opendbc_repo/opendbc/car/interfaces.py

@@ -440,6 +440,8 @@ class CarStateBase(ABC):
     self.v_ego_clu_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
 
     self.softHoldActive = 0
+    self.is_metric = True
+    self.lkas_enabled = False
 
   @abstractmethod
   def update(self, can_parsers) -> structs.CarState:

opendbc_repo/opendbc/car/mazda/carcontroller.py

@@ -3,6 +3,8 @@ from opendbc.car import Bus, apply_driver_steer_torque_limits, structs
 from opendbc.car.interfaces import CarControllerBase
 from opendbc.car.mazda import mazdacan
 from opendbc.car.mazda.values import CarControllerParams, Buttons
+from opendbc.car.common.conversions import Conversions as CV
+from openpilot.common.params import Params
 
 VisualAlert = structs.CarControl.HUDControl.VisualAlert
 
@@ -14,7 +16,15 @@ class CarController(CarControllerBase):
     self.packer = CANPacker(dbc_names[Bus.pt])
     self.brake_counter = 0
     
+    self.activateCruise = 0
+    self.speed_from_pcm = 1
+
   def update(self, CC, CS, now_nanos):
+
+    if self.frame % 50 == 0:
+      params = Params()
+      self.speed_from_pcm = params.get_int("SpeedFromPCM")
+
     can_sends = []
 
     apply_steer = 0
@@ -35,12 +45,18 @@ class CarController(CarControllerBase):
         # Cancel Stock ACC if it's enabled while OP is disengaged
         # Send at a rate of 10hz until we sync with stock ACC state
         can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.CANCEL))
-    else:
+    elif False:
       self.brake_counter = 0
       if CC.cruiseControl.resume and self.frame % 5 == 0:
         # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
         # Send Resume button when planner wants car to move
         can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME))
+    else:
+      if self.frame % 20 == 0:
+        spam_button = self.make_spam_button(CC, CS)
+        if spam_button > 0:
+          self.brake_counter = 0
+          can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, self.frame // 10, spam_button))
 
     self.apply_steer_last = apply_steer
 
@@ -62,3 +78,36 @@ class CarController(CarControllerBase):
 
     self.frame += 1
     return new_actuators, can_sends
+
+  def make_spam_button(self, CC, CS):
+    hud_control = CC.hudControl
+    set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
+    target = int(set_speed_in_units+0.5)
+    target = int(round(target / 5.0) * 5.0)
+    current = int(CS.out.cruiseState.speed*CV.MS_TO_KPH + 0.5)
+    current = int(round(current / 5.0) * 5.0)
+    v_ego_kph = CS.out.vEgo * CV.MS_TO_KPH
+
+    cant_activate = CS.out.brakePressed or CS.out.gasPressed
+
+    if CC.enabled:
+      if not CS.out.cruiseState.enabled:
+        if (hud_control.leadVisible or v_ego_kph > 10.0) and self.activateCruise == 0 and not cant_activate:
+          self.activateCruise = 1
+          print("RESUME")
+          return Buttons.RESUME
+      elif CC.cruiseControl.resume:
+        return Buttons.RESUME
+      elif target < current and current>= 31 and self.speed_from_pcm != 1:
+        print(f"SET_MINUS target={target}, current={current}")
+        return Buttons.SET_MINUS
+      elif target > current and current < 160 and self.speed_from_pcm != 1:
+        print(f"SET_PLUS target={target}, current={current}")
+        return Buttons.SET_PLUS
+    elif CS.out.activateCruise:
+      if (hud_control.leadVisible or v_ego_kph > 10.0) and self.activateCruise == 0 and not cant_activate:
+        self.activateCruise = 1
+        print("RESUME")
+        return Buttons.RESUME
+
+    return 0

opendbc_repo/opendbc/car/mazda/carstate.py

@@ -3,10 +3,11 @@ from opendbc.can.parser import CANParser
 from opendbc.car import Bus, create_button_events, structs
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.interfaces import CarStateBase
-from opendbc.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags
+from opendbc.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags, Buttons
 
 ButtonType = structs.CarState.ButtonEvent.Type
-
+BUTTONS_DICT = {Buttons.SET_PLUS: ButtonType.accelCruise, Buttons.SET_MINUS: ButtonType.decelCruise,
+                Buttons.RESUME: ButtonType.resumeCruise, Buttons.CANCEL: ButtonType.cancel}
 
 class CarState(CarStateBase):
   def __init__(self, CP):
@@ -19,7 +20,9 @@ class CarState(CarStateBase):
     self.acc_active_last = False
     self.low_speed_alert = False
     self.lkas_allowed_speed = False
+    self.lkas_disabled = False
     
+    self.prev_distance_button = 0
     self.distance_button = 0
 
   def update(self, can_parsers) -> structs.CarState:
@@ -28,9 +31,20 @@ class CarState(CarStateBase):
 
     ret = structs.CarState()
 
-    prev_distance_button = self.distance_button
+    self.prev_distance_button = self.distance_button
     self.distance_button = cp.vl["CRZ_BTNS"]["DISTANCE_LESS"]
     
+    self.prev_cruise_buttons = self.cruise_buttons
+
+    if bool(cp.vl["CRZ_BTNS"]["SET_P"]):
+      self.cruise_buttons = Buttons.SET_PLUS
+    elif bool(cp.vl["CRZ_BTNS"]["SET_M"]):
+      self.cruise_buttons = Buttons.SET_MINUS
+    elif bool(cp.vl["CRZ_BTNS"]["RES"]):
+      self.cruise_buttons = Buttons.RESUME
+    else:
+      self.cruise_buttons = Buttons.NONE
+      
     ret.wheelSpeeds = self.get_wheel_speeds(
       cp.vl["WHEEL_SPEEDS"]["FL"],
       cp.vl["WHEEL_SPEEDS"]["FR"],
@@ -113,13 +127,21 @@ class CarState(CarStateBase):
     self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"]
 
     # camera signals
+    self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
     self.cam_lkas = cp_cam.vl["CAM_LKAS"]
     self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
     ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
 
-    # TODO: add button types for inc and dec
-    ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise})
+    self.lkas_previously_enabled = self.lkas_enabled
+    self.lkas_enabled = not self.lkas_disabled
     
+    # TODO: add button types for inc and dec
+    #ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise})
+    ret.buttonEvents = [
+      *create_button_events(self.cruise_buttons, self.prev_cruise_buttons, BUTTONS_DICT),
+      *create_button_events(self.distance_button, self.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
+      *create_button_events(self.lkas_enabled, self.lkas_previously_enabled, {1: ButtonType.lfaButton}),
+    ]
     return ret
 
   @staticmethod

opendbc_repo/opendbc/car/mazda/mazdacan.py

@@ -92,20 +92,22 @@ def create_button_cmd(packer, CP, counter, button):
 
   can = int(button == Buttons.CANCEL)
   res = int(button == Buttons.RESUME)
+  inc = int(button == Buttons.SET_PLUS)
+  dec = int(button == Buttons.SET_MINUS)
   
   if CP.flags & MazdaFlags.GEN1:
     values = {
       "CAN_OFF": can,
       "CAN_OFF_INV": (can + 1) % 2,
 
-      "SET_P": 0,
-      "SET_P_INV": 1,
+      "SET_P": inc,
+      "SET_P_INV": (inc + 1) % 2,
 
       "RES": res,
       "RES_INV": (res + 1) % 2,
 
-      "SET_M": 0,
-      "SET_M_INV": 1,
+      "SET_M": dec,
+      "SET_M_INV": (dec + 1) % 2,
 
       "DISTANCE_LESS": 0,
       "DISTANCE_LESS_INV": 1,

panda/board/safety.h

@@ -780,7 +780,7 @@ void pcm_cruise_check(bool cruise_engaged) {
   // Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages
   if (!cruise_engaged) {
     controls_allowed = false;
-    print("controls_allowed(pcm) = false\n");
+    //print("controls_allowed(pcm) = false\n");
   }
   if (cruise_engaged && !cruise_engaged_prev) {
     controls_allowed = true;

panda/board/safety/safety_mazda.h

@@ -83,7 +83,7 @@ static bool mazda_tx_hook(const CANPacket_t *to_send) {
       // only allow cancel while contrls not allowed
       bool cancel_cmd = (GET_BYTE(to_send, 0) == 0x1U);
       if (!controls_allowed && !cancel_cmd) {
-        tx = false;
+        //tx = false;
       }
     }
   }