diff --git a/common/params.cc b/common/params.cc index 383d3ef..382183b 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}, @@ -347,6 +348,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 a2be221..fcece84 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 6cb4107..486b1be 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 bf9aca0..7c80476 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 ea342b4..4c56337 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 7539d5b..84d5de5 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 890adb5..6d76868 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 100120d..a7728cf 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 28e1ed1..fe2e8bb 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 a52bdf3..da8ea3f 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -10,6 +10,7 @@ from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \ CANFD_CAR, Buttons, CarControllerParams from openpilot.selfdrive.car.interfaces import CarStateBase +from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController @@ -182,9 +183,52 @@ class CarState(CarStateBase): if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0: self.main_enabled = not self.main_enabled + # FrogPilot functions + distance_pressed = self.cruise_buttons[-1] == Buttons.GAP_DIST + + # Driving personalities function + if ret.cruiseState.available: + # Sync with the onroad UI button + if self.fpf.personality_changed_via_ui: + self.personality_profile = self.fpf.current_personality + self.fpf.reset_personality_changed_param() + + if distance_pressed: + self.distance_pressed_counter += 1 + + elif self.distance_previously_pressed: + # Set the distance lines on the dash to match the new personality if the button was held down for less than 0.5 seconds + if self.distance_pressed_counter < CRUISE_LONG_PRESS and frogpilot_variables.personalities_via_wheel: + self.personality_profile = (self.personality_profile + 2) % 3 + + self.fpf.distance_button_function(self.personality_profile) + + self.distance_pressed_counter = 0 + + # Switch the current state of Experimental Mode if the button is held down for 0.5 seconds + if self.distance_pressed_counter == CRUISE_LONG_PRESS and frogpilot_variables.experimental_mode_via_distance: + if frogpilot_variables.conditional_experimental_mode: + self.fpf.update_cestatus_distance() + else: + self.fpf.update_experimental_mode() + + # Switch the current state of Traffic Mode if the button is held down for 2.5 seconds + if self.distance_pressed_counter == CRUISE_LONG_PRESS * 5 and frogpilot_variables.traffic_mode: + self.fpf.update_traffic_mode() + + # Revert the previous changes to Experimental Mode + if frogpilot_variables.experimental_mode_via_distance: + if frogpilot_variables.conditional_experimental_mode: + self.fpf.update_cestatus_distance() + else: + self.fpf.update_experimental_mode() + + self.distance_previously_pressed = distance_pressed + # 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"] + if lkas_pressed and not self.lkas_previously_pressed: if frogpilot_variables.conditional_experimental_mode: self.fpf.update_cestatus_lkas() @@ -280,11 +324,57 @@ 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 functions + distance_pressed = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0 + + # Driving personalities function + if ret.cruiseState.available: + # Sync with the onroad UI button + if self.fpf.personality_changed_via_ui: + self.personality_profile = self.fpf.current_personality + self.fpf.reset_personality_changed_param() + + if distance_pressed: + self.distance_pressed_counter += 1 + + elif self.distance_previously_pressed: + # Set the distance lines on the dash to match the new personality if the button was held down for less than 0.5 seconds + if self.distance_pressed_counter < CRUISE_LONG_PRESS and frogpilot_variables.personalities_via_wheel: + self.personality_profile = (self.personality_profile + 2) % 3 + + self.fpf.distance_button_function(self.personality_profile) + + self.distance_pressed_counter = 0 + + # Switch the current state of Experimental Mode if the button is held down for 0.5 seconds + if self.distance_pressed_counter == CRUISE_LONG_PRESS and frogpilot_variables.experimental_mode_via_distance: + if frogpilot_variables.conditional_experimental_mode: + self.fpf.update_cestatus_distance() + else: + self.fpf.update_experimental_mode() + + # Switch the current state of Traffic Mode if the button is held down for 2.5 seconds + if self.distance_pressed_counter == CRUISE_LONG_PRESS * 5 and frogpilot_variables.traffic_mode: + self.fpf.update_traffic_mode() + + # Revert the previous changes to Experimental Mode + if frogpilot_variables.experimental_mode_via_distance: + if frogpilot_variables.conditional_experimental_mode: + self.fpf.update_cestatus_distance() + else: + self.fpf.update_experimental_mode() + + self.distance_previously_pressed = distance_pressed + # 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"] + if lkas_pressed and not self.lkas_previously_pressed: - self.fpf.lkas_button_function(frogpilot_variables.conditional_experimental_mode) + if frogpilot_variables.conditional_experimental_mode: + self.fpf.update_cestatus_lkas() + else: + self.fpf.update_experimental_mode() self.lkas_previously_pressed = lkas_pressed SpeedLimitController.car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_factor diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 09e940a..c8a6976 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -126,12 +126,12 @@ def create_lfahda_mfc(packer, enabled, lat_active, 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 afb4009..02ceca2 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled, lat_active): 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 ba8f459..4305798 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -505,6 +505,8 @@ class CarStateBase(ABC): self.lkas_previously_pressed = False self.distance_pressed_counter = 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 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index f581b40..bea9269 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -193,11 +193,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 c56b258..a4f3035 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -48,6 +48,7 @@ class CarState(CarStateBase): self.lkas_hud = {} # FrogPilot variables + self.profile_restored = False self.traffic_signals = {} @@ -174,34 +175,75 @@ class CarState(CarStateBase): if self.CP.carFingerprint != CAR.PRIUS_V: self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) - # Experimental Mode via double clicking the LKAS button function + # FrogPilot functions + 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): + # distance button is wired to the ACC module (camera or radar) + if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): + distance_pressed = cp_acc.vl["ACC_CONTROL"]["DISTANCE"] + else: + distance_pressed = cp.vl["SDSU"]["FD_BUTTON"] + + # Switch the current state of Experimental Mode if the LKAS button is double pressed 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"] lkas_pressed = any(self.lkas_hud.get(key) == 1 for key in message_keys) if lkas_pressed and not self.lkas_previously_pressed: if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_distance() + self.fpf.update_cestatus_lkas() else: self.fpf.update_experimental_mode() self.lkas_previously_pressed = lkas_pressed - # Experimental Mode via long pressing the distance button function + # Distance button functions if ret.cruiseState.available: if distance_pressed: self.distance_pressed_counter += 1 elif self.distance_previously_pressed: + # Set the distance lines on the dash to match the new personality if the button was held down for less than 0.5 seconds + if self.distance_pressed_counter < CRUISE_LONG_PRESS: + self.previous_personality_profile = (self.personality_profile + 2) % 3 + self.fpf.distance_button_function(self.previous_personality_profile) + self.profile_restored = False self.distance_pressed_counter = 0 + # Switch the current state of Experimental Mode if the button is held down for 0.5 seconds if self.distance_pressed_counter == CRUISE_LONG_PRESS and frogpilot_variables.experimental_mode_via_distance: if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_lkas() + self.fpf.update_cestatus_distance() + else: + self.fpf.update_experimental_mode() + + # Switch the current state of Traffic Mode if the button is held down for 2.5 seconds + if self.distance_pressed_counter == CRUISE_LONG_PRESS * 5 and frogpilot_variables.traffic_mode: + self.fpf.update_traffic_mode() + + # Revert the previous changes to Experimental Mode + if frogpilot_variables.conditional_experimental_mode: + self.fpf.update_cestatus_distance() else: self.fpf.update_experimental_mode() self.distance_previously_pressed = distance_pressed + # Update the distance lines on the dash upon ignition/onroad UI button clicked + 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 + # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! self.update_traffic_signals(cp_cam) SpeedLimitController.car_speed_limit = self.calculate_speed_limit(frogpilot_variables) @@ -275,6 +317,11 @@ class CarState(CarStateBase): ("PRE_COLLISION", 33), ] + if CP.flags & ToyotaFlags.SMART_DSU and not CP.flags & ToyotaFlags.RADAR_CAN_FILTER: + messages += [ + ("SDSU", 100), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index eef6124..d7571a2 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -47,6 +47,14 @@ class CarInterface(CarInterfaceBase): stop_and_go = candidate in TSS2_CAR + # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it + # 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs + if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR): + ret.flags |= ToyotaFlags.SMART_DSU.value + + if 0x2AA in fingerprint[0] and candidate in NO_DSU_CAR: + ret.flags |= ToyotaFlags.RADAR_CAN_FILTER.value + if candidate == CAR.PRIUS: stop_and_go = True ret.wheelbase = 2.70 diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 6d74e75..37a24ef 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/toyota/values.py b/selfdrive/car/toyota/values.py index f2bec63..97c84a5 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -46,6 +46,7 @@ class ToyotaFlags(IntFlag): HYBRID = 1 SMART_DSU = 2 DISABLE_RADAR = 4 + RADAR_CAN_FILTER = 16 class CAR(StrEnum): diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index ee063e2..d3d6f70 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 853a19f..c130cd2 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 418462b..ae2d195 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 ec0b633..6a43932 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 0ac456f..b769122 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1149,6 +1149,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 a67324d..d423a62 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -94,7 +94,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 0000000..42d0055 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 0000000..52da0a4 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 0000000..fb77ec3 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 0000000..897ad3d 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 0000000..57c5a67 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 0000000..481e257 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 0000000..38ec4bd 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 4ca2841..4c94e1d 100644 --- a/selfdrive/frogpilot/functions/frogpilot_functions.py +++ b/selfdrive/frogpilot/functions/frogpilot_functions.py @@ -5,8 +5,6 @@ from openpilot.common.params import Params from openpilot.system.hardware import HARDWARE -params_memory = Params("/dev/shm/params") - DEFAULT_MODEL = "los-angeles" CITY_SPEED_LIMIT = 25 @@ -51,6 +49,7 @@ class MovingAverageCalculator: class FrogPilotFunctions: def __init__(self) -> None: self.params = Params() + self.params_memory = Params("/dev/shm/params") @staticmethod def get_min_accel_eco(v_ego): @@ -82,19 +81,32 @@ class FrogPilotFunctions: return min(distance_to_lane, distance_to_road_edge) - @staticmethod + @property + def current_personality(self): + return self.params.get_int("LongitudinalPersonality") + + def distance_button_function(self, new_personality): + self.params.put_int("LongitudinalPersonality", new_personality) + self.params_memory.put_bool("PersonalityChangedViaWheel", True) + + @property + def personality_changed_via_ui(self): + return self.params_memory.get_bool("PersonalityChangedViaUI") + + def reset_personality_changed_param(self): + self.params_memory.put_bool("PersonalityChangedViaUI", False) + def update_cestatus_distance(self): # Set "CEStatus" to work with "Conditional Experimental Mode" - conditional_status = params_memory.get_int("CEStatus") + 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 - params_memory.put_int("CEStatus", override_value) + self.params_memory.put_int("CEStatus", override_value) - @staticmethod def update_cestatus_lkas(self): # Set "CEStatus" to work with "Conditional Experimental Mode" - conditional_status = params_memory.get_int("CEStatus") + conditional_status = self.params_memory.get_int("CEStatus") override_value = 0 if conditional_status in (1, 2, 3, 4, 5, 6) else 5 if conditional_status >= 7 else 6 - params_memory.put_int("CEStatus", override_value) + self.params_memory.put_int("CEStatus", override_value) def update_experimental_mode(self): experimental_mode = self.params.get_bool("ExperimentalMode") diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 56ef1be..109a062 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -2,6 +2,8 @@ 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"}, {"AlwaysOnLateralMain", "Enable On Cruise Main", "Enable 'Always On Lateral' by simply turning on 'Cruise Control'.", ""}, {"HideAOLStatusBar", "Hide the Status Bar", "Don't use the status bar for 'Always On Lateral'.", ""}, @@ -82,7 +84,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") { FrogPilotParamManageControl *aolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); QObject::connect(aolToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { parentToggleClicked(); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 05c6b0f..45ce7b8 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -111,7 +111,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 9487fe9..d57a5de 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -952,6 +952,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); @@ -1164,6 +1165,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); @@ -1274,6 +1278,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; @@ -1328,6 +1334,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 && !scene.hide_map_icon); @@ -1677,6 +1692,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 ae75444..23d2b12 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 a74cade..f037a34 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -370,6 +370,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"); bool screen_management = params.getBool("ScreenManagement"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 0e372b8..6c7cd11 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -210,6 +210,7 @@ typedef struct UIScene { bool numerical_temp; bool parked; bool pedals_on_ui; + bool personalities_via_screen; bool reverse_cruise; bool reverse_cruise_ui; bool right_hand_drive;