+ Mazda Stock Custom Longitudinal Control

selfdrive/car/mazda/carcontroller.py

@@ -4,6 +4,12 @@ from openpilot.selfdrive.car import apply_driver_steer_torque_limits
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.mazda import mazdacan
 from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+from openpilot.common.conversions import Conversions as CV
+
+
+params_memory = Params("/dev/shm/params")
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -17,6 +23,13 @@ class CarController(CarControllerBase):
     self.frame = 0
 
   def update(self, CC, CS, now_nanos, frogpilot_variables):
+    hud_control = CC.hudControl
+    hud_v_cruise = hud_control.setSpeed
+    if hud_v_cruise > 70:
+      hud_v_cruise = 0
+    actuators = CC.actuators
+    accel = actuators.accel
+
     can_sends = []
 
     apply_steer = 0
@@ -43,6 +56,11 @@ class CarController(CarControllerBase):
         # 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))
+      # ACC Spam
+      elif frogpilot_variables.CSLC:
+        if CC.enabled and self.frame % 10 == 0 and CS.cruise_buttons == Buttons.NONE and not CS.out.gasPressed and not CS.distance_button:
+          slcSet = get_set_speed(self, hud_v_cruise)
+          can_sends.extend(mazdacan.create_mazda_acc_spam_command(self.packer, self, CS, slcSet, CS.out.vEgo, frogpilot_variables, accel))
 
     self.apply_steer_last = apply_steer
 
@@ -64,3 +82,13 @@ class CarController(CarControllerBase):
 
     self.frame += 1
     return new_actuators, can_sends
+
+def get_set_speed(self, hud_v_cruise):
+  v_cruise = min(hud_v_cruise, V_CRUISE_MAX * CV.KPH_TO_MS)
+
+  v_cruise_slc: float = 0.
+  v_cruise_slc = params_memory.get_float("CSLCSpeed")
+
+  if v_cruise_slc > 0.:
+    v_cruise = v_cruise_slc
+  return v_cruise

selfdrive/car/mazda/carstate.py

@@ -3,7 +3,7 @@ from openpilot.common.conversions import Conversions as CV
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.interfaces import CarStateBase
-from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags
+from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags, Buttons
 
 class CarState(CarStateBase):
   def __init__(self, CP):
@@ -29,6 +29,17 @@ class CarState(CarStateBase):
     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"],

selfdrive/car/mazda/interface.py

@@ -1,13 +1,18 @@
 #!/usr/bin/env python3
 from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
-from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS
+from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS, Buttons
 from openpilot.selfdrive.car import create_button_events, get_safety_config
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
+from openpilot.common.params import Params
+
+params = Params()
 
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = car.CarEvent.EventName
 FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
+BUTTONS_DICT = {Buttons.SET_PLUS: ButtonType.accelCruise, Buttons.SET_MINUS: ButtonType.decelCruise,
+                Buttons.RESUME: ButtonType.resumeCruise, Buttons.CANCEL: ButtonType.cancel}
 
 class CarInterface(CarInterfaceBase):
 
@@ -29,6 +34,20 @@ class CarInterface(CarInterfaceBase):
 
     ret.centerToFront = ret.wheelbase * 0.41
 
+    if params.get_bool("CSLCEnabled"):
+        # Used for CEM with CSLC
+        ret.openpilotLongitudinalControl = True
+        ret.longitudinalTuning.deadzoneBP = [0.]
+        ret.longitudinalTuning.deadzoneV = [0.9]  # == 2 mph allowable delta
+        ret.stoppingDecelRate = 4.5  # == 10 mph/s
+        ret.longitudinalActuatorDelayLowerBound = 1.
+        ret.longitudinalActuatorDelayUpperBound = 2.
+
+        ret.longitudinalTuning.kpBP = [8.94, 7.2, 28.]  # 8.94 m/s == 20 mph
+        ret.longitudinalTuning.kpV = [0., 4., 2.]  # set lower end to 0 since we can't drive below that speed
+        ret.longitudinalTuning.kiBP = [0.]
+        ret.longitudinalTuning.kiV = [0.1]
+
     return ret
 
   # returns a car.CarState
@@ -37,6 +56,7 @@ class CarInterface(CarInterfaceBase):
 
      # TODO: add button types for inc and dec
     ret.buttonEvents = [
+      *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT),
       *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
       *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
     ]

selfdrive/car/mazda/mazdacan.py

@@ -1,4 +1,5 @@
 from openpilot.selfdrive.car.mazda.values import Buttons, MazdaFlags
+from openpilot.common.conversions import Conversions as CV
 
 
 def create_steering_control(packer, CP, frame, apply_steer, lkas):
@@ -92,20 +93,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": inc,
-      "SET_P_INV": 1,
+      "SET_P_INV": (inc + 1) % 2,
 
       "RES": res,
       "RES_INV": (res + 1) % 2,
 
-      "SET_M": 0,
+      "SET_M": dec,
-      "SET_M_INV": 1,
+      "SET_M_INV": (dec + 1) % 2,
 
       "DISTANCE_LESS": 0,
       "DISTANCE_LESS_INV": 1,
@@ -126,3 +129,33 @@ def create_button_cmd(packer, CP, counter, button):
     }
 
     return packer.make_can_msg("CRZ_BTNS", 0, values)
+
+def create_mazda_acc_spam_command(packer, controller, CS, slcSet, Vego, frogpilot_variables, accel):
+  cruiseBtn = Buttons.NONE
+
+  MS_CONVERT = CV.MS_TO_KPH if frogpilot_variables.is_metric else CV.MS_TO_MPH
+
+  speedSetPoint = int(round(CS.out.cruiseState.speed * MS_CONVERT))
+  slcSet = int(round(slcSet * MS_CONVERT))
+
+  if not frogpilot_variables.experimentalMode:
+    if slcSet + 5 < Vego * MS_CONVERT:
+      slcSet = slcSet - 10 # 10 lower to increase deceleration until with 5
+  else:
+    slcSet = int(round((Vego + 5 * accel) * MS_CONVERT))
+
+  if frogpilot_variables.is_metric: # Default is by 5 kph
+    slcSet = int(round(slcSet/5.0)*5.0)
+    speedSetPoint = int(round(speedSetPoint/5.0)*5.0)
+
+  if slcSet < speedSetPoint and speedSetPoint > (30 if frogpilot_variables.is_metric else 20):
+    cruiseBtn = Buttons.SET_MINUS
+  elif slcSet > speedSetPoint:
+    cruiseBtn = Buttons.SET_PLUS
+  else:
+    cruiseBtn = Buttons.NONE
+
+  if (cruiseBtn != Buttons.NONE):
+    return [create_button_cmd(packer, controller.CP, controller.frame // 10, cruiseBtn)]
+  else:
+    return []

selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc

@@ -113,7 +113,8 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
   std::vector<std::tuple<QString, QString, QString, QString>> vehicleToggles {
     {"LongPitch", tr("Long Pitch Compensation"), tr("Smoothen out the gas and pedal controls."), ""},
     {"GasRegenCmd", tr("Truck Tune"), tr("Increase the acceleration and smoothen out the brake control when coming to a stop. For use on Silverado/Sierra only."), ""},
-    {"CSLCEnabled", tr("GM CSLC"), tr("Set cars cruise speed based on SLC, MTSC, VTSC, & CEM.\n\nTurns OpenPilot Longitudnal Control off for camera ACC cars."), ""},
+
+    {"CSLCEnabled", tr("CSLC"), tr("Set cars cruise speed based on SLC, MTSC, VTSC, & CEM.\n\nTurns OpenPilot Longitudnal Control off."), ""},
 
     {"CrosstrekTorque", tr("Subaru Crosstrek Torque Increase"), tr("Increases the maximum allowed torque for the Subaru Crosstrek."), ""},
 
@@ -243,6 +244,7 @@ void FrogPilotVehiclesPanel::hideToggles() {
   selectModelButton->setVisible(!carMake.isEmpty());
 
   bool gm = carMake == "Buick" || carMake == "Cadillac" || carMake == "Chevrolet" || carMake == "GM" || carMake == "GMC";
+  bool mazda = carMake == "Mazda";
   bool subaru = carMake == "Subaru";
   bool toyota = carMake == "Lexus" || carMake == "Toyota";
 
@@ -277,6 +279,8 @@ void FrogPilotVehiclesPanel::hideToggles() {
         toggle->setVisible(subaruKeys.find(key.c_str()) != subaruKeys.end());
       } else if (toyota) {
         toggle->setVisible(toyotaKeys.find(key.c_str()) != toyotaKeys.end());
+      } else if (mazda) {
+        toggle->setVisible(mazdaKeys.find(key.c_str()) != mazdaKeys.end());
       }
     }
   }

selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h

@@ -30,6 +30,7 @@ private:
   QStringList models;
 
   std::set<QString> gmKeys = {"CSLCEnabled", "GasRegenCmd", "LongPitch"};
+  std::set<QString> mazdaKeys = {"CSLCEnabled"};
   std::set<QString> subaruKeys = {"CrosstrekTorque"};
   std::set<QString> toyotaKeys = {"ClusterOffset", "ToyotaTune", "SNGHack", "ToyotaDoors"};