From a81f6285c192d9fc3bdf389e96e64d7f676705c3 Mon Sep 17 00:00:00 2001 From: Your Name Date: Mon, 5 Feb 2024 01:15:52 -0600 Subject: [PATCH] wip --- selfdrive/car/hyundai/carstate.py | 76 ++++++++++++++----------- selfdrive/controls/controlsd.py | 10 ++-- selfdrive/controls/lib/desire_helper.py | 6 +- selfdrive/controls/lib/events.py | 2 +- selfdrive/monitoring/driver_monitor.py | 15 +++-- 5 files changed, 62 insertions(+), 47 deletions(-) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 0dd9819..950fbf7 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -171,13 +171,13 @@ class CarState(CarStateBase): self.custom_speed_up = False self.custom_speed_down = False - if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0: - self.custom_speed_down = True + # if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0: + # self.custom_speed_down = True - lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"] - if lkas_pressed and not self.lkas_previously_pressed: - self.custom_speed_up = True - self.lkas_previously_pressed = lkas_pressed + # lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"] + # if lkas_pressed and not self.lkas_previously_pressed: + # self.custom_speed_up = True + # self.lkas_previously_pressed = lkas_pressed # Driving personalities function # if self.personalities_via_wheel and ret.cruiseState.available: @@ -295,36 +295,48 @@ 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"]) + self.custom_speed_up = False + self.custom_speed_down = False + + if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0: + self.custom_speed_down = True + + lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"] + if lkas_pressed and not self.lkas_previously_pressed: + self.custom_speed_up = True + self.lkas_previously_pressed = lkas_pressed + + # Driving personalities function - if self.personalities_via_wheel and ret.cruiseState.available: - # Sync with the onroad UI button - if self.param_memory.get_bool("PersonalityChangedViaUI"): - self.personality_profile = self.param.get_int("LongitudinalPersonality") - self.param_memory.put_bool("PersonalityChangedViaUI", False) + # if self.personalities_via_wheel and ret.cruiseState.available: + # # Sync with the onroad UI button + # if self.param_memory.get_bool("PersonalityChangedViaUI"): + # self.personality_profile = self.param.get_int("LongitudinalPersonality") + # self.param_memory.put_bool("PersonalityChangedViaUI", False) - # Change personality upon steering wheel button press - if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0: - self.param_memory.put_bool("PersonalityChangedViaWheel", True) - self.personality_profile = (self.previous_personality_profile + 2) % 3 + # # Change personality upon steering wheel button press + # if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0: + # self.param_memory.put_bool("PersonalityChangedViaWheel", True) + # self.personality_profile = (self.previous_personality_profile + 2) % 3 - if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0: - self.param.put_int("LongitudinalPersonality", self.personality_profile) - self.previous_personality_profile = self.personality_profile + # if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0: + # self.param.put_int("LongitudinalPersonality", self.personality_profile) + # self.previous_personality_profile = self.personality_profile - # Toggle Experimental Mode from steering wheel function - if self.experimental_mode_via_lkas and ret.cruiseState.available: - lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"] - if lkas_pressed and not self.lkas_previously_pressed: - if self.conditional_experimental_mode: - # Set "CEStatus" to work with "Conditional Experimental Mode" - conditional_status = self.param_memory.get_int("CEStatus") - override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2 - self.param_memory.put_int("CEStatus", override_value) - else: - experimental_mode = self.param.get_bool("ExperimentalMode") - # Invert the value of "ExperimentalMode" - put_bool_nonblocking("ExperimentalMode", not experimental_mode) - self.lkas_previously_pressed = lkas_pressed + # # Toggle Experimental Mode from steering wheel function + # if self.experimental_mode_via_lkas and ret.cruiseState.available: + # lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"] + # if lkas_pressed and not self.lkas_previously_pressed: + # if self.conditional_experimental_mode: + # # Set "CEStatus" to work with "Conditional Experimental Mode" + # conditional_status = self.param_memory.get_int("CEStatus") + # override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2 + # self.param_memory.put_int("CEStatus", override_value) + # else: + # experimental_mode = self.param.get_bool("ExperimentalMode") + # # Invert the value of "ExperimentalMode" + # put_bool_nonblocking("ExperimentalMode", not experimental_mode) + # self.lkas_previously_pressed = lkas_pressed return ret diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e4b805a..c9eac9b 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -343,11 +343,11 @@ class Controls: # self.events.add(EventName.laneChange) # Handle turning - # if not CS.standstill: - # if self.sm['lateralPlan'].desire == Desire.turnLeft: - # self.events.add(FrogPilotEventName.turningLeft) - # elif self.sm['lateralPlan'].desire == Desire.turnRight: - # self.events.add(FrogPilotEventName.turningRight) + if not CS.standstill: + if self.sm['lateralPlan'].desire == Desire.turnLeft: + self.events.add(FrogPilotEventName.turningLeft) + elif self.sm['lateralPlan'].desire == Desire.turnRight: + self.events.add(FrogPilotEventName.turningRight) for i, pandaState in enumerate(self.sm['pandaStates']): # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 3db21c4..53f9b6d 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -91,11 +91,11 @@ class DesireHelper: if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none - # elif one_blinker and below_lane_change_speed and frogpilot_planner.turn_desires: + elif one_blinker and below_lane_change_speed and frogpilot_planner.turn_desires: # BBOT - # self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight + self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight # Set the "turn_completed" flag to prevent lane changes after completing a turn - # self.turn_completed = True + self.turn_completed = True else: # TurnDirection.turnLeft / turnRight self.turn_direction = TurnDirection.none diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 0bb8296..f516afc 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -231,7 +231,7 @@ def startup_master_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM if "REPLAY" in os.environ: branch = "replay" - return StartupAlert("Hippity hoppity this is my property", "so I do what I want 🐸", alert_status=AlertStatus.frogpilot) + return StartupAlert("Openpilot Ready", "", alert_status=AlertStatus.frogpilot) def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: return NoEntryAlert(f"Drive above {get_display_speed(CP.minEnableSpeed, metric)} to engage") diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 033fa26..59a9450 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -19,12 +19,15 @@ class DRIVER_MONITOR_SETTINGS(): def __init__(self): self._DT_DMON = DT_DMON # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 - self._AWARENESS_TIME = 90. # passive wheeltouch total timeout - self._AWARENESS_PRE_TIME_TILL_TERMINAL = 60. - self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 30. - self._DISTRACTED_TIME = 90. # active monitoring total timeout - self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 60. - self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 30. + # self._AWARENESS_TIME = 90. # passive wheeltouch total timeout + # self._AWARENESS_PRE_TIME_TILL_TERMINAL = 60. + # self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 30. + self._AWARENESS_TIME = 30. # passive wheeltouch total timeout + self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15. + self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. + self._DISTRACTED_TIME = 60. # active monitoring total timeout + self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 15. + self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 10. self._FACE_THRESHOLD = 0.7 self._EYE_THRESHOLD = 0.65