From e07e1ee6bfaa13a9b06f500d81c2ba1e35ff735b Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Mon, 10 Jun 2024 06:47:01 -0700 Subject: [PATCH] Controls - Experimental Mode Activation - Double Click LKAS Enable/disable 'Experimental Mode' by double clicking the 'LKAS' button on your steering wheel. --- opendbc/chrysler_pacifica_2017_hybrid_generated.dbc | 1 + opendbc/chrysler_ram_hd_generated.dbc | 6 ++++++ panda/python/__init__.py | 1 + selfdrive/car/chrysler/carstate.py | 10 ++++++++++ selfdrive/car/chrysler/interface.py | 6 +++++- selfdrive/car/ford/carstate.py | 4 ++++ selfdrive/car/ford/interface.py | 6 +++++- selfdrive/car/gm/carstate.py | 6 ++++++ selfdrive/car/gm/interface.py | 5 ++++- selfdrive/car/honda/carstate.py | 4 ++++ selfdrive/car/honda/interface.py | 2 ++ selfdrive/car/hyundai/carstate.py | 12 ++++++++++++ selfdrive/car/hyundai/interface.py | 9 ++++++++- selfdrive/car/hyundai/values.py | 2 ++ selfdrive/car/interfaces.py | 4 ++++ selfdrive/car/mazda/carstate.py | 4 ++++ selfdrive/car/mazda/interface.py | 6 +++++- selfdrive/car/nissan/carstate.py | 1 + selfdrive/car/nissan/interface.py | 6 +++++- selfdrive/car/subaru/carstate.py | 7 ++++++- selfdrive/car/subaru/interface.py | 5 +++++ selfdrive/car/subaru/values.py | 2 ++ selfdrive/car/toyota/carstate.py | 4 ++++ selfdrive/car/toyota/interface.py | 6 +++++- selfdrive/controls/controlsd.py | 9 +++++++++ 25 files changed, 120 insertions(+), 8 deletions(-) diff --git a/opendbc/chrysler_pacifica_2017_hybrid_generated.dbc b/opendbc/chrysler_pacifica_2017_hybrid_generated.dbc index d8f4c6623..98f290168 100644 --- a/opendbc/chrysler_pacifica_2017_hybrid_generated.dbc +++ b/opendbc/chrysler_pacifica_2017_hybrid_generated.dbc @@ -312,6 +312,7 @@ BO_ 764 ACCEL_RELATED_2FC: 8 XXX BO_ 816 TRACTION_BUTTON: 8 XXX SG_ TRACTION_OFF : 19|1@0+ (1,0) [0|3] "" XXX SG_ TOGGLE_PARKSENSE : 52|1@0+ (1,0) [0|3] "" XXX + SG_ TOGGLE_LKAS : 53|1@0+ (1,0) [0|1] "" XXX BO_ 878 ACCEL_RELATED_36E: 8 XXX SG_ ACCEL_OR_RPM_2 : 15|8@0+ (1,0) [0|255] "" XXX diff --git a/opendbc/chrysler_ram_hd_generated.dbc b/opendbc/chrysler_ram_hd_generated.dbc index 697c3bf91..b636b7316 100644 --- a/opendbc/chrysler_ram_hd_generated.dbc +++ b/opendbc/chrysler_ram_hd_generated.dbc @@ -207,3 +207,9 @@ BO_ 630 LKAS_COMMAND: 8 XXX SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX +BO_ 650 Center_Stack_2: 8 XXX + SG_ LKAS_Button : 57|1@1+ (1,0) [0|0] "" XXX + +BO_ 816 Center_Stack_1: 8 XXX + SG_ LKAS_Button : 53|1@1+ (1,0) [0|0] "" XXX + SG_ Traction_Button : 54|1@0+ (1,0) [0|1] "" XXX diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 110d45f50..d31235075 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -209,6 +209,7 @@ class Panda: FLAG_HYUNDAI_CANFD_ALT_BUTTONS = 32 FLAG_HYUNDAI_ALT_LIMITS = 64 FLAG_HYUNDAI_CANFD_HDA2_ALT_STEERING = 128 + FLAG_HYUNDAI_LFA_BTN = 256 FLAG_TESLA_POWERTRAIN = 1 FLAG_TESLA_LONG_CONTROL = 2 diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 05d6c7802..93d72fe4a 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -102,6 +102,13 @@ class CarState(CarStateBase): self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"] self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"] + # FrogPilot CarState functions + self.lkas_previously_enabled = self.lkas_enabled + if self.CP.carFingerprint in RAM_CARS: + self.lkas_enabled = cp.vl["Center_Stack_2"]["LKAS_Button"] or cp.vl["Center_Stack_1"]["LKAS_Button"] + else: + self.lkas_enabled = cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"] == 1 + return ret, fp_ret @staticmethod @@ -135,11 +142,14 @@ class CarState(CarStateBase): ("ESP_8", 50), ("EPS_3", 50), ("Transmission_Status", 50), + ("Center_Stack_1", 1), + ("Center_Stack_2", 1), ] else: messages += [ ("GEAR", 50), ("SPEED_1", 100), + ("TRACTION_BUTTON", 1), ] messages += CarState.get_cruise_messages() diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 94065b2ae..028321ffe 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -6,6 +6,7 @@ from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CAR from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @@ -79,7 +80,10 @@ class CarInterface(CarInterfaceBase): def _update(self, c, frogpilot_toggles): ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) - ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] # events events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low]) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index ab79d091f..db9abdf6c 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -108,6 +108,10 @@ class CarState(CarStateBase): self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] + # FrogPilot CarState functions + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = ret.genericToggle + return ret, fp_ret @staticmethod diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index a723b9dc6..627668b61 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -7,6 +7,7 @@ from openpilot.selfdrive.car.ford.values import Ecu, FordFlags from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type TransmissionType = car.CarParams.TransmissionType GearShifter = car.CarState.GearShifter @@ -70,7 +71,10 @@ class CarInterface(CarInterfaceBase): def _update(self, c, frogpilot_toggles): ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) - ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic]) if not self.CS.vehicle_sensors_valid: diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index e2a8d53c5..2b3f85473 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -172,6 +172,12 @@ class CarState(CarStateBase): # FrogPilot CarState functions fp_ret.hasMenu = not (self.CP.flags & GMFlags.NO_CAMERA.value or self.CP.carFingerprint in CC_ONLY_CAR) + self.lkas_previously_enabled = self.lkas_enabled + if self.CP.carFingerprint in SDGM_CAR: + self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"] + else: + self.lkas_enabled = pt_cp.vl["ASCMSteeringButton"]["LKAButton"] + return ret, fp_ret @staticmethod diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index cde9b057d..de894b4e2 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -13,6 +13,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLater from openpilot.selfdrive.controls.lib.drive_helpers import get_friction ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type EventName = car.CarEvent.EventName GearShifter = car.CarState.GearShifter TransmissionType = car.CarParams.TransmissionType @@ -318,7 +319,9 @@ class CarInterface(CarInterfaceBase): *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, unpressed_btn=CruiseButtons.UNPRESS), *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, - {1: ButtonType.gapAdjustCruise}) + {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, + {1: FrogPilotButtonType.lkas}), ] # The ECM allows enabling on falling edge of set, but only rising edge of resume diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 18ec601ce..554b3b1cb 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -273,6 +273,10 @@ class CarState(CarStateBase): ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1 ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1 + # FrogPilot CarState functions + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = self.cruise_setting == 1 + return ret, fp_ret def get_can_parser(self, CP): diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 10cf5684a..922ea0922 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -12,6 +12,7 @@ from openpilot.selfdrive.car.disable_ecu import disable_ecu ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type EventName = car.CarEvent.EventName TransmissionType = car.CarParams.TransmissionType BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, @@ -253,6 +254,7 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = [ *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT), *create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, SETTINGS_BUTTONS_DICT), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), ] # events diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 0f40cdd49..0b5b50bd0 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -172,6 +172,11 @@ class CarState(CarStateBase): if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0: self.main_enabled = not self.main_enabled + # FrogPilot CarState functions + if self.CP.flags & HyundaiFlags.CAN_LFA_BTN: + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"] + return ret, fp_ret def update_canfd(self, cp, cp_cam, frogpilot_toggles): @@ -258,6 +263,10 @@ class CarState(CarStateBase): self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else cp_cam.vl["CAM_0x2a4"]) + # FrogPilot CarState functions + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"] + return ret, fp_ret def get_can_parser(self, CP): @@ -308,6 +317,9 @@ class CarState(CarStateBase): else: messages.append(("LVR12", 100)) + if CP.flags & HyundaiFlags.CAN_LFA_BTN: + messages.append(("BCM_PO_11", 50)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index ba61fab2d..b08c9a3eb 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -124,6 +124,10 @@ class CarInterface(CarInterfaceBase): if candidate in CAMERA_SCC_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC + if 0x391 in fingerprint[0]: + ret.flags |= HyundaiFlags.CAN_LFA_BTN.value + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LFA_BTN + if ret.openpilotLongitudinalControl: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG if ret.flags & HyundaiFlags.HYBRID: @@ -155,7 +159,10 @@ class CarInterface(CarInterfaceBase): ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) if self.CS.CP.openpilotLongitudinalControl: - ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT) + ret.buttonEvents = [ + *create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] # On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state # To avoid re-engaging when openpilot cancels, check user engagement intention via buttons diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 8cb29c803..3d9a8fc9d 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -95,6 +95,8 @@ class HyundaiFlags(IntFlag): MIN_STEER_32_MPH = 2 ** 23 + # FrogPilot HKG flags + CAN_LFA_BTN = 2 ** 24 class Footnote(Enum): CANFD = CarFootnote( diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 47e4e6dd9..5a848dbaa 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -389,6 +389,10 @@ class CarStateBase(ABC): K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R) self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) + # FrogPilot variables + self.lkas_enabled = False + self.lkas_previously_enabled = False + def update_speed_kf(self, v_ego_raw): if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego_kf.set_x([[v_ego_raw], [0.0]]) diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index bf0065501..b7a64659a 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -111,6 +111,10 @@ class CarState(CarStateBase): self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 + # FrogPilot CarState functions + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = not self.lkas_disabled + return ret, fp_ret @staticmethod diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 8eec5b7df..7c4179c8a 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -6,6 +6,7 @@ from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): @@ -35,7 +36,10 @@ class CarInterface(CarInterfaceBase): ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) # TODO: add button types for inc and dec - ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] # events events = self.create_common_events(ret) diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 5144765e9..3a67c59e4 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -108,6 +108,7 @@ class CarState(CarStateBase): can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + self.lkas_previously_enabled = self.lkas_enabled if self.CP.carFingerprint == CAR.NISSAN_ALTIMA: self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) else: diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 60d676644..c2d94f1de 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -5,6 +5,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.nissan.values import CAR ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @@ -32,7 +33,10 @@ class CarInterface(CarInterfaceBase): def _update(self, c, frogpilot_toggles): ret, fp_ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam, frogpilot_toggles) - ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.brake]) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index fe438d170..0a46bf3ca 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, CanBus, SubaruFlags +from openpilot.selfdrive.car.subaru.values import DBC, CanBus, PREGLOBAL_CARS, SubaruFlags from openpilot.selfdrive.car import CanSignalRateCalculator @@ -126,6 +126,11 @@ class CarState(CarStateBase): if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"]) + # FrogPilot CarState functions + if self.car_fingerprint not in PREGLOBAL_CARS: + self.lkas_previously_enabled = self.lkas_enabled + self.lkas_enabled = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"] + return ret, fp_ret @staticmethod diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 26f4c5696..a032b765d 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -5,6 +5,7 @@ from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type class CarInterface(CarInterfaceBase): @@ -101,6 +102,10 @@ class CarInterface(CarInterfaceBase): ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_toggles) + ret.buttonEvents = [ + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] + ret.events = self.create_common_events(ret).to_msg() return ret, fp_ret diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index b138b7f83..bfdb8aaeb 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -209,6 +209,8 @@ class CAR(Platforms): ) +PREGLOBAL_CARS = {CAR.SUBARU_FORESTER_PREGLOBAL, CAR.SUBARU_LEGACY_PREGLOBAL, CAR.SUBARU_OUTBACK_PREGLOBAL, CAR.SUBARU_OUTBACK_PREGLOBAL_2018} + SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 70262481f..fd4f5b24a 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -191,6 +191,10 @@ class CarState(CarStateBase): self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] # FrogPilot CarState functions + if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: + self.lkas_previously_enabled = self.lkas_enabled + message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"] + self.lkas_enabled = any(self.lkas_hud.get(key) == 1 for key in message_keys) # ZSS Support - Credit goes to the DragonPilot team! if self.CP.flags & ToyotaFlags.ZSS and self.zss_threshold_count < ZSS_THRESHOLD_COUNT: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 2c6ece2ce..661d2d4d7 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -8,6 +8,7 @@ from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type EventName = car.CarEvent.EventName SteerControlType = car.CarParams.SteerControlType @@ -155,7 +156,10 @@ class CarInterface(CarInterfaceBase): ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): - ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + ret.buttonEvents = [ + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), + ] # events events = self.create_common_events(ret) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a0f979dec..b60d10666 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -52,6 +52,7 @@ LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection EventName = car.CarEvent.EventName ButtonType = car.CarState.ButtonEvent.Type +FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter SafetyModel = car.CarParams.SafetyModel @@ -923,6 +924,14 @@ class Controls: if self.frogpilot_toggles.conditional_experimental_mode: self.experimental_mode = self.sm['frogpilotPlan'].conditionalExperimentalActive + if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents) and self.frogpilot_toggles.experimental_mode_via_lkas: + if self.frogpilot_toggles.conditional_experimental_mode: + conditional_status = self.params_memory.get_int("CEStatus") + override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 3 if conditional_status >= 7 else 4 + self.params_memory.put_int("CEStatus", override_value) + else: + self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode) + FPCC = custom.FrogPilotCarControl.new_message() FPCC.alwaysOnLateral = self.always_on_lateral_active