diff --git a/common/params.cc b/common/params.cc index 9834789..b7ec39b 100644 --- a/common/params.cc +++ b/common/params.cc @@ -215,6 +215,7 @@ std::unordered_map keys = { {"AccelerationPath", PERSISTENT}, {"AccelerationProfile", PERSISTENT}, {"AdjacentPath", PERSISTENT}, + {"AdjustablePersonalities", PERSISTENT}, {"AggressiveAcceleration", PERSISTENT}, {"AggressiveFollow", PERSISTENT}, {"AggressiveJerk", PERSISTENT}, @@ -306,6 +307,8 @@ std::unordered_map keys = { {"PathEdgeWidth", PERSISTENT}, {"PathWidth", PERSISTENT}, {"PauseLateralOnSignal", PERSISTENT}, + {"PersonalityChangedViaUI", PERSISTENT}, + {"PersonalityChangedViaWheel", PERSISTENT}, {"PreferredSchedule", PERSISTENT}, {"PreviousSpeedLimit", PERSISTENT}, {"RelaxedFollow", PERSISTENT}, diff --git a/opendbc/gm_global_a_powertrain_volt.dbc b/opendbc/gm_global_a_powertrain_volt.dbc index de9f138..75a272f 100644 --- a/opendbc/gm_global_a_powertrain_volt.dbc +++ b/opendbc/gm_global_a_powertrain_volt.dbc @@ -265,6 +265,7 @@ BO_ 880 ASCMActiveCruiseControlStatus: 6 K124_ASCM SG_ ACCGapLevel : 21|2@0+ (1,0) [0|0] "" NEO SG_ ACCResumeButton : 1|1@0+ (1,0) [0|0] "" NEO SG_ ACCCmdActive : 23|1@0+ (1,0) [0|0] "" NEO + SG_ DisplayDistance : 4|1@0+ (1,0) [0|1] "" XXX SG_ FCWAlert : 41|2@0+ (1,0) [0|3] "" XXX BO_ 967 EVDriveMode: 4 XXX diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index b460a0d..4d71fec 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -185,7 +185,7 @@ class CarController: # Send dashboard UI commands (ACC status) send_fcw = hud_alert == VisualAlert.fcw can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled, - hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw)) + hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw, CS.display_menu, CS.personality_profile)) else: # to keep accel steady for logs when not sending gas accel += self.accel_g diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index a38c0a5..c90aded 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -156,6 +156,44 @@ class CarState(CarStateBase): ret.cruiseState.speed = pt_cp.vl["ECMCruiseControl"]["CruiseSetSpeed"] * CV.KPH_TO_MS ret.cruiseState.enabled = pt_cp.vl["ECMCruiseControl"]["CruiseActive"] != 0 + # Driving personalities function - Credit goes to Mangomoose! + 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.previous_personality_profile = self.personality_profile + self.param_memory.put_bool("PersonalityChangedViaUI", False) + + # Check if the car has a camera + has_camera = self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.flags & GMFlags.NO_CAMERA.value and not self.CP.carFingerprint in (CC_ONLY_CAR) + + if has_camera: + # Need to subtract by 1 to comply with the personality profiles of "0", "1", and "2" + self.personality_profile = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCGapLevel"] - 1 + else: + if self.CP.carFingerprint in SDGM_CAR: + self.distance_button = cam_cp.vl["ASCMSteeringButton"]["DistanceButton"] + else: + self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"] + + if self.distance_button and not self.distance_previously_pressed: + if self.display_menu: + self.personality_profile = (self.previous_personality_profile + 2) % 3 + self.display_timer = 350 + self.distance_previously_pressed = self.distance_button + + # Check if the display is open + if self.display_timer > 0: + self.display_timer -= 1 + self.display_menu = True + else: + self.display_menu = False + + if self.personality_profile != self.previous_personality_profile: + self.param.put_int("LongitudinalPersonality", self.personality_profile) + self.param_memory.put_bool("PersonalityChangedViaWheel", True) + self.previous_personality_profile = self.personality_profile + # Toggle Experimental Mode from steering wheel function if self.experimental_mode_via_press and ret.cruiseState.available: if self.CP.carFingerprint in SDGM_CAR: diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 8ebb3fd..c4ea34d 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -106,14 +106,15 @@ def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_s return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values) -def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw): +def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw, display, personality): target_speed = min(target_speed_kph, 255) values = { "ACCAlwaysOne": 1, "ACCResumeButton": 0, + "DisplayDistance": display, "ACCSpeedSetpoint": target_speed, - "ACCGapLevel": 3 * enabled, # 3 "far", 0 "inactive" + "ACCGapLevel": min(personality + 1, 3), # 3 "far", 0 "inactive" "ACCCmdActive": enabled, "ACCAlwaysOne2": 1, "ACCLeadCar": lead_car_in_sight, diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 26facb0..7854b76 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -94,7 +94,7 @@ def process_hud_alert(hud_alert): HUDData = namedtuple("HUDData", - ["pcm_accel", "v_cruise", "lead_visible", + ["pcm_accel", "v_cruise", "lead_visible", "distance_lines", "lanes_visible", "fcw", "acc_alert", "steer_required"]) @@ -251,7 +251,7 @@ class CarController: # Send dashboard UI commands. if self.frame % 10 == 0: - hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible, + hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible, CS.personality_profile + 1, hud_control.lanesVisible, fcw_display, acc_alert, steer_required) can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud)) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index d7c298b..0a1884b 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -268,6 +268,25 @@ 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 + # 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) + + # Change personality upon steering wheel button press + self.distance_button = self.cruise_setting == 3 + + if self.distance_button and not self.distance_previously_pressed: + self.param_memory.put_bool("PersonalityChangedViaWheel", True) + self.personality_profile = (self.previous_personality_profile + 2) % 3 + self.distance_previously_pressed = self.distance_button + + 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_press and ret.cruiseState.available: lkas_pressed = self.cruise_setting == 1 diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index c4517cf..11c995c 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -132,7 +132,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, acc_hud_values = { 'CRUISE_SPEED': hud.v_cruise, 'ENABLE_MINI_CAR': 1 if enabled else 0, - 'HUD_DISTANCE': 0, # max distance setting on display + 'HUD_DISTANCE': hud.distance_lines, 'IMPERIAL_UNIT': int(not is_metric), 'HUD_LEAD': 2 if enabled and hud.lead_visible else 1 if enabled else 0, 'SET_ME_X01_2': 1, diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 0c6caf0..726f708 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -131,7 +131,7 @@ class CarController: can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)) if self.frame % 2 == 0: can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, - set_speed_in_units)) + set_speed_in_units, CS.personality_profile)) self.accel_last = accel else: # button presses @@ -151,7 +151,7 @@ class CarController: use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), hud_control.leadVisible, set_speed_in_units, stopping, - CC.cruiseControl.override, use_fca, CS.out.cruiseState.available)) + CC.cruiseControl.override, use_fca, CS.out.cruiseState.available, CS.personality_profile)) # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 43a84f7..85c030d 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -167,6 +167,22 @@ class CarState(CarStateBase): if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0: self.main_enabled = not self.main_enabled + # 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) + + # 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 + # Toggle Experimental Mode from steering wheel function if self.experimental_mode_via_press and ret.cruiseState.available: lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"] @@ -267,6 +283,22 @@ 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"]) + # 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) + + # 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 + # Toggle Experimental Mode from steering wheel function if self.experimental_mode_via_press and ret.cruiseState.available: lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"] diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index dcc7adf..e5febf4 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -126,12 +126,12 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0): } return packer.make_can_msg("LFAHDA_MFC", 0, values) -def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca, cruise_available): +def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca, cruise_available, personality): commands = [] scc11_values = { "MainMode_ACC": 1 if cruise_available else 0, - "TauGapSet": 4, + "TauGapSet": personality + 1, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, "ObjValid": 1, # close lead makes controls tighter diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index a35fcb7..a368c87 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled): return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) -def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed): +def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, personality): jerk = 5 jn = jerk / 50 if not enabled or gas_override: @@ -146,7 +146,7 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov "SET_ME_2": 0x4, "SET_ME_3": 0x3, "SET_ME_TMP_64": 0x64, - "DISTANCE_SETTING": 4, + "DISTANCE_SETTING": personality + 1, } return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 94aac29..2fc014e 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -495,8 +495,16 @@ class CarStateBase(ABC): self.param = Params() self.param_memory = Params("/dev/shm/params") + self.display_menu = False + self.distance_previously_pressed = False self.lkas_previously_pressed = False self.main_enabled = False + self.profile_restored = False + + self.display_timer = 0 + self.distance_button = 0 + self.personality_profile = self.param.get_int("LongitudinalPersonality") + self.previous_personality_profile = self.param.get_int("LongitudinalPersonality") 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 @@ -589,6 +597,7 @@ class CarStateBase(ABC): def update_frogpilot_params(self, params): self.conditional_experimental_mode = params.get_bool("ConditionalExperimental") self.experimental_mode_via_press = params.get_bool("ExperimentalModeViaPress") + self.personalities_via_wheel = params.get_int("AdjustablePersonalities") in {1, 3} INTERFACE_ATTR_FILE = { "FINGERPRINTS": "fingerprints", diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 031320a..74ed227 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -163,10 +163,10 @@ class CarController: if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) elif self.CP.openpilotLongitudinalControl: - can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, self.reverse_cruise_increase)) + can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, self.reverse_cruise_increase, CS.distance_button)) self.accel = pcm_accel_cmd else: - can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.reverse_cruise_increase)) + can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.reverse_cruise_increase, CS.distance_button)) if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 87f259b..1ee0f21 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -165,6 +165,41 @@ class CarState(CarStateBase): if self.CP.carFingerprint != CAR.PRIUS_V: self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) + # Driving personalities function + if self.personalities_via_wheel and ret.cruiseState.available: + # Need to subtract by 1 to comply with the personality profiles of "0", "1", and "2" + self.personality_profile = cp.vl["PCM_CRUISE_SM"]["DISTANCE_LINES"] - 1 + + # Sync with the onroad UI button + if self.param_memory.get_bool("PersonalityChangedViaUI"): + self.profile_restored = False + self.previous_personality_profile = self.param.get_int("LongitudinalPersonality") + self.param_memory.put_bool("PersonalityChangedViaUI", False) + + # Set personality to the previous drive's personality or when the user changes it via the UI + if self.personality_profile == self.previous_personality_profile: + self.profile_restored = True + if not self.profile_restored: + self.distance_previously_pressed = not self.distance_previously_pressed + self.distance_button = not self.distance_previously_pressed + + if self.profile_restored: + if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): + # KRKeegan - Add support for toyota distance button + self.distance_button = cp_cam.vl["ACC_CONTROL"]["DISTANCE"] + + elif self.CP.carFingerprint in RADAR_ACC_CAR: + # These cars have the acc_control on car can + self.distance_button = cp.vl["ACC_CONTROL"]["DISTANCE"] + + elif self.CP.flags & ToyotaFlags.SMART_DSU: + self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] + + if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0: + self.param.put_int("LongitudinalPersonality", self.personality_profile) + self.param_memory.put_bool("PersonalityChangedViaWheel", True) + self.previous_personality_profile = self.personality_profile + # Toggle Experimental Mode from steering wheel function if self.experimental_mode_via_press and ret.cruiseState.available and self.CP.carFingerprint != CAR.PRIUS_V: message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"] @@ -252,6 +287,9 @@ class CarState(CarStateBase): ("PRE_COLLISION", 33), ] + if CP.flags & ToyotaFlags.SMART_DSU: + messages.append(("SDSU", 33)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 47a243d..3ab6fe4 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -33,12 +33,12 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, return packer.make_can_msg("STEERING_LTA", 0, values) -def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, reverse_cruise): +def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, reverse_cruise, distance): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": accel, "ACC_TYPE": acc_type, - "DISTANCE": 0, + "DISTANCE": distance, "MINI_CAR": lead, "PERMIT_BRAKING": 1, "RELEASE_STANDSTILL": not standstill_req, diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 126e00d..24c97ca 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -104,7 +104,7 @@ class CarController: # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem? set_speed = hud_control.setSpeed * CV.MS_TO_KPH can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed, - lead_distance)) + lead_distance, CS.personality_profile)) # **** Stock ACC Button Controls **************************************** # diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 25c5bc0..3d7b238 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -151,6 +151,25 @@ class CarState(CarStateBase): # Digital instrument clusters expect the ACC HUD lead car distance to be scaled differently self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) + # 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) + + # Change personality upon steering wheel button press + self.distance_button = pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"] + + if self.distance_button and not self.distance_previously_pressed: + self.param_memory.put_bool("PersonalityChangedViaWheel", True) + self.personality_profile = (self.previous_personality_profile + 2) % 3 + self.distance_previously_pressed = self.distance_button + + 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 + return ret def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 787c7de..d30ac3d 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -125,11 +125,11 @@ def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_cont return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, personality): values = { "ACC_Status_Anzeige": acc_hud_status, "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36, - "ACC_Gesetzte_Zeitluecke": 3, + "ACC_Gesetzte_Zeitluecke": 1 if personality == 0 else 3 if personality == 1 else 5, "ACC_Display_Prio": 3, "ACC_Abstandsindex": lead_distance, } diff --git a/selfdrive/frogpilot/assets/other_images/aggressive.png b/selfdrive/frogpilot/assets/other_images/aggressive.png new file mode 100644 index 0000000..42d0055 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/aggressive.png differ diff --git a/selfdrive/frogpilot/assets/other_images/relaxed.png b/selfdrive/frogpilot/assets/other_images/relaxed.png new file mode 100644 index 0000000..fb77ec3 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/relaxed.png differ diff --git a/selfdrive/frogpilot/assets/other_images/standard.png b/selfdrive/frogpilot/assets/other_images/standard.png new file mode 100644 index 0000000..57c5a67 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/standard.png differ diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_distance.png b/selfdrive/frogpilot/assets/toggle_icons/icon_distance.png new file mode 100644 index 0000000..38ec4bd Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_distance.png differ diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index df18266..f6db926 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -3,6 +3,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) { const std::vector> controlToggles { + {"AdjustablePersonalities", "Adjustable Personalities", "Use the 'Distance' button on the steering wheel or the onroad UI to switch between openpilot's driving personalities.\n\n1 bar = Aggressive\n2 bars = Standard\n3 bars = Relaxed", "../frogpilot/assets/toggle_icons/icon_distance.png"}, {"AlwaysOnLateral", "Always on Lateral", "Maintain openpilot lateral control when the brake or gas pedals are used.\n\nDeactivation occurs only through the 'Cruise Control' button.", "../frogpilot/assets/toggle_icons/icon_always_on_lateral.png"}, {"ConditionalExperimental", "Conditional Experimental Mode", "Automatically switches to 'Experimental Mode' under predefined conditions.", "../frogpilot/assets/toggle_icons/icon_conditional.png"}, @@ -55,7 +56,10 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil for (const auto &[param, title, desc, icon] : controlToggles) { ParamControl *toggle; - if (param == "AlwaysOnLateral") { + if (param == "AdjustablePersonalities") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 3, {{0, "None"}, {1, "Steering Wheel"}, {2, "Onroad UI Button"}, {3, "Wheel + Button"}}, this, true); + + } else if (param == "AlwaysOnLateral") { std::vector aolToggles{tr("AlwaysOnLateralMain")}; std::vector aolToggleNames{tr("Enable On Cruise Main")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, aolToggles, aolToggleNames); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index c9ac3b3..bde9942 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -110,7 +110,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { toggles[param.toStdString()] = toggle; // insert longitudinal personality after NDOG toggle - if (param == "DisengageOnAccelerator") { + if (param == "DisengageOnAccelerator" && !params.getInt("AdjustablePersonalities")) { addItem(long_personality_setting); } } diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index a73add3..94dd816 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -879,6 +879,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) int offset = UI_BORDER_SIZE + btn_size / 2; offset += alwaysOnLateral || conditionalExperimental || roadNameUI ? 25 : 0; int x = rightHandDM ? width() - offset : offset; + x += onroadAdjustableProfiles ? 250 : 0; int y = height() - offset; float opacity = dmActive ? 0.65 : 0.2; drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity); @@ -1110,6 +1111,9 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { bottom_layout = new QHBoxLayout(); + personality_btn = new PersonalityButton(this); + bottom_layout->addWidget(personality_btn); + QSpacerItem *spacer = new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Minimum); bottom_layout->addItem(spacer); @@ -1185,6 +1189,7 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { muteDM = scene.mute_dm; obstacleDistance = scene.obstacle_distance; obstacleDistanceStock = scene.obstacle_distance_stock; + onroadAdjustableProfiles = scene.personalities_via_screen; roadNameUI = scene.road_name_ui; showDriverCamera = scene.show_driver_camera; slcOverridden = scene.speed_limit_overridden; @@ -1225,6 +1230,15 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { bottom_layout->setAlignment(compass_img, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight)); } + const bool enablePersonalityButton = onroadAdjustableProfiles && !hideBottomIcons; + personality_btn->setVisible(enablePersonalityButton); + if (enablePersonalityButton) { + if (paramsMemory.getBool("PersonalityChangedViaWheel")) { + personality_btn->checkUpdate(); + } + bottom_layout->setAlignment(personality_btn, (rightHandDM ? Qt::AlignRight : Qt::AlignLeft)); + } + map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled()); if (map_settings_btn_bottom->isEnabled()) { map_settings_btn_bottom->setVisible(!hideBottomIcons && !compass); @@ -1470,6 +1484,79 @@ void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) { p.restore(); } +PersonalityButton::PersonalityButton(QWidget *parent) : QPushButton(parent), scene(uiState()->scene) { + setFixedSize(btn_size * 1.5, btn_size * 1.5); + + // Configure the profile vector + profile_data = { + {QPixmap("../frogpilot/assets/other_images/aggressive.png"), "Aggressive"}, + {QPixmap("../frogpilot/assets/other_images/standard.png"), "Standard"}, + {QPixmap("../frogpilot/assets/other_images/relaxed.png"), "Relaxed"} + }; + + // Start the timer as soon as the button is created + transitionTimer.start(); + + // Initialize the click event + connect(this, &QPushButton::clicked, this, &PersonalityButton::handleClick); + + personalityProfile = params.getInt("LongitudinalPersonality"); + setVisible(scene.personalities_via_screen); +} + +void PersonalityButton::checkUpdate() { + // Sync with the steering wheel button + personalityProfile = params.getInt("LongitudinalPersonality"); + updateState(); + paramsMemory.putBool("PersonalityChangedViaWheel", false); +} + +void PersonalityButton::handleClick() { + int mapping[] = {2, 0, 1}; + personalityProfile = mapping[personalityProfile]; + + params.putInt("LongitudinalPersonality", personalityProfile); + paramsMemory.putBool("PersonalityChangedViaUI", true); + + updateState(); +} + +void PersonalityButton::updateState() { + // Start the transition + transitionTimer.restart(); +} + +void PersonalityButton::paintEvent(QPaintEvent *) { + // Declare the constants + constexpr qreal fadeDuration = 1000.0; // 1 second + constexpr qreal textDuration = 3000.0; // 3 seconds + + QPainter p(this); + int elapsed = transitionTimer.elapsed(); + qreal textOpacity = qBound(0.0, 1.0 - ((elapsed - textDuration) / fadeDuration), 1.0); + qreal imageOpacity = qBound(0.0, (elapsed - textDuration) / fadeDuration, 1.0); + + // Enable Antialiasing + p.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing); + + // Configure the button + const auto &[profile_image, profile_text] = profile_data[personalityProfile]; + QRect rect(0, 0, width(), height() + 95); + + // Draw the profile text with the calculated opacity + if (textOpacity > 0.0) { + p.setOpacity(textOpacity); + p.setFont(InterFont(40, QFont::Bold)); + p.setPen(Qt::white); + p.drawText(rect, Qt::AlignCenter, profile_text); + } + + // Draw the profile image with the calculated opacity + if (imageOpacity > 0.0) { + drawIcon(p, QPoint((btn_size / 2) * 1.25, btn_size / 2 + 95), profile_image, Qt::transparent, imageOpacity); + } +} + void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { p.save(); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 62d24cc..fd33a65 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -104,6 +104,30 @@ private: QPixmap settings_img; }; +// FrogPilot buttons +class PersonalityButton : public QPushButton { +public: + explicit PersonalityButton(QWidget *parent = 0); + + void checkUpdate(); + void handleClick(); + void updateState(); + +private: + void paintEvent(QPaintEvent *event) override; + + Params params; + Params paramsMemory{"/dev/shm/params"}; + + UIScene &scene; + + int personalityProfile = 0; + + QElapsedTimer transitionTimer; + + QVector> profile_data; +}; + // container window for the NVG UI class AnnotatedCameraWidget : public CameraWidget { Q_OBJECT @@ -153,6 +177,7 @@ private: UIScene &scene; Compass *compass_img; + PersonalityButton *personality_btn; ScreenRecorder *recorder_btn; QHBoxLayout *bottom_layout; @@ -168,6 +193,7 @@ private: bool leadInfo; bool mapOpen; bool muteDM; + bool onroadAdjustableProfiles; bool roadNameUI; bool showDriverCamera; bool slcOverridden; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 7d4c852..47a8467 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -325,6 +325,7 @@ void ui_update_params(UIState *s) { scene.driver_camera = params.getBool("DriverCamera"); scene.experimental_mode_via_press = params.getBool("ExperimentalModeViaPress"); scene.mute_dm = params.getBool("FireTheBabysitter") && params.getBool("MuteDM"); + scene.personalities_via_screen = (params.getInt("AdjustablePersonalities") == 2 || params.getInt("AdjustablePersonalities") == 3); scene.rotating_wheel = params.getBool("RotatingWheel"); scene.screen_brightness = params.getInt("ScreenBrightness"); scene.speed_limit_controller = params.getBool("SpeedLimitController"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 9e02b4f..bd615a8 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -189,6 +189,7 @@ typedef struct UIScene { bool map_open; bool model_ui; bool mute_dm; + bool personalities_via_screen; bool road_name_ui; bool rotating_wheel; bool show_driver_camera;