diff --git a/common/params.cc b/common/params.cc index 2d3b11a43..3e0744784 100644 --- a/common/params.cc +++ b/common/params.cc @@ -213,6 +213,7 @@ std::unordered_map keys = { {"AccelerationProfile", PERSISTENT}, {"AdjacentPath", PERSISTENT}, {"AdjacentPathMetrics", PERSISTENT}, + {"AdjustablePersonalities", PERSISTENT}, {"AggressiveAcceleration", PERSISTENT}, {"AggressiveFollow", PERSISTENT}, {"AggressiveJerk", PERSISTENT}, @@ -335,6 +336,10 @@ std::unordered_map keys = { {"PathWidth", PERSISTENT}, {"PauseLateralOnSignal", PERSISTENT}, {"PedalsOnUI", PERSISTENT}, + {"PersonalitiesViaScreen", PERSISTENT}, + {"PersonalitiesViaWheel", PERSISTENT}, + {"PersonalityChangedViaUI", PERSISTENT}, + {"PersonalityChangedViaWheel", PERSISTENT}, {"PreferredSchedule", PERSISTENT}, {"PreviousSpeedLimit", PERSISTENT}, {"PromptVolume", PERSISTENT}, diff --git a/opendbc/gm_global_a_powertrain_volt.dbc b/opendbc/gm_global_a_powertrain_volt.dbc index a2be22165..fcece8429 100644 --- a/opendbc/gm_global_a_powertrain_volt.dbc +++ b/opendbc/gm_global_a_powertrain_volt.dbc @@ -263,6 +263,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 6cb4107b7..486b1be8d 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 e61352a02..1c372e4f2 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -30,6 +30,11 @@ class CarState(CarStateBase): # FrogPilot variables self.single_pedal_mode = False + # FrogPilot variables + self.display_menu = False + + self.display_timer = 0 + def update(self, pt_cp, cam_cp, loopback_cp, frogpilot_variables): ret = car.CarState.new_message() @@ -162,6 +167,45 @@ class CarState(CarStateBase): ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1 ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1 + # Driving personalities function - Credit goes to Mangomoose! + if frogpilot_variables.personalities_via_wheel and ret.cruiseState.available: + # Sync with the onroad UI button + if self.fpf.personality_changed_via_ui: + self.personality_profile = self.fpf.current_personality + self.previous_personality_profile = self.personality_profile + self.fpf.reset_personality_changed_param() + + # Check if the car has a camera + has_camera = self.CP.networkLocation == NetworkLocation.fwdCamera + has_camera &= not self.CP.flags & GMFlags.NO_CAMERA.value + has_camera &= 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: + distance_button = cam_cp.vl["ASCMSteeringButton"]["DistanceButton"] + else: + distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"] + + if 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 = 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 and self.personality_profile >= 0: + self.fpf.distance_button_function(self.personality_profile) + self.previous_personality_profile = self.personality_profile + # Toggle Experimental Mode from steering wheel function if frogpilot_variables.experimental_mode_via_lkas 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 ea342b4e8..4c5633752 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -105,14 +105,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_profile): 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_profile + 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 7539d5b49..84d5de524 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", "personality_profile", "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, CC.latActive)) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 1f4293a01..8bde31944 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 frogpilot_variables.personalities_via_wheel and ret.cruiseState.available: + # Sync with the onroad UI button + if self.fpf.personality_changed_via_ui: + self.personality_profile = self.fpf.current_personality + self.previous_personality_profile = self.personality_profile + self.fpf.reset_personality_changed_param() + + # Change personality upon steering wheel button press + distance_button = self.cruise_setting == 3 + + if distance_button and not self.distance_previously_pressed: + self.personality_profile = (self.previous_personality_profile + 2) % 3 + self.distance_previously_pressed = distance_button + + if self.personality_profile != self.previous_personality_profile: + self.fpf.distance_button_function(self.personality_profile) + self.previous_personality_profile = self.personality_profile + # Toggle Experimental Mode from steering wheel function if frogpilot_variables.experimental_mode_via_lkas 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 100120d3f..a7728cf53 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.personality_profile, '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 dd1ae24cc..76c701700 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 e92b67159..9c2a51ad6 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -182,6 +182,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 frogpilot_variables.personalities_via_wheel and ret.cruiseState.available: + # Sync with the onroad UI button + if self.fpf.personality_changed_via_ui: + self.personality_profile = self.fpf.current_personality + self.previous_personality_profile = self.personality_profile + self.fpf.reset_personality_changed_param() + + # Change personality upon steering wheel button press + if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0: + self.personality_profile = (self.previous_personality_profile + 2) % 3 + + if self.personality_profile != self.previous_personality_profile: + self.fpf.distance_button_function(self.personality_profile) + self.previous_personality_profile = self.personality_profile + # Toggle Experimental Mode from steering wheel function if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available and self.CP.flags & HyundaiFlags.CAN_LFA_BTN: lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"] @@ -277,6 +293,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 frogpilot_variables.personalities_via_wheel and ret.cruiseState.available: + # Sync with the onroad UI button + if self.fpf.personality_changed_via_ui: + self.personality_profile = self.fpf.current_personality + self.previous_personality_profile = self.personality_profile + self.fpf.reset_personality_changed_param() + + # Change personality upon steering wheel button press + if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0: + self.personality_profile = (self.previous_personality_profile + 2) % 3 + + if self.personality_profile != self.previous_personality_profile: + self.fpf.distance_button_function(self.personality_profile) + self.previous_personality_profile = self.personality_profile + # Toggle Experimental Mode from steering wheel function if frogpilot_variables.experimental_mode_via_lkas 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 dcc7adf4e..569912b9a 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_profile): commands = [] scc11_values = { "MainMode_ACC": 1 if cruise_available else 0, - "TauGapSet": 4, + "TauGapSet": personality_profile + 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 a35fcb777..2319d322f 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_profile): 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_profile + 1, } return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 6d3fb4a38..9829f14c5 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -500,8 +500,13 @@ class CarStateBase(ABC): # FrogPilot variables self.fpf = FrogPilotFunctions() + self.distance_previously_pressed = False self.lkas_previously_pressed = False + self.distance_button = 0 + self.personality_profile = self.fpf.current_personality + self.previous_personality_profile = self.personality_profile + 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/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 8a52e5a4f..330d87acc 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -195,11 +195,11 @@ class CarController: 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, accel_raw, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, - frogpilot_variables)) + CS.distance_button, frogpilot_variables)) self.accel = pcm_accel_cmd else: can_sends.append(toyotacan.create_accel_command(self.packer, 0, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, - frogpilot_variables)) + CS.distance_button, frogpilot_variables)) 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 f3eb31855..4f1f5f9f0 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -47,6 +47,7 @@ class CarState(CarStateBase): self.lkas_hud = {} # FrogPilot variables + self.profile_restored = False self.traffic_signals = {} @@ -173,6 +174,34 @@ class CarState(CarStateBase): if self.CP.carFingerprint != CAR.PRIUS_V: self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) + # Driving personalities function + if frogpilot_variables.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.fpf.personality_changed_via_ui: + self.profile_restored = False + self.previous_personality_profile = self.fpf.current_personality + self.fpf.reset_personality_changed_param() + + # 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_button = not self.distance_button + + if self.profile_restored: + if self.CP.flags & ToyotaFlags.SMART_DSU: + self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] + elif self.CP.carFingerprint not in RADAR_ACC_CAR: + # KRKeegan - Add support for toyota distance button + self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"] + + if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0: + self.fpf.distance_button_function(self.personality_profile) + self.previous_personality_profile = self.personality_profile + # Toggle Experimental Mode from steering wheel function if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available and self.CP.carFingerprint != CAR.PRIUS_V: message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"] @@ -255,6 +284,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 6d74e75a5..37a24eff6 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, accel_raw, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, frogpilot_variables): +def create_accel_command(packer, accel, accel_raw, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance_button, frogpilot_variables): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": accel, # compensated accel command "ACC_TYPE": acc_type, - "DISTANCE": 0, + "DISTANCE": distance_button, "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 7dd0c38a2..c30185115 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 853a19f3e..c130cd2f3 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 frogpilot_variables.personalities_via_wheel and ret.cruiseState.available: + # Sync with the onroad UI button + if self.fpf.personality_changed_via_ui: + self.personality_profile = self.fpf.current_personality + self.previous_personality_profile = self.personality_profile + self.fpf.reset_personality_changed_param() + + # Change personality upon steering wheel button press + distance_button = pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"] + + if distance_button and not self.distance_previously_pressed: + self.personality_profile = (self.previous_personality_profile + 2) % 3 + self.distance_previously_pressed = distance_button + + if self.personality_profile != self.previous_personality_profile: + self.fpf.distance_button_function(self.personality_profile) + self.previous_personality_profile = self.personality_profile + return ret def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables): diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 787c7de53..82d9214dd 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_profile): 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_profile == 0 else 3 if personality_profile == 1 else 5, "ACC_Display_Prio": 3, "ACC_Abstandsindex": lead_distance, } diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index f42c3cf78..939943fb7 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -91,7 +91,7 @@ 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_profile): values = { "ACA_StaACC": acc_hud_status, "ACA_Zeitluecke": 2, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5d2dbf98f..79c7151d4 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1147,6 +1147,8 @@ class Controls: self.lane_detection = self.params.get_bool("LaneDetection") and self.params.get_bool("NudgelessLaneChange") self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if self.is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0 + self.frogpilot_variables.personalities_via_wheel = self.params.get_bool("PersonalitiesViaWheel") and self.params.get_bool("AdjustablePersonalities") + quality_of_life = self.params.get_bool("QOLControls") self.pause_lateral_on_signal = self.params.get_int("PauseLateralOnSignal") * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS) if quality_of_life else 0 self.frogpilot_variables.reverse_cruise_increase = quality_of_life and self.params.get_bool("ReverseCruise") diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 022134c13..58382fc83 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -89,7 +89,7 @@ class LongitudinalPlanner: return x, v, a, j def update(self, sm, frogpilot_planner, params_memory): - if self.param_read_counter % 50 == 0: + if self.param_read_counter % 50 == 0 or params_memory.get_bool("PersonalityChangedViaUI") or params_memory.get_bool("PersonalityChangedViaWheel"): self.read_param() self.param_read_counter += 1 self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' diff --git a/selfdrive/frogpilot/assets/other_images/aggressive.png b/selfdrive/frogpilot/assets/other_images/aggressive.png new file mode 100644 index 000000000..42d005538 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/aggressive.png differ diff --git a/selfdrive/frogpilot/assets/other_images/aggressive_new.png b/selfdrive/frogpilot/assets/other_images/aggressive_new.png new file mode 100644 index 000000000..52da0a46a Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/aggressive_new.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 000000000..fb77ec3ca Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/relaxed.png differ diff --git a/selfdrive/frogpilot/assets/other_images/relaxed_new.png b/selfdrive/frogpilot/assets/other_images/relaxed_new.png new file mode 100644 index 000000000..897ad3d2a Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/relaxed_new.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 000000000..57c5a67e7 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/standard.png differ diff --git a/selfdrive/frogpilot/assets/other_images/standard_new.png b/selfdrive/frogpilot/assets/other_images/standard_new.png new file mode 100644 index 000000000..481e25772 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/standard_new.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 000000000..38ec4bd21 Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_distance.png differ diff --git a/selfdrive/frogpilot/functions/frogpilot_functions.py b/selfdrive/frogpilot/functions/frogpilot_functions.py index dd4be424d..57b8ead75 100644 --- a/selfdrive/frogpilot/functions/frogpilot_functions.py +++ b/selfdrive/frogpilot/functions/frogpilot_functions.py @@ -78,6 +78,23 @@ class FrogPilotFunctions: return min(distance_to_lane, distance_to_road_edge) + @property + def current_personality(self): + return params.get_int("LongitudinalPersonality") + + @staticmethod + def distance_button_function(new_personality): + params.put_int("LongitudinalPersonality", new_personality) + params_memory.put_bool("PersonalityChangedViaWheel", True) + + @property + def personality_changed_via_ui(self): + return params_memory.get_bool("PersonalityChangedViaUI") + + @staticmethod + def reset_personality_changed_param(): + params_memory.put_bool("PersonalityChangedViaUI", False) + @staticmethod def lkas_button_function(conditional_experimental_mode): if conditional_experimental_mode: diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index ddd7f70b4..ebfc85a35 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -2,6 +2,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"}, @@ -75,7 +76,12 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil for (const auto &[param, title, desc, icon] : controlToggles) { ParamControl *toggle; - if (param == "AlwaysOnLateral") { + if (param == "AdjustablePersonalities") { + std::vector adjustablePersonalitiesToggles{"PersonalitiesViaWheel", "PersonalitiesViaScreen"}; + std::vector adjustablePersonalitiesNames{tr("Distance Button"), tr("Screen")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, adjustablePersonalitiesToggles, adjustablePersonalitiesNames); + + } else if (param == "AlwaysOnLateral") { std::vector aolToggles{"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 c6e33aad3..d3952e8ec 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.getBool("AdjustablePersonalities")) { addItem(long_personality_setting); } } diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 08605e140..708b35e59 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -920,6 +920,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); @@ -1130,6 +1131,9 @@ void AnnotatedCameraWidget::showEvent(QShowEvent *event) { 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); @@ -1203,6 +1207,8 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { mapOpen = scene.map_open; fullMapOpen = mapOpen && scene.full_map; + onroadAdjustableProfiles = scene.personalities_via_screen; + pedalsOnUI = scene.pedals_on_ui; rightHandDrive = scene.right_hand_drive; @@ -1257,6 +1263,15 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { pedal_icons->updateState(); } + 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); @@ -1595,6 +1610,76 @@ void AnnotatedCameraWidget::drawSLCConfirmation(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"} + }; + + personalityProfile = params.getInt("LongitudinalPersonality"); + + transitionTimer.start(); + + connect(this, &QPushButton::clicked, this, &PersonalityButton::handleClick); +} + +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 + 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 fe8e7fa4f..2b378c7a2 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -120,6 +120,29 @@ private: float acceleration; }; +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 @@ -172,6 +195,7 @@ private: Compass *compass_img; PedalIcons *pedal_icons; + PersonalityButton *personality_btn; ScreenRecorder *recorder_btn; QHBoxLayout *bottom_layout; @@ -186,6 +210,7 @@ private: bool fullMapOpen; bool leadInfo; bool mapOpen; + bool onroadAdjustableProfiles; bool pedalsOnUI; bool rightHandDrive; bool roadNameUI; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 498667577..6cd26cab1 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -362,6 +362,7 @@ void ui_update_frogpilot_params(UIState *s) { scene.hide_speed_ui = scene.hide_speed && params.getBool("HideSpeedUI"); scene.map_style = quality_of_life_visuals ? params.getInt("MapStyle") : 0; + scene.personalities_via_screen = params.getBool("PersonalitiesViaScreen") && params.getBool("AdjustablePersonalities"); scene.rotating_wheel = params.getBool("RotatingWheel"); scene.screen_brightness = params.getInt("ScreenBrightness"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 9c678e2aa..eb3d5f310 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -201,6 +201,7 @@ typedef struct UIScene { bool model_ui; bool numerical_temp; bool pedals_on_ui; + bool personalities_via_screen; bool reverse_cruise; bool reverse_cruise_ui; bool right_hand_drive;