From 5ea3ac86614b229dd270ef3f057c7a64f2ce0fe5 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Mon, 29 Jan 2024 11:31:42 -0700 Subject: [PATCH] Merge pull request #62 from garrettpall/frogpilot-gm-acc-spam GM Stock Custom Longitudinal Control --- common/params.cc | 2 + opendbc/gm_global_a_powertrain_generated.dbc | 1 + panda/board/safety/safety_gm.h | 4 +- selfdrive/car/gm/carcontroller.py | 25 +++++++- selfdrive/car/gm/gmcan.py | 63 +++++++++++++++++++ selfdrive/controls/controlsd.py | 3 +- selfdrive/controls/lib/drive_helpers.py | 9 ++- .../frogpilot/functions/frogpilot_planner.py | 11 +++- selfdrive/frogpilot/ui/vehicle_settings.cc | 3 +- 9 files changed, 112 insertions(+), 9 deletions(-) diff --git a/common/params.cc b/common/params.cc index 3bd14992164e52..d6856e18ce3c7f 100644 --- a/common/params.cc +++ b/common/params.cc @@ -241,6 +241,8 @@ std::unordered_map keys = { {"Compass", PERSISTENT}, {"ConditionalExperimental", PERSISTENT}, {"CurrentRandomEvent", PERSISTENT}, + {"CSLCEnabled", PERSISTENT}, + {"CSLCSpeed", PERSISTENT}, {"CurveSensitivity", PERSISTENT}, {"CustomColors", PERSISTENT}, {"CustomIcons", PERSISTENT}, diff --git a/opendbc/gm_global_a_powertrain_generated.dbc b/opendbc/gm_global_a_powertrain_generated.dbc index 56bdd9cbe9423e..0b0d0aac3dc452 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 c636a3e567d8dd..b02241b5d125fa 100644 --- a/panda/board/safety/safety_gm.h +++ b/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); } diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index dac6ff863d5d7d..43fc510eb531bb 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -6,9 +6,10 @@ 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 @@ -16,6 +17,8 @@ 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 diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 790b37f8449c17..274f44c32d440b 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/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 [] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4e61edac23d9e2..9f9768dab7cd15 100644 --- a/selfdrive/controls/controlsd.py +++ b/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 diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index b990ae165dd42e..c79ed3e41d43ce 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/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 diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 79f4f1a35e445a..b2dd8b7971dbcb 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/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) diff --git a/selfdrive/frogpilot/ui/vehicle_settings.cc b/selfdrive/frogpilot/ui/vehicle_settings.cc index f66c58cfc21e05..6c3ddd0ab68148 100644 --- a/selfdrive/frogpilot/ui/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/vehicle_settings.cc @@ -102,6 +102,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil addItem(forceFingerprint); std::vector> 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 rebootKeys = {"GasRegenCmd", "LongPitch", "LowerVolt", "TSS2Tune"};