diff --git a/CHANGELOGS.md b/CHANGELOGS.md index e6066f7545..1b3e254db6 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -11,6 +11,8 @@ sunnypilot - 0.9.5.1 (2023-09-27) * GAP - Driving Personality * DLP - Dynamic Lane Profile * SLC - Speed Limit Control +* NEW❗: Subaru - Stop and Go auto-resume support thanks to martinl! + * Global (excluding Gen 2 and Hybrid) and Pre-Global support * NEW❗: Toyota - Stop and Go hack * Allow some Toyota/Lexus cars to auto resume during stop and go traffic * Only applicable to certain models and model years diff --git a/common/params.cc b/common/params.cc index dcaf7fc2db..585e9c5c67 100644 --- a/common/params.cc +++ b/common/params.cc @@ -290,6 +290,7 @@ std::unordered_map keys = { {"SpeedLimitOffsetType", PERSISTENT}, {"StandStillTimer", PERSISTENT}, {"StockLongToyota", PERSISTENT}, + {"SubaruManualParkingBrakeSng", PERSISTENT}, {"TorqueDeadzoneDeg", PERSISTENT}, {"TorqueFriction", PERSISTENT}, {"TorqueMaxLatAccel", PERSISTENT}, diff --git a/panda b/panda index 235217badc..a83841a157 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 235217badc8d88d6d21def19cd93ba8cc12ae8c0 +Subproject commit a83841a157e3c41765f6b8666a4e50c318d7a415 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 03a2d01235..6c22b75103 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,14 +1,20 @@ +from cereal import car +from typing import Tuple from openpilot.common.numpy_fast import clip, interp +from openpilot.common.params import Params from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance from openpilot.selfdrive.car.subaru import subarucan -from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, STEER_RATE_LIMITED, CanBus, CarControllerParams, SubaruFlags +from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, STEER_RATE_LIMITED, CanBus, CarControllerParams, SubaruFlags, SubaruFlagsSP # FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and # involves the total steering angle change rather than rate, but these limits work well for now MAX_STEER_RATE = 25 # deg/s MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut +_SNG_ACC_MIN_DIST = 3 +_SNG_ACC_MAX_DIST = 4.5 + class CarController: def __init__(self, dbc_name, CP, VM): @@ -19,6 +25,21 @@ class CarController: self.cruise_button_prev = 0 self.steer_rate_counter = 0 + self.param_s = Params() + + if CP.spFlags & SubaruFlagsSP.SP_SUBARU_SNG: + self.subaru_sng = True + self.manual_parking_brake = self.param_s.get_bool("SubaruManualParkingBrakeSng") + self.throttle_cnt = -1 + self.brake_pedal_cnt = -1 + self.prev_close_distance = 0 + self.prev_standstill = False + self.standstill_start = 0 + self.sng_acc_resume = False + self.sng_acc_resume_cnt = -1 + self.manual_hold = False + self.prev_cruise_state = 0 + self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) @@ -27,6 +48,9 @@ class CarController: hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel + if self.frame % 250 == 0 and self.subaru_sng: + self.manual_parking_brake = self.param_s.get_bool("SubaruManualParkingBrakeSng") + can_sends = [] # *** steering *** @@ -56,6 +80,9 @@ class CarController: self.apply_steer_last = apply_steer + # *** stop and go *** + throttle_cmd, speed_cmd = self.stop_and_go(CC, CS) + # *** longitudinal *** if CC.longActive: @@ -92,6 +119,9 @@ class CarController: can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg)) + if self.subaru_sng: + can_sends.append(subarucan.create_preglobal_throttle(self.packer, 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, self.CP.openpilotLongitudinalControl, @@ -104,6 +134,12 @@ class CarController: if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert)) + if self.subaru_sng: + can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg, throttle_cmd)) + + if self.frame % 2 == 0: + can_sends.append(subarucan.create_brake_pedal(self.packer, CS.brake_pedal_msg, speed_cmd, pcm_cancel_cmd)) + if self.CP.openpilotLongitudinalControl: if self.frame % 5 == 0: can_sends.append(subarucan.create_es_status(self.packer, self.frame // 5, CS.es_status_msg, @@ -125,3 +161,58 @@ class CarController: self.frame += 1 return new_actuators, can_sends + + # Stop and Go auto-resume thanks to martinl from subaru-community + def stop_and_go(self, CC: car.CarControl, CS: car.CarState, throttle_cmd: bool = False, speed_cmd: bool = False) -> Tuple[bool, bool]: + if not self.subaru_sng: + return throttle_cmd, speed_cmd + if self.CP.carFingerprint in PREGLOBAL_CARS: + # Initiate the ACC resume sequence if conditions are met + if (CC.enabled # ACC active + and CS.car_follow == 1 # lead car + and CS.out.standstill # must be standing still + and CS.close_distance > _SNG_ACC_MIN_DIST # acc resume trigger low threshold + and CS.close_distance < _SNG_ACC_MAX_DIST # acc resume trigger high threshold + and CS.close_distance > self.prev_close_distance): # distance with lead car is increasing + self.sng_acc_resume = True + elif self.CP.carFingerprint not in (GLOBAL_GEN2 | HYBRID_CARS): + if self.manual_parking_brake: + # Send brake message with non-zero speed in standstill to avoid non-EPB ACC disengage + if (CC.enabled # ACC active + and CS.car_follow == 1 # lead car + and CS.out.standstill + and self.frame > self.standstill_start + 50): # standstill for >0.5 second + speed_cmd = True + else: + # Record manual hold set while in standstill and no car in front + if CS.out.standstill and self.prev_cruise_state == 1 and CS.cruise_state == 3 and CS.car_follow == 0: + self.manual_hold = True + # Cancel manual hold when car starts moving + if not CS.out.standstill: + self.manual_hold = False + # Initiate the ACC resume sequence if conditions are met + if (CC.enabled # ACC active + and not self.manual_hold + and CS.car_follow == 1 # lead car + and CS.cruise_state == 3 # ACC HOLD (only with EPB) + and CS.close_distance > _SNG_ACC_MIN_DIST # acc resume trigger low threshold + and CS.close_distance < _SNG_ACC_MAX_DIST # acc resume trigger high threshold + and CS.close_distance > self.prev_close_distance): # distance with lead car is increasing + self.sng_acc_resume = 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/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index ce352e9da6..aa4e8acc8f 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, CanBus, SubaruFlags +from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, CanBus, SubaruFlags, SubaruFlagsSP from openpilot.selfdrive.car import CanSignalRateCalculator @@ -123,8 +123,16 @@ class CarState(CarStateBase): self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"]) self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"]) + if self.CP.spFlags & SubaruFlagsSP.SP_SUBARU_SNG: + self.cruise_state = cp_cam.vl["ES_DashStatus"]["Cruise_State"] + self.brake_pedal_msg = copy.copy(cp.vl["Brake_Pedal"]) + if self.car_fingerprint not in HYBRID_CARS: self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) + if self.CP.spFlags & SubaruFlagsSP.SP_SUBARU_SNG: + self.throttle_msg = copy.copy(cp.vl["Throttle"]) + self.car_follow = cp_es_distance.vl["ES_Distance"]["Car_Follow"] + self.close_distance = cp_es_distance.vl["ES_Distance"]["Close_Distance"] self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"]) if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index bc157589ff..0780ee4351 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -2,7 +2,7 @@ from cereal import car from panda import Panda from openpilot.selfdrive.car import get_safety_config, create_mads_event from openpilot.selfdrive.car.interfaces import CarInterfaceBase -from openpilot.selfdrive.car.subaru.values import CAR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags +from openpilot.selfdrive.car.subaru.values import CAR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags, SubaruFlagsSP ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -35,6 +35,11 @@ class CarInterface(CarInterfaceBase): if candidate in GLOBAL_GEN2: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2 + if candidate not in (GLOBAL_GEN2 | HYBRID_CARS): + ret.autoResumeSng = True + ret.spFlags |= SubaruFlagsSP.SP_SUBARU_SNG.value + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_SNG + ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index f73c23420b..411b034323 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -291,3 +291,67 @@ def create_preglobal_es_distance(packer, cruise_button, es_distance_msg): values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_Distance") return packer.make_can_msg("ES_Distance", CanBus.main, values) + + +def create_brake_pedal(packer, brake_pedal_msg, speed_cmd, brake_cmd): + values = {s: brake_pedal_msg[s] for s in [ + "COUNTER", + "Signal1", + "Speed", + "Signal2", + "Brake_Lights", + "Signal3", + "Brake_Pedal", + "Signal4", + ]} + + 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, throttle_msg, throttle_cmd): + values = {s: throttle_msg[s] for s in [ + "CHECKSUM", + "COUNTER", + "Signal1", + "Engine_RPM", + "Signal2", + "Throttle_Pedal", + "Throttle_Cruise", + "Throttle_Combo", + "Signal3", + "Off_Accel", + ]} + + if throttle_cmd: + values["Throttle_Pedal"] = 5 + + return packer.make_can_msg("Throttle", 2, values) + + +def create_preglobal_throttle(packer, throttle_msg, throttle_cmd): + values = {s: throttle_msg[s] for s in [ + "Throttle_Pedal", + "COUNTER", + "Signal1", + "Not_Full_Throttle", + "Signal2", + "Engine_RPM", + "Off_Throttle", + "Signal3", + "Throttle_Cruise", + "Throttle_Combo", + "Throttle_Body", + "Off_Throttle_2", + "Signal4", + ]} + + if throttle_cmd: + values["Throttle_Pedal"] = 5 + + return packer.make_can_msg("Throttle", 2, values) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 1547876008..19c50fa8da 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -60,6 +60,10 @@ class SubaruFlags(IntFlag): SEND_INFOTAINMENT = 1 +class SubaruFlagsSP(IntFlag): + SP_SUBARU_SNG = 1 + + class CanBus: main = 0 alt = 1 diff --git a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc index a3ee8a376e..dc989452b8 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc @@ -562,6 +562,17 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(SPVehiclesPanel *parent) : ListWi hkgSmoothStop->setConfirmation(true, false); addItem(hkgSmoothStop); + // Subaru + addItem(new LabelControl(tr("Subaru"))); + auto subaruManualParkingBrakeSng = new ParamControl( + "SubaruManualParkingBrakeSng", + tr("Manual Parking Brake: Stop and Go (Beta)"), + tr("Experimental feature to enable stop and go for Subaru Global models with manual handbrake. Models with electric parking brake should keep this disabled. Thanks to martinl for this implementation!"), + "../assets/offroad/icon_blank.png" + ); + subaruManualParkingBrakeSng->setConfirmation(true, false); + addItem(subaruManualParkingBrakeSng); + // Toyota/Lexus addItem(new LabelControl(tr("Toyota/Lexus"))); stockLongToyota = new ParamControl(