From 7f9430751c6f0b1099e8a83316fa8ebfff3ea7a8 Mon Sep 17 00:00:00 2001 From: James <91348155+FrogAi@users.noreply.github.com> Date: Mon, 1 Dec 2025 12:00:00 -0700 Subject: [PATCH] "Stop-and-Go" for Subaru Co-Authored-By: martinl <148686+martinl@users.noreply.github.com> --- frogpilot/common/frogpilot_variables.py | 2 + .../opendbc/car/subaru/carcontroller.py | 65 +++++++++++++++++++ opendbc_repo/opendbc/car/subaru/carstate.py | 7 ++ opendbc_repo/opendbc/car/subaru/interface.py | 2 +- opendbc_repo/opendbc/car/subaru/subarucan.py | 65 +++++++++++++++++++ 5 files changed, 140 insertions(+), 1 deletion(-) diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index a7abd092..f02ecca5 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -729,6 +729,8 @@ class FrogPilotVariables: toggle.startup_alert_top = self.get_value("StartupMessageTop", cast=str, default="") toggle.startup_alert_bottom = self.get_value("StartupMessageBottom", cast=str, default="") + toggle.subaru_sng = self.get_value("SubaruSNG", condition=toggle.car_make == "subaru" and not (CP.flags & SubaruFlags.GLOBAL_GEN2 or CP.flags & SubaruFlags.HYBRID)) + toggle.tethering_config = self.get_value("TetheringEnabled", cast=float) toyota_doors = self.get_value("ToyotaDoors", condition=toggle.car_make == "toyota") diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index fcb7c0d5..8b192e86 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -12,6 +12,8 @@ MAX_STEER_RATE = 25 # deg/s MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut # FrogPilot variables +_SNG_ACC_MIN_DIST = 3 +_SNG_ACC_MAX_DIST = 4.5 class CarController(CarControllerBase): @@ -26,6 +28,14 @@ class CarController(CarControllerBase): self.packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) # FrogPilot variables + self.manual_hold = False + self.prev_standstill = False + self.sng_acc_resume = False + + self.prev_close_distance = 0 + self.prev_cruise_state = 0 + self.sng_acc_resume_cnt = 0 + self.standstill_start = 0 def update(self, CC, CS, now_nanos, frogpilot_toggles): actuators = CC.actuators @@ -62,6 +72,9 @@ class CarController(CarControllerBase): self.apply_torque_last = apply_torque # FrogPilot variables + # *** stop and go *** + if frogpilot_toggles.subaru_sng: + throttle_cmd, speed_cmd = self.stop_and_go(CC, CS) # *** longitudinal *** @@ -100,6 +113,8 @@ class CarController(CarControllerBase): can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg)) # FrogPilot variables + if frogpilot_toggles.subaru_sng: + can_sends.append(subarucan.create_preglobal_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, CS.throttle_msg, throttle_cmd)) else: if self.frame % 10 == 0: can_sends.append(subarucan.create_es_dashstatus(self.packer, self.frame // 10, CS.es_dashstatus_msg, CC.enabled, @@ -113,6 +128,10 @@ class CarController(CarControllerBase): can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert)) # FrogPilot variables + if frogpilot_toggles.subaru_sng: + can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, CS.throttle_msg, throttle_cmd)) + if self.frame % 2 == 0: + can_sends.append(subarucan.create_brake_pedal(self.packer, self.frame // 2, CS.brake_pedal_msg, speed_cmd, pcm_cancel_cmd)) if self.CP.openpilotLongitudinalControl: if self.frame % 5 == 0: @@ -153,3 +172,49 @@ class CarController(CarControllerBase): return new_actuators, can_sends # FrogPilot variables + def stop_and_go(self, CC, CS, speed_cmd=False, throttle_cmd=False): + if self.CP.flags & SubaruFlags.PREGLOBAL: + trigger_resume = CC.enabled + trigger_resume &= CS.car_follow == 1 + trigger_resume &= CS.close_distance > self.prev_close_distance + trigger_resume &= CS.out.standstill + trigger_resume &= _SNG_ACC_MIN_DIST < CS.close_distance < _SNG_ACC_MAX_DIST + + if trigger_resume: + self.sng_acc_resume = True + else: + if CS.car_follow == 0 and CS.cruise_state == 3 and CS.out.standstill and self.prev_cruise_state == 1: + self.manual_hold = True + + if not CS.out.standstill: + self.manual_hold = False + + trigger_resume = CC.enabled + trigger_resume &= CS.car_follow == 1 + trigger_resume &= CS.close_distance > self.prev_close_distance + trigger_resume &= CS.cruise_state == 3 + trigger_resume &= not self.manual_hold + trigger_resume &= _SNG_ACC_MIN_DIST < CS.close_distance < _SNG_ACC_MAX_DIST + + if trigger_resume: + self.sng_acc_resume = True + + if CC.enabled and CS.car_follow == 1 and CS.out.standstill and self.frame > self.standstill_start + 50: + speed_cmd = True + + if CS.out.standstill and not self.prev_standstill: + self.standstill_start = self.frame + + self.prev_standstill = CS.out.standstill + self.prev_cruise_state = CS.cruise_state + + if self.sng_acc_resume: + if self.sng_acc_resume_cnt < 5: + throttle_cmd = True + self.sng_acc_resume_cnt += 1 + else: + self.sng_acc_resume = False + self.sng_acc_resume_cnt = -1 + + self.prev_close_distance = CS.close_distance + return throttle_cmd, speed_cmd diff --git a/opendbc_repo/opendbc/car/subaru/carstate.py b/opendbc_repo/opendbc/car/subaru/carstate.py index 02151e00..e25a137f 100644 --- a/opendbc_repo/opendbc/car/subaru/carstate.py +++ b/opendbc_repo/opendbc/car/subaru/carstate.py @@ -127,6 +127,13 @@ class CarState(CarStateBase): # FrogPilot variables fp_ret = custom.FrogPilotCarState.new_message() + if frogpilot_toggles.subaru_sng: + self.brake_pedal_msg = copy.copy(cp.vl["Brake_Pedal"]) + self.car_follow = cp_es_distance.vl["ES_Distance"]["Car_Follow"] + self.close_distance = cp_es_distance.vl["ES_Distance"]["Close_Distance"] + self.cruise_state = cp_cam.vl["ES_DashStatus"]["Cruise_State"] + self.throttle_msg = copy.copy(cp.vl["Throttle"]) + return ret, fp_ret @staticmethod diff --git a/opendbc_repo/opendbc/car/subaru/interface.py b/opendbc_repo/opendbc/car/subaru/interface.py index b11a987d..0790dc43 100644 --- a/opendbc_repo/opendbc/car/subaru/interface.py +++ b/opendbc_repo/opendbc/car/subaru/interface.py @@ -19,7 +19,7 @@ class CarInterface(CarInterfaceBase): # - to find the Cruise_Activated bit from the car # - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc) ret.dashcamOnly = bool(ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID)) - ret.autoResumeSng = False + ret.autoResumeSng = not (ret.flags & SubaruFlags.GLOBAL_GEN2 or ret.flags & SubaruFlags.HYBRID) # Detect infotainment message sent from the camera if not (ret.flags & SubaruFlags.PREGLOBAL) and 0x323 in fingerprint[2]: diff --git a/opendbc_repo/opendbc/car/subaru/subarucan.py b/opendbc_repo/opendbc/car/subaru/subarucan.py index a8a5458b..66e205dd 100644 --- a/opendbc_repo/opendbc/car/subaru/subarucan.py +++ b/opendbc_repo/opendbc/car/subaru/subarucan.py @@ -337,3 +337,68 @@ def subaru_checksum(address: int, sig, d: bytearray) -> int: # FrogPilot variables +def create_brake_pedal(packer, frame, brake_pedal_msg, speed_cmd, brake_cmd): + values = {s: brake_pedal_msg[s] for s in sorted([ + "Brake_Lights", + "Brake_Pedal", + "Signal1", + "Signal2", + "Signal3", + "Signal4", + "Speed", + ])} + + values["COUNTER"] = frame % 0x10 + + if speed_cmd: + values["Speed"] = 3 + if brake_cmd: + values["Brake_Pedal"] = 5 + values["Brake_Lights"] = 1 + + return packer.make_can_msg("Brake_Pedal", CanBus.camera, values) + + +def create_throttle(packer, frame, throttle_msg, throttle_cmd): + values = {s: throttle_msg[s] for s in sorted([ + "CHECKSUM", + "Engine_RPM", + "Off_Accel", + "Signal1", + "Signal2", + "Signal3", + "Throttle_Combo", + "Throttle_Cruise", + "Throttle_Pedal", + ])} + + values["COUNTER"] = frame % 0x10 + + if throttle_cmd: + values["Throttle_Pedal"] = 5 + + return packer.make_can_msg("Throttle", 2, values) + + +def create_preglobal_throttle(packer, frame, throttle_msg, throttle_cmd): + values = {s: throttle_msg[s] for s in sorted([ + "Engine_RPM", + "Not_Full_Throttle", + "Off_Throttle", + "Off_Throttle_2", + "Signal1", + "Signal2", + "Signal3", + "Signal4", + "Throttle_Body", + "Throttle_Combo", + "Throttle_Cruise", + "Throttle_Pedal", + ])} + + values["COUNTER"] = frame % 0x10 + + if throttle_cmd: + values["Throttle_Pedal"] = 5 + + return packer.make_can_msg("Throttle", 2, values)