From 1b9fc72d80b0ccc45e5303dee490e9d7f4792623 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sat, 25 May 2024 02:39:44 -0700 Subject: [PATCH] GM Stock Custom Longitudinal Control Co-Authored-By: garrettpall <76917194+garrettpall@users.noreply.github.com> --- common/params.cc | 2 + opendbc/gm_global_a_powertrain_generated.dbc | 1 + panda/board/safety/safety_gm.h | 9 ++- panda/python/__init__.py | 1 + selfdrive/car/card.py | 2 +- selfdrive/car/gm/carcontroller.py | 27 ++++++- selfdrive/car/gm/gmcan.py | 73 +++++++++++++++++++ selfdrive/car/gm/interface.py | 38 +++++++++- selfdrive/controls/controlsd.py | 8 ++ selfdrive/controls/lib/drive_helpers.py | 19 ++++- .../frogpilot/controls/frogpilot_planner.py | 7 +- .../controls/lib/frogpilot_variables.py | 4 +- .../ui/qt/offroad/control_settings.cc | 4 +- .../ui/qt/offroad/vehicle_settings.cc | 3 +- .../ui/qt/offroad/vehicle_settings.h | 2 +- 15 files changed, 184 insertions(+), 16 deletions(-) diff --git a/common/params.cc b/common/params.cc index 801cb76..5fad08d 100644 --- a/common/params.cc +++ b/common/params.cc @@ -253,6 +253,8 @@ std::unordered_map keys = { {"Compass", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS}, {"ConditionalExperimental", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"CrosstrekTorque", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, + {"CSLCEnabled", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, + {"CSLCSpeed", PERSISTENT}, {"CurrentHolidayTheme", PERSISTENT}, {"CurrentRandomEvent", PERSISTENT}, {"CurveSensitivity", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, diff --git a/opendbc/gm_global_a_powertrain_generated.dbc b/opendbc/gm_global_a_powertrain_generated.dbc index 3b40d93..c7ace50 100644 --- a/opendbc/gm_global_a_powertrain_generated.dbc +++ b/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 diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 55a3186..02718ca 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -58,7 +58,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 @@ -84,6 +84,7 @@ const uint16_t GM_PARAM_NO_CAMERA = 32; const uint16_t GM_PARAM_NO_ACC = 64; const uint16_t GM_PARAM_PEDAL_LONG = 128; // TODO: this can be inferred const uint16_t GM_PARAM_PEDAL_INTERCEPTOR = 256; +const uint16_t GM_PARAM_CSLC = 512; enum { GM_BTN_UNPRESS = 1, @@ -105,6 +106,7 @@ bool gm_pedal_long = false; bool gm_cc_long = false; bool gm_skip_relay_check = false; bool gm_force_ascm = false; +bool gm_cslc = false; static void handle_gm_wheel_buttons(const CANPacket_t *to_push) { int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4; @@ -261,8 +263,8 @@ static bool gm_tx_hook(const CANPacket_t *to_send) { int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U; bool allowed_btn = (button == GM_BTN_CANCEL) && cruise_engaged_prev; - // For standard CC, allow spamming of SET / RESUME - if (gm_cc_long) { + // For standard CC and CSLC, allow spamming of SET / RESUME + if (gm_cc_long || (((gm_hw == GM_SDGM) || (gm_hw == GM_CAM)) && gm_cslc)) { allowed_btn |= cruise_engaged_prev && (button == GM_BTN_SET || button == GM_BTN_RESUME || button == GM_BTN_UNPRESS); } @@ -335,6 +337,7 @@ static safety_config gm_init(uint16_t param) { gm_skip_relay_check = GET_FLAG(param, GM_PARAM_NO_CAMERA); gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC); enable_gas_interceptor = GET_FLAG(param, GM_PARAM_PEDAL_INTERCEPTOR); + gm_cslc = GET_FLAG(param, GM_PARAM_CSLC); safety_config ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_ASCM_TX_MSGS); if (gm_hw == GM_CAM) { diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 1827544..8e1dc30 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -239,6 +239,7 @@ class Panda: FLAG_GM_NO_ACC = 64 FLAG_GM_PEDAL_LONG = 128 # TODO: This can be inferred FLAG_GM_GAS_INTERCEPTOR = 256 + FLAG_GM_CSLC = 512 FLAG_FORD_LONG_CONTROL = 1 FLAG_FORD_CANFD = 2 diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 5a6a567..918bdc1 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -59,7 +59,7 @@ class CarD: if self.always_on_lateral: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL - self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX # Also needed for GM CSLC car_recognized = self.CP.carName != 'mock' openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index ac76f4d..ccc3893 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -7,9 +7,9 @@ from openpilot.common.realtime import DT_CTRL 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, SDGM_CAR, EV_CAR +from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR, CAMERA_ACC_CAR from openpilot.selfdrive.car.interfaces import CarControllerBase -from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone +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 VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -18,6 +18,8 @@ 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 @@ -117,7 +119,7 @@ class CarController(CarControllerBase): idx = self.lka_steering_cmd_counter % 4 can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive)) - if self.CP.openpilotLongitudinalControl: + if self.CP.openpilotLongitudinalControl and not frogpilot_variables.CSLC: # Gas/regen, brakes, and UI commands - all at 25Hz if self.frame % 4 == 0: stopping = actuators.longControlState == LongCtrlState.stopping @@ -244,6 +246,15 @@ class CarController(CarControllerBase): 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: + bus = CanBus.CAMERA + if CC.enabled and self.frame % 3 == 0 and CS.cruise_buttons == CruiseButtons.UNPRESS and not CS.out.gasPressed and not CS.distance_button: + slcSet = get_set_speed(self, hud_v_cruise) + can_sends.extend(gmcan.create_gm_acc_spam_command(self.packer_pt, self, CS, slcSet, bus, CS.out.vEgo, frogpilot_variables, accel, self.CP.carFingerprint in SDGM_CAR)) + elif CC.enabled and self.frame % 51 == 0 and CS.cruise_buttons == CruiseButtons.UNPRESS and CS.out.gasPressed and CS.out.cruiseState.speed < CS.out.vEgo < hud_v_cruise and not CS.distance_button: + can_sends.extend([gmcan.create_buttons(self.packer_pt, bus, (CS.buttons_counter + 1) % 4, CruiseButtons.DECEL_SET)] * (25 if self.CP.carFingerprint in SDGM_CAR else 1)) + if self.CP.networkLocation == NetworkLocation.fwdCamera: # Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1 if self.frame % 10 == 0: @@ -259,3 +270,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 diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index ff7538a..2ab71e4 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -13,6 +13,7 @@ def create_buttons(packer, bus, idx, button): "RollingCounter": idx, "ACCAlwaysOne": 1, "DistanceButton": 0, + "ACCByFive": 0, } checksum = 240 + int(values["ACCAlwaysOne"] * 0xf) @@ -23,6 +24,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 [ @@ -222,3 +254,44 @@ 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, Vego, frogpilot_variables, accel, sdgm): + cruiseBtn = CruiseButtons.INIT + byfive = 0 + + 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)) + + FRAMES_ON = 6 + FRAMES_OFF = (30 if sdgm else 12) - FRAMES_ON + + 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 * 1.01 + 4.6 * accel + 0.7 * accel ** 3 - 1 / 35 * accel ** 5) * MS_CONVERT)) # 1.01 factor to match cluster speed better + + if slcSet <= int(math.floor((speedSetPoint - 1)/5.0)*5.0) and speedSetPoint > (25 if frogpilot_variables.is_metric else 20) and sdgm: + cruiseBtn = CruiseButtons.DECEL_SET + byfive = 1 + elif slcSet >= int(math.ceil((speedSetPoint + 1)/5.0)*5.0) and sdgm: + cruiseBtn = CruiseButtons.RES_ACCEL + byfive = 1 + elif slcSet < speedSetPoint and speedSetPoint > (25 if frogpilot_variables.is_metric else 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 if sdgm else 2)) % 4 + return [create_buttons_five(packer, bus, idx, cruiseBtn, byfive)]*(23 if sdgm else 3) + else: + return [] diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index babcd07..0bd36dd 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -12,6 +12,10 @@ from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerP from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel from openpilot.selfdrive.controls.lib.drive_helpers import get_friction +from openpilot.common.params import Params + +params = Params() + ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName GearShifter = car.CarState.GearShifter @@ -95,6 +99,8 @@ class CarInterface(CarInterfaceBase): def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs): params.put_bool("HideDisableOpenpilotLongitudinal", candidate not in (SDGM_CAR | CAMERA_ACC_CAR)) + cslcEnabled = params.get_bool("CSLCEnabled") + ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False @@ -134,11 +140,25 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kpV = [1.5, 1.125] ret.stopAccel = -0.25 - if experimental_long: + if experimental_long and not cslcEnabled: ret.pcmCruise = False ret.openpilotLongitudinalControl = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG + if cslcEnabled: + # Used for CEM with CSLC + ret.openpilotLongitudinalControl = True + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.9] # == 2 mph allowable delta + ret.stoppingDecelRate = 3.25 # == 8.33 mph/s (OFF + ON = 12 frames) + ret.longitudinalActuatorDelayLowerBound = 1. + ret.longitudinalActuatorDelayUpperBound = 2. + + ret.longitudinalTuning.kpBP = [7.15, 7.2, 28.] # 7.15 m/s == 16 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] + elif candidate in SDGM_CAR: ret.experimentalLongitudinalAvailable = False ret.networkLocation = NetworkLocation.fwdCamera @@ -148,6 +168,19 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = 30 * CV.MPH_TO_MS ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_SDGM + # Used for CEM with CSLC + ret.openpilotLongitudinalControl = cslcEnabled + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.9] # == 2 mph allowable delta + ret.stoppingDecelRate = 7.45 # == 16.67 mph/s (OFF + ON = 30 frames) + ret.longitudinalActuatorDelayLowerBound = 1. + ret.longitudinalActuatorDelayUpperBound = 2. + + ret.longitudinalTuning.kpBP = [7.15, 7.2, 28.] # 7.15 m/s == 16 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] + else: # ASCM, OBD-II harness ret.openpilotLongitudinalControl = not disable_openpilot_long ret.networkLocation = NetworkLocation.gateway @@ -307,6 +340,9 @@ class CarInterface(CarInterfaceBase): if ACCELERATOR_POS_MSG not in fingerprint[CanBus.POWERTRAIN]: ret.flags |= GMFlags.NO_ACCELERATOR_POS_MSG.value + if cslcEnabled: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_CSLC + return ret # returns a car.CarState diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8279cbe..43a601c 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -501,6 +501,13 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" + resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) + set_pressed = any(be.type in (ButtonType.decelCruise, ButtonType.setCruise) for be in CS.buttonEvents) + if resume_pressed: + self.frogpilot_toggles.prev_button = ButtonType.resumeCruise + elif set_pressed: + self.frogpilot_toggles.prev_button = ButtonType.setCruise + self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.FPCC.speedLimitChanged, self.frogpilot_toggles) # decrement the soft disable timer at every step, as it's reset on @@ -855,6 +862,7 @@ class Controls: controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter controlsState.experimentalMode = self.experimental_mode controlsState.personality = self.personality + self.frogpilot_toggles.experimentalMode = self.experimental_mode lat_tuning = self.CP.lateralTuning.which() if self.joystick_mode: diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index f8c7ac4..5323ef6 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -57,7 +57,7 @@ class VCruiseHelper: 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, speed_limit_changed, frogpilot_variables) self.v_cruise_cluster_kph = self.v_cruise_kph @@ -148,11 +148,26 @@ class VCruiseHelper: def initialize_v_cruise(self, CS, experimental_mode: bool, desired_speed_limit, frogpilot_variables) -> None: # initializing is handled by the PCM - if self.CP.pcmCruise: + if self.CP.pcmCruise and not frogpilot_variables.CSLC: return initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL + # CSLC resume/set logic + if frogpilot_variables.CSLC: + if frogpilot_variables.prev_button == ButtonType.resumeCruise and self.v_cruise_kph_last < 250: + self.v_cruise_kph = self.v_cruise_kph_last + else: + # Initial set speed + if desired_speed_limit != 0 and frogpilot_variables.set_speed_limit: + # If there's a known speed limit and the corresponding FP toggle is set, push it to the car + self.v_cruise_kph = int(round(desired_speed_limit * CV.MS_TO_KPH)) + else: + # Use fixed initial set speed from mode etc. + self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) + self.v_cruise_cluster_kph = self.v_cruise_kph + return + # 250kph or above probably means we never had a set speed if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: self.v_cruise_kph = self.v_cruise_kph_last diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 01c4c5c..e6ab330 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -173,6 +173,8 @@ class FrogPilotPlanner: self.road_curvature = calculate_road_curvature(modelData, v_ego) self.v_cruise = self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego, frogpilot_toggles) + self.params_memory.put_float("CSLCSpeed", self.v_cruise) + if frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.green_light_alert: self.cem.update(carState, controlsState.enabled, frogpilotNavigation, lead_distance, self.lead_one, modelData, self.road_curvature, self.slower_lead, v_ego, v_lead, frogpilot_toggles) @@ -212,7 +214,10 @@ class FrogPilotPlanner: v_cruise_diff = v_cruise_cluster - v_cruise v_ego_cluster = max(carState.vEgoCluster, v_ego) - v_ego_diff = v_ego_cluster - v_ego + if frogpilot_toggles.CSLC: + v_ego_diff = 0 + else: + v_ego_diff = v_ego_cluster - v_ego # Pfeiferj's Map Turn Speed Controller if frogpilot_toggles.map_turn_speed_controller and v_ego > CRUISING_SPEED and controlsState.enabled and gps_check: diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py index d1a4a3d..16bb962 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py @@ -39,14 +39,16 @@ class FrogPilotVariables: if msg_bytes is None: car_name = "mock" + toggles.CSLC = False openpilot_longitudinal = False pcm_cruise = False else: with car.CarParams.from_bytes(msg_bytes) as msg: CP = msg car_name = CP.carName + toggles.CSLC = self.params.get_bool("CSLCEnabled") openpilot_longitudinal = CP.openpilotLongitudinalControl - pcm_cruise = CP.pcmCruise + pcm_cruise = CP.pcmCruise and not toggles.CSLC toggles.is_metric = self.params.get_bool("IsMetric") distance_conversion = 1 if toggles.is_metric else CV.FOOT_TO_METER diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index faf63a1..2f3b304 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -1011,8 +1011,8 @@ void FrogPilotControlsPanel::updateCarToggles() { hasCommaNNFFSupport = checkCommaNNFFSupport(carFingerprint); hasDashSpeedLimits = carName == "hyundai" || carName == "toyota"; hasNNFFLog = checkNNFFLogFileExists(carFingerprint); - hasOpenpilotLongitudinal = CP.getOpenpilotLongitudinalControl() && !params.getBool("DisableOpenpilotLongitudinal"); - hasPCMCruise = CP.getPcmCruise(); + hasOpenpilotLongitudinal = (CP.getOpenpilotLongitudinalControl() && !params.getBool("DisableOpenpilotLongitudinal")) || params.getBool("CSLCEnabled"); + hasPCMCruise = CP.getPcmCruise() && !params.getBool("CSLCEnabled"); isGM = carName == "gm"; isHKGCanFd = (carName == "hyundai") && (safetyModel == cereal::CarParams::SafetyModel::HYUNDAI_CANFD); isToyota = carName == "toyota"; diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc index aa9b108..5f8043a 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc @@ -113,6 +113,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil std::vector> 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."), ""}, {"CrosstrekTorque", tr("Subaru Crosstrek Torque Increase"), tr("Increases the maximum allowed torque for the Subaru Crosstrek."), ""}, @@ -165,7 +166,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil }); } - std::set rebootKeys = {"CrosstrekTorque", "GasRegenCmd"}; + std::set rebootKeys = {"CrosstrekTorque", "GasRegenCmd", "CSLCEnabled"}; for (const QString &key : rebootKeys) { QObject::connect(static_cast(toggles[key.toStdString().c_str()]), &ToggleControl::toggleFlipped, [this]() { if (started) { diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h index d640536..d6a6401 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h @@ -29,7 +29,7 @@ private: QStringList models; - std::set gmKeys = {"GasRegenCmd", "LongPitch"}; + std::set gmKeys = {"CSLCEnabled", "GasRegenCmd", "LongPitch"}; std::set subaruKeys = {"CrosstrekTorque"}; std::set toyotaKeys = {"ClusterOffset", "ToyotaTune", "SNGHack", "ToyotaDoors"};