Merge pull request commaai#62 from garrettpall/frogpilot-gm-acc-spam

GM Stock Custom Longitudinal Control

common/params.cc

@@ -241,6 +241,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"Compass", PERSISTENT},
     {"ConditionalExperimental", PERSISTENT},
     {"CurrentRandomEvent", PERSISTENT},
+    {"CSLCEnabled", PERSISTENT},
+    {"CSLCSpeed", PERSISTENT},
     {"CurveSensitivity", PERSISTENT},
     {"CustomColors", PERSISTENT},
     {"CustomIcons", PERSISTENT},

opendbc/gm_global_a_powertrain_generated.dbc

@@ -165,6 +165,7 @@ BO_ 481 ASCMSteeringButton: 7 K124_ASCM
  SG_ DistanceButton : 22|1@0+ (1,0) [0|0] ""  NEO
  SG_ LKAButton : 23|1@0+ (1,0) [0|0] ""  NEO
  SG_ ACCAlwaysOne : 24|1@0+ (1,0) [0|1] "" XXX
+ SG_ ACCByFive : 26|2@0+ (1,0) [0|3] "" XXX
  SG_ ACCButtons : 46|3@0+ (1,0) [0|0] ""  NEO
  SG_ DriveModeButton : 39|1@0+ (1,0) [0|1] "" XXX
  SG_ RollingCounter : 33|2@0+ (1,0) [0|3] "" NEO

panda/board/safety/safety_gm.h

@@ -44,7 +44,7 @@ const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8
                                       {0x1E1, 2, 7}, {0x184, 2, 8}};  // camera bus
 
 const CanMsg GM_SDGM_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7},  // pt bus
-                                  {0x184, 2, 8}};  // camera bus
+                                  {0x184, 2, 8}, {0x1E1, 2, 7}};  // camera bus
 
 const CanMsg GM_CC_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7},  // pt bus
                                      {0x184, 2, 8}, {0x1E1, 2, 7}};  // camera bus
@@ -241,7 +241,7 @@ static bool gm_tx_hook(const CANPacket_t *to_send) {
 
     bool allowed_btn = (button == GM_BTN_CANCEL) && cruise_engaged_prev;
     // For standard CC, allow spamming of SET / RESUME
-    if (gm_cc_long) {
+    if (gm_cc_long || (gm_hw == GM_SDGM) || (gm_hw == GM_CAM && gm_pcm_cruise)) {
       allowed_btn |= cruise_engaged_prev && (button == GM_BTN_SET || button == GM_BTN_RESUME || button == GM_BTN_UNPRESS);
     }
 

selfdrive/car/gm/carcontroller.py

@@ -6,16 +6,19 @@
 from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import apply_driver_steer_torque_limits, create_gas_interceptor_command
 from openpilot.selfdrive.car.gm import gmcan
-from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, EV_CAR, SDGM_CAR
-from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone
+from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, EV_CAR, SDGM_CAR, CAMERA_ACC_CAR
+from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone, V_CRUISE_MAX
 from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.common.params import Params
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 NetworkLocation = car.CarParams.NetworkLocation
 LongCtrlState = car.CarControl.Actuators.LongControlState
 GearShifter = car.CarState.GearShifter
 TransmissionType = car.CarParams.TransmissionType
 
+params_memory = Params("/dev/shm/params")
+
 # Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s
 CAMERA_CANCEL_DELAY_FRAMES = 10
 # Enforce a minimum interval between steering messages to avoid a fault
@@ -232,6 +235,13 @@ def update(self, CC, CS, now_nanos, frogpilot_variables):
           else:
             can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
 
+      # ACC Spam
+      if (self.CP.carFingerprint in SDGM_CAR or self.CP.carFingerprint in CAMERA_ACC_CAR) and frogpilot_variables.CSLC:
+        if CC.enabled and self.frame % 3 == 0 and CS.cruise_buttons == CruiseButtons.UNPRESS and not CS.out.gasPressed:
+          bus = CanBus.CAMERA if self.CP.carFingerprint in SDGM_CAR else CanBus.POWERTRAIN
+          slcSet = get_set_speed(self, hud_v_cruise)
+          can_sends.extend(gmcan.create_gm_acc_spam_command(self.packer_pt, self, CS, slcSet, bus))
+
     if self.CP.networkLocation == NetworkLocation.fwdCamera:
       # Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
       if self.frame % 10 == 0:
@@ -247,3 +257,14 @@ def update(self, CC, CS, now_nanos, frogpilot_variables):
 
     self.frame += 1
     return new_actuators, can_sends
+
+def get_set_speed(self, hud_v_cruise):
+  v_cruise_kph = min(hud_v_cruise * CV.MS_TO_KPH, V_CRUISE_MAX)
+  v_cruise = int(round(v_cruise_kph * CV.KPH_TO_MPH))
+
+  v_cruise_slc: int = 0
+  v_cruise_slc = params_memory.get_float("CSLCSpeed")
+
+  if v_cruise_slc > 0:
+    v_cruise = v_cruise_slc
+  return v_cruise

selfdrive/car/gm/gmcan.py

@@ -12,6 +12,7 @@ def create_buttons(packer, bus, idx, button):
     "RollingCounter": idx,
     "ACCAlwaysOne": 1,
     "DistanceButton": 0,
+    "ACCByFive": 0,
   }
 
   checksum = 240 + int(values["ACCAlwaysOne"] * 0xf)
@@ -22,6 +23,37 @@ def create_buttons(packer, bus, idx, button):
   values["SteeringButtonChecksum"] = checksum
   return packer.make_can_msg("ASCMSteeringButton", bus, values)
 
+def create_buttons_five(packer, bus, idx, button, byfive):
+  if byfive and button == CruiseButtons.RES_ACCEL:
+    accbyfive = 2
+    ACCAlwaysOne = 1
+    checkoffset = -4
+  elif byfive and button == CruiseButtons.DECEL_SET:
+    accbyfive = 3
+    ACCAlwaysOne = 0
+    checkoffset = 255*idx+10
+  else:
+    accbyfive = 0
+    ACCAlwaysOne = 1
+    checkoffset = 0
+
+  values = {
+    "ACCButtons": button,
+    "RollingCounter": idx,
+    "ACCAlwaysOne": ACCAlwaysOne,
+    "DistanceButton": 0,
+    "ACCByFive": accbyfive,
+  }
+
+  checksum = 240 + int(values["ACCAlwaysOne"] * 0xf)
+  checksum += values["RollingCounter"] * (0x4ef if values["ACCAlwaysOne"] != 0 else 0x3f0)
+  checksum -= int(values["ACCButtons"] - 1) << 4  # not correct if value is 0
+  checksum -= 2 * values["DistanceButton"]
+  checksum += checkoffset
+
+  values["SteeringButtonChecksum"] = checksum
+  return packer.make_can_msg("ASCMSteeringButton", bus, values)
+
 
 def create_pscm_status(packer, bus, pscm_status):
   values = {s: pscm_status[s] for s in [
@@ -217,3 +249,34 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators):
     return [create_buttons(packer, CanBus.POWERTRAIN, idx, cruiseBtn)]
   else:
     return []
+
+def create_gm_acc_spam_command(packer, controller, CS, slcSet, bus):
+  cruiseBtn = CruiseButtons.INIT
+  byfive = 0
+  speedSetPoint = int(round(CS.out.cruiseState.speed * CV.MS_TO_MPH))
+
+  FRAMES_ON = 12
+  FRAMES_OFF = 18
+
+  if slcSet <= int(math.floor((speedSetPoint - 1)/5.0)*5.0) and speedSetPoint > 20:
+    cruiseBtn = CruiseButtons.DECEL_SET
+    byfive = 1
+  elif slcSet >= int(math.ceil((speedSetPoint + 1)/5.0)*5.0):
+    cruiseBtn = CruiseButtons.RES_ACCEL
+    byfive = 1
+  elif slcSet < speedSetPoint and speedSetPoint > 16:
+    cruiseBtn = CruiseButtons.DECEL_SET
+    byfive = 0
+  elif slcSet > speedSetPoint:
+    cruiseBtn = CruiseButtons.RES_ACCEL
+    byfive = 0
+  else:
+    cruiseBtn = CruiseButtons.INIT
+    byfive = 0
+
+  if (cruiseBtn != CruiseButtons.INIT) and controller.frame % (FRAMES_ON + FRAMES_OFF) < FRAMES_ON:
+    controller.last_button_frame = controller.frame
+    idx = (CS.buttons_counter + 1) % 4
+    return [create_buttons_five(packer, bus, idx, cruiseBtn, byfive)]
+  else:
+    return []

selfdrive/controls/controlsd.py

@@ -614,7 +614,7 @@ def state_transition(self, CS):
           else:
             self.state = State.enabled
           self.current_alert_types.append(ET.ENABLE)
-          self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.conditional_experimental_mode)
+          self.v_cruise_helper.initialize_v_cruise(CS, self.frogpilot_variables, self.experimental_mode, self.conditional_experimental_mode)
 
     # Check if openpilot is engaged and actuators are enabled
     self.enabled = self.state in ENABLED_STATES
@@ -1002,6 +1002,7 @@ def controlsd_thread(self):
 
   def update_frogpilot_params(self):
     self.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental")
+    self.frogpilot_variables.CSLC = self.params.get_bool("CSLCEnabled")
 
     custom_theme = self.params.get_bool("CustomTheme")
     custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0

selfdrive/controls/lib/drive_helpers.py

@@ -53,7 +53,7 @@ def update_v_cruise(self, CS, enabled, is_metric, frogpilot_variables):
     self.v_cruise_kph_last = self.v_cruise_kph
 
     if CS.cruiseState.available:
-      if not self.CP.pcmCruise:
+      if not self.CP.pcmCruise or frogpilot_variables.CSLC:
         # if stock cruise is completely disabled, then we can use our own set speed logic
         self._update_v_cruise_non_pcm(CS, enabled, is_metric, frogpilot_variables)
         self.v_cruise_cluster_kph = self.v_cruise_kph
@@ -136,7 +136,12 @@ def update_button_timers(self, CS, enabled):
 
   def initialize_v_cruise(self, CS, experimental_mode: bool, conditional_experimental_mode) -> None:
     # initializing is handled by the PCM
-    if self.CP.pcmCruise:
+    if self.CP.pcmCruise and not frogpilot_variables.CSLC:
+      return
+
+    if frogpilot_variables.CSLC:
+      self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+      self.v_cruise_cluster_kph = self.v_cruise_kph
       return
 
     initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode and not conditional_experimental_mode else V_CRUISE_INITIAL

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -18,6 +18,7 @@
 
 class FrogPilotPlanner:
   def __init__(self, params, params_memory):
+    self.params_memory = params_memory
     self.cem = ConditionalExperimentalMode()
     self.mtsc = MapTurnSpeedController()
 
@@ -69,6 +70,8 @@ def update(self, carState, controlsState, modelData, mpc, sm, v_cruise, v_ego):
       self.lane_width_left = 0
       self.lane_width_right = 0
 
+    self.params_memory.put_float("CSLCSpeed", self.v_cruise * CV.MS_TO_MPH)
+
   def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise, v_ego):
     # Pfeiferj's Map Turn Speed Controller
     if self.map_turn_speed_controller:
@@ -128,7 +131,11 @@ def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise,
     else:
       self.vtsc_target = v_cruise
 
-    v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
+    if self.CSLC:
+      v_ego_diff = 0
+    else:
+      v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
+
     return min(v_cruise, self.mtsc_target, self.slc_target, self.vtsc_target) - v_ego_diff
 
   def publish(self, sm, pm, mpc):
@@ -161,6 +168,8 @@ def publish(self, sm, pm, mpc):
   def update_frogpilot_params(self, params, params_memory):
     self.is_metric = params.get_bool("IsMetric")
 
+    self.CSLC = params.get_bool("CSLCEnabled")
+
     self.conditional_experimental_mode = params.get_bool("ConditionalExperimental")
     if self.conditional_experimental_mode:
       self.cem.update_frogpilot_params(self.is_metric, params)

selfdrive/frogpilot/ui/vehicle_settings.cc

@@ -102,6 +102,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
   addItem(forceFingerprint);
 
   std::vector<std::tuple<QString, QString, QString, QString>> vehicleToggles {
+    {"CSLCEnabled", "GM SDGM CSLC", "Set cars cruise speed based on SLC, MTSC, and VTSC.", ""},
     {"EVTable", "EV Lookup Tables", "Smoothen out the gas and brake controls for EV vehicles.", ""},
     {"GasRegenCmd", "GM Truck Gas Tune", "Increase acceleration and smoothen brake to stop. For use on Silverado/Sierra only.", ""},
     {"LongPitch", "Long Pitch Compensation", "Reduce speed and acceleration error for greater passenger comfort and improved vehicle efficiency.", ""},
@@ -124,7 +125,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
     });
   }
 
-  gmKeys = {"EVTable", "GasRegenCmd", "LongPitch", "LowerVolt"};
+  gmKeys = {"CSLCEnabled", "EVTable", "GasRegenCmd", "LongPitch", "LowerVolt"};
   toyotaKeys = {"LockDoors", "SNGHack", "TSS2Tune"};
 
   std::set<std::string> rebootKeys = {"GasRegenCmd", "LongPitch", "LowerVolt", "TSS2Tune"};