diff --git a/cereal/custom.capnp b/cereal/custom.capnp index e9e150859..c577ad906 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -98,6 +98,7 @@ struct StarPilotCarState @0xf35cc4560bbf6ec2 { teslaCCNotArmed @27 :Bool; # lateral engaged but DI_cruiseState != STANDBY/ENABLED accelHardCruise @28 :Bool; # current/releasing accel cruise button came from GM hard-press signal decelHardCruise @29 :Bool; # current/releasing decel cruise button came from GM hard-press signal + pulseAndGlide @30 :Bool; # developer-only wheel-button pulse-and-glide mode is enabled } struct StarPilotDeviceState @0xda96579883444c35 { diff --git a/common/params_keys.h b/common/params_keys.h index 122afae05..82932ba5c 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -522,6 +522,7 @@ inline static std::unordered_map keys = { {"PreviousSpeedLimit", {PERSISTENT, FLOAT, "0.0", "0.0"}}, {"PromptDistractedVolume", {PERSISTENT, INT, "101", "101", 2, SETTINGS_SIMPLE}}, {"PromptVolume", {PERSISTENT, INT, "101", "101", 2, SETTINGS_SIMPLE}}, + {"PulseGlideSpeedDelta", {PERSISTENT, FLOAT, "5.0", "5.0", 3}}, {"QOLLateral", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}}, {"QOLLongitudinal", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}}, {"QOLVisuals", {PERSISTENT, BOOL, "1", "0", 0, SETTINGS_SIMPLE}}, diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index 6ba55bf43..2e48655a0 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -24,6 +24,7 @@ TEMP_STEER_FAULTS = (0, 9, 11, 21, 25) # - prolonged high driver torque: 17 (permanent) PERM_STEER_FAULTS = (3, 17) LKAS_BUTTON_CAR = TSS2_CAR | {CAR.TOYOTA_PRIUS} +DISTANCE_BUTTON_CAR = {CAR.TOYOTA_SIENNA_4TH_GEN} # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! @@ -244,6 +245,11 @@ class CarState(CarStateBase): buttonEvents += create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + if self.CP.carFingerprint in DISTANCE_BUTTON_CAR: + prev_distance_button = self.distance_button + self.distance_button = cp.vl["PCM_CRUISE_4"]["DISTANCE"] + buttonEvents += create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + fp_ret = custom.StarPilotCarState.new_message() if self.has_SDSU and not self.has_can_filter: @@ -292,6 +298,9 @@ class CarState(CarStateBase): if CP.enableGasInterceptorDEPRECATED: pt_messages.append(("GAS_SENSOR", 50)) + if CP.carFingerprint in DISTANCE_BUTTON_CAR: + pt_messages.append(("PCM_CRUISE_4", 50)) + return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2), diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 180367363..d2a4b45a5 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -90,6 +90,31 @@ class TestToyotaInterfaces: assert params.lateralTuning.torque.latAccelFactor == pytest.approx(1.7) assert params.lateralTuning.torque.friction == pytest.approx(0.14) + def test_sienna_4th_gen_parses_distance_button(self): + params = CarInterface.get_params( + CAR.TOYOTA_SIENNA_4TH_GEN, + {bus: {} for bus in range(8)}, + [], + alpha_long=False, + is_release=False, + docs=False, + starpilot_toggles=SimpleNamespace(force_torque_controller=False, nnff=False, nnff_lite=False), + ) + parser = CarState.get_can_parsers(params)[Bus.pt] + + assert "PCM_CRUISE_4" in parser.vl + + other_params = CarInterface.get_params( + CAR.TOYOTA_RAV4_PRIME, + {bus: {} for bus in range(8)}, + [], + alpha_long=False, + is_release=False, + docs=False, + starpilot_toggles=SimpleNamespace(force_torque_controller=False, nnff=False, nnff_lite=False), + ) + assert "PCM_CRUISE_4" not in CarState.get_can_parsers(other_params)[Bus.pt].vl + def test_tss2_dbc(self): # We make some assumptions about TSS2 platforms, # like looking up certain signals only in this DBC diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 2ff55192d..cd0cd3e29 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -111,6 +111,7 @@ class LatControlTorque(LatControl): self.is_rav4_tss2 = CP.carFingerprint in RAV4_TSS2_CARS self.is_rav4_prime = CP.carFingerprint in RAV4_PRIME_CARS self.is_sienna_4th_gen = CP.carFingerprint in SIENNA_4TH_GEN_CARS + self.is_toyota_corolla_tss2 = CP.carFingerprint in TOYOTA_COROLLA_TSS2_CARS self.is_lexus_is = CP.carFingerprint in LEXUS_IS_CARS self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS @@ -305,6 +306,7 @@ class LatControlTorque(LatControl): rav4_tss2_active = self.is_rav4_tss2 rav4_prime_active = self.is_rav4_prime sienna_4th_gen_active = self.is_sienna_4th_gen + toyota_corolla_tss2_active = self.is_toyota_corolla_tss2 lexus_is_active = self.is_lexus_is ioniq_5_active = self.is_ioniq_5 ioniq_ev_old_active = self.is_ioniq_ev_old @@ -392,6 +394,8 @@ class LatControlTorque(LatControl): elif sienna_4th_gen_active: ff *= get_sienna_4th_gen_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_sienna_4th_gen_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + elif toyota_corolla_tss2_active: + ff *= get_toyota_corolla_tss2_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif lexus_is_active: ff *= get_lexus_is_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif ioniq_5_active: @@ -566,6 +570,8 @@ class LatControlTorque(LatControl): elif sienna_4th_gen_active: output_torque *= get_sienna_4th_gen_center_taper_scale(setpoint, CS.vEgo) output_torque *= get_sienna_4th_gen_high_speed_output_taper_scale(CS.vEgo) + elif toyota_corolla_tss2_active: + output_torque *= get_toyota_corolla_tss2_center_output_scale(setpoint, CS.vEgo) elif prius_active: output_torque *= prius_center_taper output_torque *= get_prius_high_speed_output_taper_scale(setpoint, CS.vEgo) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index d767762fe..48c26df21 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -180,6 +180,10 @@ SIENNA_4TH_GEN_CARS = ( TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN, ) +TOYOTA_COROLLA_TSS2_CARS = ( + TOYOTA_CAR.TOYOTA_COROLLA_TSS2, +) + LEXUS_IS_CARS = ( TOYOTA_CAR.LEXUS_IS, ) @@ -232,7 +236,7 @@ GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30 GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08 GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0 GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5 -GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.10 +GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.12 GENESIS_G70_CENTER_OUTPUT_TAPER_LAT = 0.30 GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10 GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED = 18.0 @@ -255,7 +259,7 @@ GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT = 0.14 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH = 0.05 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.0 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5 -GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.06 +GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.04 GENESIS_G70_CURVE_UNWIND_SPEED = 18.0 GENESIS_G70_CURVE_UNWIND_SPEED_WIDTH = 3.0 GENESIS_G70_CURVE_UNWIND_LAT = 0.25 @@ -1062,6 +1066,21 @@ SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_ONSET_WIDTH = 2.0 SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED = 27.0 SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX_SPEED_WIDTH = 3.0 +TOYOTA_COROLLA_TSS2_PHASE_SCALE = 0.12 +TOYOTA_COROLLA_TSS2_TURN_IN_FF_BOOST = 0.035 +TOYOTA_COROLLA_TSS2_UNWIND_FF_REDUCTION = 0.06 +TOYOTA_COROLLA_TSS2_CURVE_LAT_ONSET = 0.24 +TOYOTA_COROLLA_TSS2_CURVE_LAT_WIDTH = 0.10 +TOYOTA_COROLLA_TSS2_SPEED_ONSET = 4.0 +TOYOTA_COROLLA_TSS2_SPEED_ONSET_WIDTH = 1.5 +TOYOTA_COROLLA_TSS2_SPEED_CUTOFF = 24.0 +TOYOTA_COROLLA_TSS2_SPEED_CUTOFF_WIDTH = 3.0 +TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_MAX = 0.30 +TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT = 0.18 +TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08 +TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED = 4.5 +TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 1.5 + LEXUS_IS_PHASE_SCALE = 0.10 # The Lexus route still fell short during a clean high-speed turn-in while # already at the controller limit. Keep this correction small and phase-gated @@ -1560,6 +1579,41 @@ def get_sienna_4th_gen_high_speed_output_taper_scale(v_ego: float) -> float: return 1.0 - SIENNA_4TH_GEN_HIGH_SPEED_OUTPUT_TAPER_MAX * onset * cutoff +def get_toyota_corolla_tss2_ff_scale(desired_lateral_accel: float, + desired_lateral_jerk: float, + v_ego: float) -> float: + """Add a small, transition-only turn-in correction for Corolla TSS2 torque EPS.""" + if desired_lateral_accel == 0.0: + return 1.0 + + phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / + TOYOTA_COROLLA_TSS2_PHASE_SCALE) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + curve_weight = _sigmoid((abs(desired_lateral_accel) - TOYOTA_COROLLA_TSS2_CURVE_LAT_ONSET) / + TOYOTA_COROLLA_TSS2_CURVE_LAT_WIDTH) + speed_weight = (_sigmoid((v_ego - TOYOTA_COROLLA_TSS2_SPEED_ONSET) / + TOYOTA_COROLLA_TSS2_SPEED_ONSET_WIDTH) * + _sigmoid((TOYOTA_COROLLA_TSS2_SPEED_CUTOFF - v_ego) / + TOYOTA_COROLLA_TSS2_SPEED_CUTOFF_WIDTH)) + boost = _flm_vehicle_knob("toyota_corolla_tss2.turn_in_ff_boost", + TOYOTA_COROLLA_TSS2_TURN_IN_FF_BOOST) + unwind_reduction = _flm_vehicle_knob("toyota_corolla_tss2.unwind_ff_reduction", + TOYOTA_COROLLA_TSS2_UNWIND_FF_REDUCTION) + return 1.0 + curve_weight * speed_weight * (boost * turn_in_weight - unwind_reduction * unwind_weight) + + +def get_toyota_corolla_tss2_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + """Taper only near-center crawl-speed torque during manual handoff.""" + center_weight = _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH) + low_speed_weight = _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED - v_ego) / + TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH) + reduction = _flm_vehicle_knob("toyota_corolla_tss2.center_output_taper_max", + TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_MAX) * center_weight * low_speed_weight + return max(1.0 - reduction, 0.65) + + def get_lexus_is_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: if desired_lateral_accel == 0.0: return 1.0 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 00df097ff..70f24ae9a 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -105,6 +105,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_sienna_4th_gen_ff_scale, get_sienna_4th_gen_friction_threshold, get_sienna_4th_gen_high_speed_output_taper_scale, + get_toyota_corolla_tss2_center_output_scale, + get_toyota_corolla_tss2_ff_scale, get_lexus_is_ff_scale, get_camry_ff_scale, get_ioniq_5_ff_scale, @@ -403,6 +405,22 @@ class TestLatControl: assert unwind_left < steady_left assert unwind_right < steady_right + def test_toyota_corolla_tss2_ff_scale_is_transition_only(self): + assert get_toyota_corolla_tss2_ff_scale(0.0, 0.0, 10.0) == 1.0 + steady = get_toyota_corolla_tss2_ff_scale(0.5, 0.0, 10.0) + turn_in = get_toyota_corolla_tss2_ff_scale(0.5, 0.8, 10.0) + unwind = get_toyota_corolla_tss2_ff_scale(0.5, -0.8, 10.0) + assert turn_in > steady + assert unwind < steady + assert get_toyota_corolla_tss2_ff_scale(0.5, 0.8, 40.0) < turn_in + + def test_toyota_corolla_tss2_center_output_taper_is_low_speed_and_center_only(self): + crawl_center = get_toyota_corolla_tss2_center_output_scale(0.0, 1.0) + cruise_center = get_toyota_corolla_tss2_center_output_scale(0.0, 15.0) + crawl_curve = get_toyota_corolla_tss2_center_output_scale(0.6, 1.0) + assert 0.65 <= crawl_center < cruise_center <= 1.0 + assert crawl_curve > crawl_center + def test_flm_standard_friction_curve_override(self): base = get_standard_friction_threshold(10.0) overrides = normalize_flm_overrides({ diff --git a/selfdrive/controls/tests/test_starpilot_acceleration.py b/selfdrive/controls/tests/test_starpilot_acceleration.py index 25ddb3112..5b1601006 100644 --- a/selfdrive/controls/tests/test_starpilot_acceleration.py +++ b/selfdrive/controls/tests/test_starpilot_acceleration.py @@ -44,6 +44,7 @@ def make_toggles(**overrides): "set_speed_limit": True, "set_speed_offset": 0, "speed_limit_controller": True, + "pulse_glide_speed_delta": 0.0, } defaults.update(overrides) return SimpleNamespace(**defaults) @@ -54,7 +55,7 @@ def make_lead(status=False, d_rel=150.0, v_lead=0.0, a_lead_k=0.0): def make_sm(*, set_speed_kph=100.0, lead_one=None, lead_two=None, standstill=False, force_decel=False, - eco_gear=False, sport_gear=False, force_coast=False, traffic_mode=False, v_ego_cluster=0.0): + eco_gear=False, sport_gear=False, force_coast=False, pulse_and_glide=False, traffic_mode=False, v_ego_cluster=0.0): return { "carState": SimpleNamespace(vCruise=set_speed_kph, standstill=standstill, vEgoCluster=v_ego_cluster), "controlsState": SimpleNamespace(forceDecel=force_decel), @@ -66,6 +67,7 @@ def make_sm(*, set_speed_kph=100.0, lead_one=None, lead_two=None, standstill=Fal ecoGear=eco_gear, sportGear=sport_gear, forceCoast=force_coast, + pulseAndGlide=pulse_and_glide, trafficModeEnabled=traffic_mode, ), } @@ -206,3 +208,35 @@ def test_force_coast_wins_over_traffic_mode_decel(): accel.update(5.0, sm, make_toggles()) assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO) + + +def test_pulse_and_glide_coasts_at_set_speed_then_resumes_below_delta(): + set_speed = 100.0 * CV.KPH_TO_MS + delta = 10.0 * CV.KPH_TO_MS + accel = StarPilotAcceleration(FakePlanner(v_cruise=set_speed)) + toggles = make_toggles( + pulse_glide_speed_delta=delta, + deceleration_profile=DECELERATION_PROFILES["STANDARD"], + ) + + accel.update(set_speed, make_sm(set_speed_kph=100.0, pulse_and_glide=True), toggles) + assert accel.pulse_glide_coasting is True + assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO) + + accel.update((90.0 * CV.KPH_TO_MS) - 0.05, make_sm(set_speed_kph=100.0, pulse_and_glide=True), toggles) + assert accel.pulse_glide_coasting is False + assert accel.min_accel == pytest.approx(A_CRUISE_MIN) + + accel.update(99.8 * CV.KPH_TO_MS, make_sm(set_speed_kph=100.0, pulse_and_glide=True), toggles) + assert accel.pulse_glide_coasting is True + assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO) + + +def test_pulse_and_glide_is_inert_when_disabled(): + accel = StarPilotAcceleration(FakePlanner(v_cruise=100.0 * CV.KPH_TO_MS)) + sm = make_sm(set_speed_kph=100.0, pulse_and_glide=False) + + accel.update(100.0 * CV.KPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["STANDARD"], pulse_glide_speed_delta=10.0)) + + assert accel.pulse_glide_coasting is False + assert accel.min_accel == pytest.approx(A_CRUISE_MIN) diff --git a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index 810677e70..f4214dde0 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -310,10 +310,11 @@ class ConditionalDriveModeView(AdjustorTogglesPanelView): "CCMSpeed": {"title": tr("Above Speed"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {}, "presets": [0, 35, 55, 65, 80]}, "CCMSpeedLead": {"title": tr("Speed w/ Lead"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {}, "presets": [0, 35, 55, 65, 80]}, "CCMSetSpeedMargin": {"title": tr("Set Speed Margin"), "min": 0, "max": 30.0 if is_metric else 15.0, "unit": speed_unit, "labels": {}, "presets": [0, 5, 10, 15]}, + "PulseGlideSpeedDelta": {"title": tr("Pulse and Glide Delta"), "min": 0.5, "max": 30.0 if is_metric else 15.0, "unit": speed_unit, "labels": {}, "presets": [1, 3, 5, 10]}, } spec = specs[key] - is_float = key == "CEModelStopTime" + is_float = key in ("CEModelStopTime", "PulseGlideSpeedDelta") original_val = float(self._controller._params.get_float(key) if is_float else self._controller._params.get_int(key)) def on_close(res, val): @@ -325,7 +326,7 @@ class ConditionalDriveModeView(AdjustorTogglesPanelView): gui_app.push_widget(AetherSliderDialog( title=spec["title"], - min_val=float(spec["min"]), max_val=float(spec["max"]), step=0.1 if is_float else 1.0, + min_val=float(spec["min"]), max_val=float(spec["max"]), step=0.1 if key == "CEModelStopTime" else (0.5 if key == "PulseGlideSpeedDelta" else 1.0), current_val=original_val, on_close=on_close, presets=[float(p) for p in spec["presets"]], unit=spec["unit"], labels=spec["labels"], color=PANEL_STYLE.accent @@ -759,6 +760,11 @@ class StarPilotLongitudinalLayout(_SettingsPage): on_click=lambda: self._show_slider("SetSpeedOffset", 0, 150 if self._is_metric() else 99, unit=self._speed_unit()), visible=lambda: self._params.get_bool("QOLLongitudinal")), + SettingRow("PulseGlideSpeedDelta", "value", tr_noop("Pulse and Glide Delta"), + subtitle=tr_noop("Coast this far below the current cruise target before accelerating back up."), + get_value=lambda: f"{self._params.get_float('PulseGlideSpeedDelta'):.1f}{self._speed_unit()}", + on_click=lambda: self._show_slider("PulseGlideSpeedDelta"), + visible=lambda: self._developer_feature_access()), SettingRow("MapGears", "toggle", tr_noop("Map Gears"), subtitle="", get_state=lambda: self._params.get_bool("MapGears"), @@ -1018,6 +1024,7 @@ class StarPilotLongitudinalLayout(_SettingsPage): "CESpeed", "CESpeedLead", "CESignalSpeed", "CCMSpeed", "CCMSpeedLead", "CCMSetSpeedMargin", ) + _SPEED_RESCALE_FLOAT_KEYS = ("PulseGlideSpeedDelta",) # Distance-typed int params stored in the current unit (ft or m); rescaled # when IsMetric flips so the numeric value stays correct in the new unit. @@ -1050,6 +1057,8 @@ class StarPilotLongitudinalLayout(_SettingsPage): speed_factor = CV.MPH_TO_KPH if current else CV.KPH_TO_MPH for key in self._SPEED_RESCALE_KEYS: self._params.put_int(key, int(round(self._params.get_int(key) * speed_factor))) + for key in self._SPEED_RESCALE_FLOAT_KEYS: + self._params.put_float(key, self._params.get_float(key) * speed_factor) distance_factor = CV.FOOT_TO_METER if current else CV.METER_TO_FOOT for key in self._DISTANCE_RESCALE_KEYS: self._params.put_int(key, int(round(self._params.get_int(key) * distance_factor))) @@ -1062,6 +1071,12 @@ class StarPilotLongitudinalLayout(_SettingsPage): """Abbreviated/Active-Only sub-toggles only make sense when sources are shown.""" return self._params.get_bool("SpeedLimitSources") + def _developer_feature_access(self) -> bool: + return ( + starpilot_state.car_state.hasOpenpilotLongitudinal and + (self._params.get_bool("DeveloperUI") or self._params.get_bool("GalaxyDeveloperMode")) + ) + def _speed_unit(self) -> str: self._maybe_rescale_on_metric_change() return " km/h" if self._is_metric() else " mph" diff --git a/selfdrive/ui/layouts/settings/starpilot/vehicle.py b/selfdrive/ui/layouts/settings/starpilot/vehicle.py index c9e8c7b68..99c4e2cad 100644 --- a/selfdrive/ui/layouts/settings/starpilot/vehicle.py +++ b/selfdrive/ui/layouts/settings/starpilot/vehicle.py @@ -52,6 +52,7 @@ ACTION_OPTIONS = [ {"id": 0, "name": tr_noop("No Action")}, {"id": 1, "name": tr_noop("Change Personality"), "requires_longitudinal": True}, {"id": 2, "name": tr_noop("Force Coast"), "requires_longitudinal": True}, + {"id": 14, "name": tr_noop("Pulse and Glide"), "requires_longitudinal": True, "requires_developer": True}, {"id": 3, "name": tr_noop("Pause Steering")}, {"id": 4, "name": tr_noop("Pause Accel/Brake"), "requires_longitudinal": True}, {"id": 5, "name": tr_noop("Toggle Experimental"), "requires_longitudinal": True}, @@ -811,11 +812,14 @@ class StarPilotVehicleSettingsLayout(_SettingsPage): allowed_ids = {0, 9, 10, 11, 12, 13} options = [o for o in ACTION_OPTIONS if o["id"] in allowed_ids] else: - allowed_ids = set(range(9)) | {11, 12, 13} + allowed_ids = set(range(9)) | {11, 12, 13, 14} if key == "LKASButtonControl": allowed_ids.add(9) + developer_access = self._params.get_bool("DeveloperUI") or self._params.get_bool("GalaxyDeveloperMode") options = [o for o in ACTION_OPTIONS - if o["id"] in allowed_ids and (cs.hasOpenpilotLongitudinal or not o.get("requires_longitudinal", False))] + if o["id"] in allowed_ids and + (cs.hasOpenpilotLongitudinal or not o.get("requires_longitudinal", False)) and + (developer_access or not o.get("requires_developer", False))] option_labels = [tr(o["name"]) for o in options] option_ids = [o["id"] for o in options] diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 29fab7440..32834c72c 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -9,6 +9,7 @@ from openpilot.selfdrive.ui.mici.onroad.speed_limit_utils import resolve_display from openpilot.selfdrive.ui.onroad.starpilot.navigation_card import NavigationCardRenderer from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.lib.utils import draw_circle_gradient_compat from openpilot.system.ui.lib.multilang import tr from openpilot.system.ui.lib.text_measure import measure_text_cached from openpilot.system.ui.widgets import Widget @@ -408,8 +409,8 @@ class HudRenderer(Widget): # draw drop shadow circle_radius = 162 // 2 - rl.draw_circle_gradient(rl.Vector2(x + circle_radius, y + circle_radius), circle_radius, - rl.Color(0, 0, 0, int(255 / 2 * alpha)), rl.BLANK) + draw_circle_gradient_compat(x + circle_radius, y + circle_radius, circle_radius, + rl.Color(0, 0, 0, int(255 / 2 * alpha)), rl.BLANK) set_speed_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha)) max_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha)) @@ -630,7 +631,7 @@ class HudRenderer(Widget): center = rl.Vector2(button_rect.x + button_rect.width / 2, button_rect.y + button_rect.height / 2) radius = min(button_rect.width, button_rect.height) / 2 - rl.draw_circle_gradient(center, radius, rl.Color(0, 0, 0, 90), rl.BLANK) + draw_circle_gradient_compat(center.x, center.y, radius, rl.Color(0, 0, 0, 90), rl.BLANK) rl.draw_circle(int(center.x), int(center.y), radius, fill) rl.draw_ring(center, radius - 6, radius, 0, 360, 48, outline) diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index cd4eeb27d..3cc367382 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -155,6 +155,7 @@ BUTTON_FUNCTIONS = { "NOTHING": 0, "PERSONALITY_PROFILE": 1, "FORCE_COAST": 2, + "PULSE_AND_GLIDE": 14, "PAUSE_LATERAL": 3, "PAUSE_LONGITUDINAL": 4, "EXPERIMENTAL_MODE": 5, @@ -950,10 +951,22 @@ class StarPilotVariables: condition=toggle.car_make == "gm" and toggle.has_pedal and "BOLT" in toggle.car_model, ) + developer_feature_access = self.params.get_bool("DeveloperUI") or self.params.get_bool("GalaxyDeveloperMode") + toggle.pulse_and_glide_available = toggle.openpilot_longitudinal and developer_feature_access + toggle.pulse_glide_speed_delta = self.get_value( + "PulseGlideSpeedDelta", + cast=float, + condition=toggle.pulse_and_glide_available, + conversion=speed_conversion, + min=0.5 * speed_conversion, + max=30.0 * speed_conversion, + ) + distance_button_control = self.get_button_function("DistanceButtonControl") toggle.experimental_mode_via_distance = toggle.openpilot_longitudinal and distance_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press = toggle.experimental_mode_via_distance toggle.force_coast_via_distance = toggle.openpilot_longitudinal and distance_button_control == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_distance = toggle.pulse_and_glide_available and distance_button_control == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_distance = distance_button_control == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_distance = toggle.openpilot_longitudinal and distance_button_control == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_distance = toggle.openpilot_longitudinal and distance_button_control == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -966,6 +979,7 @@ class StarPilotVariables: toggle.experimental_mode_via_distance_long = toggle.openpilot_longitudinal and distance_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_distance_long toggle.force_coast_via_distance_long = toggle.openpilot_longitudinal and distance_button_control_long == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_distance_long = toggle.pulse_and_glide_available and distance_button_control_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_distance_long = distance_button_control_long == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_distance_long = toggle.openpilot_longitudinal and distance_button_control_long == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_distance_long = toggle.openpilot_longitudinal and distance_button_control_long == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -978,6 +992,7 @@ class StarPilotVariables: toggle.experimental_mode_via_distance_very_long = toggle.openpilot_longitudinal and distance_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_distance_very_long toggle.force_coast_via_distance_very_long = toggle.openpilot_longitudinal and distance_button_control_very_long == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_distance_very_long = toggle.pulse_and_glide_available and distance_button_control_very_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_distance_very_long = distance_button_control_very_long == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_distance_very_long = toggle.openpilot_longitudinal and distance_button_control_very_long == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_distance_very_long = toggle.openpilot_longitudinal and distance_button_control_very_long == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -990,6 +1005,7 @@ class StarPilotVariables: toggle.experimental_mode_via_cancel = toggle.openpilot_longitudinal and cancel_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_cancel toggle.force_coast_via_cancel = toggle.openpilot_longitudinal and cancel_button_control == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_cancel = toggle.pulse_and_glide_available and cancel_button_control == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_cancel = cancel_button_control == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_cancel = toggle.openpilot_longitudinal and cancel_button_control == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_cancel = toggle.openpilot_longitudinal and cancel_button_control == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -1002,6 +1018,7 @@ class StarPilotVariables: toggle.experimental_mode_via_cancel_long = toggle.openpilot_longitudinal and cancel_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_cancel_long toggle.force_coast_via_cancel_long = toggle.openpilot_longitudinal and cancel_button_control_long == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_cancel_long = toggle.pulse_and_glide_available and cancel_button_control_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_cancel_long = cancel_button_control_long == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_cancel_long = toggle.openpilot_longitudinal and cancel_button_control_long == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_cancel_long = toggle.openpilot_longitudinal and cancel_button_control_long == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -1014,6 +1031,7 @@ class StarPilotVariables: toggle.experimental_mode_via_cancel_very_long = toggle.openpilot_longitudinal and cancel_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_cancel_very_long toggle.force_coast_via_cancel_very_long = toggle.openpilot_longitudinal and cancel_button_control_very_long == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_cancel_very_long = toggle.pulse_and_glide_available and cancel_button_control_very_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_cancel_very_long = cancel_button_control_very_long == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_cancel_very_long = toggle.openpilot_longitudinal and cancel_button_control_very_long == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_cancel_very_long = toggle.openpilot_longitudinal and cancel_button_control_very_long == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -1070,6 +1088,7 @@ class StarPilotVariables: toggle.experimental_mode_via_lkas = toggle.openpilot_longitudinal and lkas_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_lkas toggle.force_coast_via_lkas = toggle.openpilot_longitudinal and lkas_button_control == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_lkas = toggle.pulse_and_glide_available and lkas_button_control == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_lkas = lkas_button_control == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_lkas = toggle.openpilot_longitudinal and lkas_button_control == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_lkas = toggle.openpilot_longitudinal and lkas_button_control == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -1083,6 +1102,7 @@ class StarPilotVariables: toggle.experimental_mode_via_mode = toggle.openpilot_longitudinal and mode_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_mode toggle.force_coast_via_mode = toggle.openpilot_longitudinal and mode_button_control == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_mode = toggle.pulse_and_glide_available and mode_button_control == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_mode = mode_button_control == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_mode = toggle.openpilot_longitudinal and mode_button_control == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_mode = toggle.openpilot_longitudinal and mode_button_control == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -1095,6 +1115,7 @@ class StarPilotVariables: toggle.experimental_mode_via_mode_long = toggle.openpilot_longitudinal and mode_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_mode_long toggle.force_coast_via_mode_long = toggle.openpilot_longitudinal and mode_button_control_long == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_mode_long = toggle.pulse_and_glide_available and mode_button_control_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_mode_long = mode_button_control_long == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_mode_long = toggle.openpilot_longitudinal and mode_button_control_long == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_mode_long = toggle.openpilot_longitudinal and mode_button_control_long == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -1107,6 +1128,7 @@ class StarPilotVariables: toggle.experimental_mode_via_mode_very_long = toggle.openpilot_longitudinal and mode_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_mode_very_long toggle.force_coast_via_mode_very_long = toggle.openpilot_longitudinal and mode_button_control_very_long == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_mode_very_long = toggle.pulse_and_glide_available and mode_button_control_very_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_mode_very_long = mode_button_control_very_long == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_mode_very_long = toggle.openpilot_longitudinal and mode_button_control_very_long == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_mode_very_long = toggle.openpilot_longitudinal and mode_button_control_very_long == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -1119,6 +1141,7 @@ class StarPilotVariables: toggle.experimental_mode_via_star = toggle.openpilot_longitudinal and star_button_control == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_star toggle.force_coast_via_star = toggle.openpilot_longitudinal and star_button_control == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_star = toggle.pulse_and_glide_available and star_button_control == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_star = star_button_control == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_star = toggle.openpilot_longitudinal and star_button_control == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_star = toggle.openpilot_longitudinal and star_button_control == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -1131,6 +1154,7 @@ class StarPilotVariables: toggle.experimental_mode_via_star_long = toggle.openpilot_longitudinal and star_button_control_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_star_long toggle.force_coast_via_star_long = toggle.openpilot_longitudinal and star_button_control_long == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_star_long = toggle.pulse_and_glide_available and star_button_control_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_star_long = star_button_control_long == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_star_long = toggle.openpilot_longitudinal and star_button_control_long == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_star_long = toggle.openpilot_longitudinal and star_button_control_long == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] @@ -1143,6 +1167,7 @@ class StarPilotVariables: toggle.experimental_mode_via_star_very_long = toggle.openpilot_longitudinal and star_button_control_very_long == BUTTON_FUNCTIONS["EXPERIMENTAL_MODE"] toggle.experimental_mode_via_press |= toggle.experimental_mode_via_star_very_long toggle.force_coast_via_star_very_long = toggle.openpilot_longitudinal and star_button_control_very_long == BUTTON_FUNCTIONS["FORCE_COAST"] + toggle.pulse_and_glide_via_star_very_long = toggle.pulse_and_glide_available and star_button_control_very_long == BUTTON_FUNCTIONS["PULSE_AND_GLIDE"] toggle.pause_lateral_via_star_very_long = star_button_control_very_long == BUTTON_FUNCTIONS["PAUSE_LATERAL"] toggle.pause_longitudinal_via_star_very_long = toggle.openpilot_longitudinal and star_button_control_very_long == BUTTON_FUNCTIONS["PAUSE_LONGITUDINAL"] toggle.personality_profile_via_star_very_long = toggle.openpilot_longitudinal and star_button_control_very_long == BUTTON_FUNCTIONS["PERSONALITY_PROFILE"] diff --git a/starpilot/controls/lib/starpilot_acceleration.py b/starpilot/controls/lib/starpilot_acceleration.py index 30c398738..451b9b2d8 100644 --- a/starpilot/controls/lib/starpilot_acceleration.py +++ b/starpilot/controls/lib/starpilot_acceleration.py @@ -76,6 +76,9 @@ SLC_COAST_MIN_SPEED = 4.0 SLC_TARGET_EPS = 0.15 RELEVANT_LEAD_MIN_CLOSING_SPEED = 0.5 RELEVANT_LEAD_MIN_BRAKE = -0.4 +PULSE_GLIDE_MIN_TARGET_SPEED = 5.0 +PULSE_GLIDE_MIN_LOWER_SPEED = 3.0 +PULSE_GLIDE_HYSTERESIS = 0.25 # Drive mode -> profile mapping used by the map_acceleration / map_deceleration toggles. GEAR_STATE_PROFILES = { @@ -148,6 +151,61 @@ class StarPilotAcceleration: self.min_accel = 0 self.last_gear_state = "init" + self.pulse_glide_coasting = False + + def _update_pulse_glide(self, v_ego, sm, starpilot_toggles): + pulse_glide_enabled = bool(getattr(sm["starpilotCarState"], "pulseAndGlide", False)) + if not pulse_glide_enabled: + self.pulse_glide_coasting = False + return False + + raw_v_cruise_kph = 0.0 if sm["carState"].vCruise == V_CRUISE_UNSET else min(sm["carState"].vCruise, V_CRUISE_MAX) + if 0 < raw_v_cruise_kph < V_CRUISE_UNSET and getattr(starpilot_toggles, "set_speed_offset", 0) > 0: + raw_v_cruise_kph += starpilot_toggles.set_speed_offset + raw_v_cruise = raw_v_cruise_kph * CV.KPH_TO_MS + if raw_v_cruise <= 0.0: + self.pulse_glide_coasting = False + return False + + effective_slc_target = get_active_slc_control_target( + getattr(starpilot_toggles, "speed_limit_controller", False), + getattr(starpilot_toggles, "set_speed_limit", False), + getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0), + getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0), + getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0), + max(float(getattr(sm["carState"], "vEgoCluster", v_ego) or v_ego), v_ego) - v_ego, + allow_lower_override=(getattr(starpilot_toggles, "redneck_cruise", False) and + getattr(starpilot_toggles, "speed_limit_controller_override_set_speed", False)), + ) + v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise) + if effective_slc_target > 0.0: + v_target = min(v_target, effective_slc_target) + + delta = max(0.0, float(getattr(starpilot_toggles, "pulse_glide_speed_delta", 0.0))) + lower_target = v_target - delta + if v_target <= PULSE_GLIDE_MIN_TARGET_SPEED or lower_target < PULSE_GLIDE_MIN_LOWER_SPEED: + self.pulse_glide_coasting = False + return False + + has_relevant_lead = any(lead_is_braking_relevant(lead, v_ego) for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo)) + stop_context = ( + sm["carState"].standstill or + getattr(sm["controlsState"], "forceDecel", False) or + getattr(self.starpilot_planner.starpilot_cem, "stop_light_detected", False) or + getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False) or + getattr(self.starpilot_planner.starpilot_following, "disable_throttle", False) + ) + if has_relevant_lead or stop_context: + self.pulse_glide_coasting = False + return False + + if self.pulse_glide_coasting: + if v_ego <= lower_target + PULSE_GLIDE_HYSTERESIS: + self.pulse_glide_coasting = False + elif v_ego >= v_target - PULSE_GLIDE_HYSTERESIS: + self.pulse_glide_coasting = True + + return self.pulse_glide_coasting def update(self, v_ego, sm, starpilot_toggles): eco_gear = sm["starpilotCarState"].ecoGear @@ -188,7 +246,8 @@ class StarPilotAcceleration: if self.starpilot_planner.starpilot_weather.weather_id != 0: self.max_accel -= self.max_accel * self.starpilot_planner.starpilot_weather.reduce_acceleration - if sm["starpilotCarState"].forceCoast: + pulse_glide_coasting = self._update_pulse_glide(v_ego, sm, starpilot_toggles) + if sm["starpilotCarState"].forceCoast or pulse_glide_coasting: self.min_accel = A_CRUISE_MIN_ECO elif sm["starpilotCarState"].trafficModeEnabled: self.min_accel = A_CRUISE_MIN_TRAFFIC diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 444bd5e16..8e4502b4b 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -55,6 +55,7 @@ class StarPilotCard: self.cancelPressed_previously = False self.distancePressed_previously = False self.force_coast = False + self.pulse_and_glide = False self.modePressed_previously = False self.mode_counter = 0 self.customPressed_previously = False @@ -85,6 +86,9 @@ class StarPilotCard: self.handle_bookmark() elif getattr(starpilot_toggles, f"force_coast_via_{key}"): self.force_coast = not self.force_coast + elif getattr(starpilot_toggles, f"pulse_and_glide_via_{key}"): + if getattr(sm["carControl"], "longActive", False): + self.pulse_and_glide = not self.pulse_and_glide elif getattr(starpilot_toggles, f"pause_lateral_via_{key}"): self.pause_lateral = not self.pause_lateral elif getattr(starpilot_toggles, f"pause_longitudinal_via_{key}"): @@ -298,6 +302,10 @@ class StarPilotCard: self.handle_button_event("star_long", sm, starpilot_toggles) self.handle_button_event("star_very_long", sm, starpilot_toggles) + if not getattr(starpilot_toggles, "pulse_and_glide_available", False): + self.pulse_and_glide = False + self.pulse_and_glide &= bool(getattr(sm["carControl"], "longActive", False)) + self.pulse_and_glide &= not (carState.brakePressed or carState.gasPressed) self.force_coast &= not (carState.brakePressed or carState.gasPressed) starpilotCarState.accelPressed = self.accel_pressed @@ -309,6 +317,7 @@ class StarPilotCard: starpilotCarState.distanceLongPressed = self.very_long_press_threshold > self.gap_counter >= self.long_press_threshold starpilotCarState.distanceVeryLongPressed = self.gap_counter >= self.very_long_press_threshold starpilotCarState.forceCoast = self.force_coast + starpilotCarState.pulseAndGlide = self.pulse_and_glide starpilotCarState.isParked = carState.gearShifter == GearShifter.park starpilotCarState.pauseLateral = self.pause_lateral starpilotCarState.pauseLongitudinal = self.pause_longitudinal diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index a640f2dbc..684d6797f 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -77,6 +77,8 @@ def make_toggles(**overrides): "conditional_experimental_mode": False, "experimental_mode_via_lkas": False, "force_coast_via_lkas": False, + "pulse_and_glide_available": False, + "pulse_and_glide_via_lkas": False, "lkas_allowed_for_aol": False, "main_cruise_aol_toggle": False, "main_cruise_slc_adopt": False, @@ -90,13 +92,38 @@ def make_toggles(**overrides): return SimpleNamespace(**defaults) -def make_car_state(available=False, enabled=False, button_events=None): +def test_pulse_and_glide_requires_developer_access_and_active_longitudinal(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard(SimpleNamespace(brand="gm"), SimpleNamespace(alternativeExperience=0)) + sm = make_sm() + starpilot_car_state = SimpleNamespace(distancePressed=False) + toggles = make_toggles( + pulse_and_glide_available=True, + pulse_and_glide_via_lkas=True, + ) + + card.handle_button_event("lkas", sm, toggles) + assert card.pulse_and_glide is False + + sm["carControl"].longActive = True + card.handle_button_event("lkas", sm, toggles) + assert card.pulse_and_glide is True + + car_state = make_car_state(gas_pressed=True) + result = card.update(car_state, starpilot_car_state, sm, toggles) + assert result.pulseAndGlide is False + + +def make_car_state(available=False, enabled=False, button_events=None, brake_pressed=False, gas_pressed=False): return SimpleNamespace( buttonEvents=button_events or [], cruiseState=SimpleNamespace(available=available, enabled=enabled), gearShifter=spc.GearShifter.drive, - brakePressed=False, - gasPressed=False, + brakePressed=brake_pressed, + gasPressed=gas_pressed, standstill=False, vEgo=15.0, ) diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings.js b/starpilot/system/the_galaxy/assets/components/tools/device_settings.js index 5fa241d50..b16823652 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings.js +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings.js @@ -201,6 +201,7 @@ function scheduleSyncInputs() { function applySelectOptions(el, options) { el.innerHTML = "" for (const opt of options || []) { + if (opt?.developer_only && !state.values[GALAXY_DEVELOPER_MODE_KEY]) continue const o = document.createElement("option") o.value = String(opt.value) o.textContent = opt.label diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index f503db503..de1363238 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json @@ -3055,6 +3055,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3113,6 +3118,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3171,6 +3181,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3229,6 +3244,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3287,6 +3307,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3345,6 +3370,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3407,6 +3437,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3499,6 +3534,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3557,6 +3597,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3615,6 +3660,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3673,6 +3723,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3731,6 +3786,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -3789,6 +3849,11 @@ { "value": 13, "label": "Favorite #3" + }, + { + "value": 14, + "label": "Pulse and Glide", + "developer_only": true } ], "settings_tier": "simple" @@ -4110,6 +4175,19 @@ "parent_key": "GalaxyDeveloperMode", "settings_tier": "advanced" }, + { + "key": "PulseGlideSpeedDelta", + "label": "Pulse and Glide Speed Delta", + "description": "When Pulse and Glide is assigned to a wheel button, coast this far below the current cruise target before accelerating back up.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.5, + "max": 30.0, + "step": 0.5, + "precision": 1, + "parent_key": "GalaxyDeveloperMode", + "settings_tier": "advanced" + }, { "key": "CameraOffset", "label": "Camera Offset", diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index a99356038..b686244b3 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -83,7 +83,7 @@ from openpilot.starpilot.common.favorite_slots import ( ) from openpilot.starpilot.common.lateral_delay import full_lateral_delay from openpilot.starpilot.common.starpilot_utilities import delete_file, get_lock_status, run_cmd -from openpilot.starpilot.common.starpilot_variables import ACTIVE_THEME_PATH, ERROR_LOGS_PATH, EXCLUDED_KEYS, LEGACY_STARPILOT_PARAM_RENAMES, MAPS_PATH, MODELS_PATH, RESOURCES_REPO, SCREEN_RECORDINGS_PATH, STOCK_THEME_PATH, THEME_SAVE_PATH,\ +from openpilot.starpilot.common.starpilot_variables import ACTIVE_THEME_PATH, BUTTON_FUNCTIONS, ERROR_LOGS_PATH, EXCLUDED_KEYS, LEGACY_STARPILOT_PARAM_RENAMES, MAPS_PATH, MODELS_PATH, RESOURCES_REPO, SCREEN_RECORDINGS_PATH, STOCK_THEME_PATH, THEME_SAVE_PATH,\ default_ev_tuning_enabled, migrate_cancel_button_controls, update_starpilot_toggles from openpilot.starpilot.common.testing_grounds import ( DEFAULT_TESTING_GROUND_VARIANT as SHARED_DEFAULT_TESTING_GROUND_VARIANT, @@ -109,6 +109,13 @@ LEGACY_LATERAL_METHOD_API_PREFIX = "/api/" + "".join(("f", "t", "m")) VASM_CONFIGURATION_KEYS = {"VASMEnabled", "VASMConfidenceThreshold", "VASMSmoothSeconds", "VASMAnnotationConfig"} PIP_PREVIEW_CONFIGURATION_KEYS = {"PIPPreviewEnabled", "PIPPreviewMask", "PIPPreviewShowOnBlinker", "PIPPreviewShowOnBSM"} MODEL_SMOOTHING_KEYS = {"LatSmoothSeconds", "LongSmoothSeconds"} +PULSE_GLIDE_BUTTON_KEYS = { + "CancelButtonControl", "DistanceButtonControl", + "LongCancelButtonControl", "LongDistanceButtonControl", + "VeryLongCancelButtonControl", "VeryLongDistanceButtonControl", + "LKASButtonControl", "ModeButtonControl", "LongModeButtonControl", "VeryLongModeButtonControl", + "StarButtonControl", "LongStarButtonControl", "VeryLongStarButtonControl", +} SENTRY_NUMERIC_PARAM_BOUNDS = { "SentryModeSensitivity": (0.005, 1.0, 0.005), "SentryModeWarningTime": (0.1, 10.0, 0.1), @@ -4889,6 +4896,10 @@ def setup(app): if key not in allowed_keys: return jsonify({"error": f"Parameter '{key}' is not editable."}), 403 + if key == "PulseGlideSpeedDelta" or (key in PULSE_GLIDE_BUTTON_KEYS and str_val.strip() == str(BUTTON_FUNCTIONS["PULSE_AND_GLIDE"])): + if not params.get_bool("GalaxyDeveloperMode"): + return jsonify({"error": "Pulse and Glide is available only with Galaxy Developer Mode enabled."}), 403 + if key in SENTRY_NUMERIC_PARAM_BOUNDS: minimum, maximum, step = SENTRY_NUMERIC_PARAM_BOUNDS[key] try: diff --git a/system/ui/lib/tests/test_utils.py b/system/ui/lib/tests/test_utils.py new file mode 100644 index 000000000..9de047840 --- /dev/null +++ b/system/ui/lib/tests/test_utils.py @@ -0,0 +1,37 @@ +from types import SimpleNamespace + +from openpilot.system.ui.lib import utils + + +def test_draw_circle_gradient_uses_and_caches_vector_api(monkeypatch): + calls = [] + monkeypatch.setattr(utils.rl, "Vector2", lambda x, y: SimpleNamespace(x=x, y=y)) + monkeypatch.setattr(utils.rl, "draw_circle_gradient", lambda *args: calls.append(args)) + monkeypatch.setattr(utils, "_draw_circle_gradient_vector_api", None) + + utils.draw_circle_gradient_compat(10.5, 20.5, 30, "inner", "outer") + utils.draw_circle_gradient_compat(11.5, 21.5, 31, "inner", "outer") + + assert [len(call) for call in calls] == [4, 4] + assert calls[0][0].x == 10.5 + assert calls[0][0].y == 20.5 + + +def test_draw_circle_gradient_falls_back_and_caches_legacy_api(monkeypatch): + calls = [] + + def draw_circle_gradient(*args): + calls.append(args) + if len(args) == 4: + raise RuntimeError("function requires 5 arguments") + + monkeypatch.setattr(utils.rl, "Vector2", lambda x, y: SimpleNamespace(x=x, y=y)) + monkeypatch.setattr(utils.rl, "draw_circle_gradient", draw_circle_gradient) + monkeypatch.setattr(utils, "_draw_circle_gradient_vector_api", None) + + utils.draw_circle_gradient_compat(10.5, 20.5, 30, "inner", "outer") + utils.draw_circle_gradient_compat(11.5, 21.5, 31, "inner", "outer") + + assert [len(call) for call in calls] == [4, 5, 5] + assert calls[1][:2] == (10, 20) + assert calls[2][:2] == (11, 21) diff --git a/system/ui/lib/utils.py b/system/ui/lib/utils.py index 77035d0da..7d6005500 100644 --- a/system/ui/lib/utils.py +++ b/system/ui/lib/utils.py @@ -1,6 +1,32 @@ import pyray as rl +_draw_circle_gradient_vector_api: bool | None = None + + +def draw_circle_gradient_compat(center_x: float, center_y: float, radius: float, + inner: rl.Color, outer: rl.Color) -> None: + """Draw a circle gradient with either the Raylib 5 or Raylib 6 Python API.""" + global _draw_circle_gradient_vector_api + + if _draw_circle_gradient_vector_api is True: + rl.draw_circle_gradient(rl.Vector2(center_x, center_y), radius, inner, outer) + return + if _draw_circle_gradient_vector_api is False: + rl.draw_circle_gradient(int(center_x), int(center_y), radius, inner, outer) + return + + # Raylib 6 changed DrawCircleGradient from (x, y, radius, colors) to + # (Vector2, radius, colors). StarPilot supports devices on both bindings. + try: + rl.draw_circle_gradient(rl.Vector2(center_x, center_y), radius, inner, outer) + except (RuntimeError, TypeError): + rl.draw_circle_gradient(int(center_x), int(center_y), radius, inner, outer) + _draw_circle_gradient_vector_api = False + else: + _draw_circle_gradient_vector_api = True + + class GuiStyleContext: def __init__(self, styles: list[tuple[int, int, int]]): """styles is a list of tuples (control, prop, new_value)""" diff --git a/system/ui/widgets/mici_keyboard.py b/system/ui/widgets/mici_keyboard.py index bc3041aa3..2c235d719 100644 --- a/system/ui/widgets/mici_keyboard.py +++ b/system/ui/widgets/mici_keyboard.py @@ -3,6 +3,7 @@ import pyray as rl import numpy as np from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos, MouseEvent from openpilot.system.ui.lib.text_measure import measure_text_cached +from openpilot.system.ui.lib.utils import draw_circle_gradient_compat from openpilot.system.ui.widgets import Widget from openpilot.common.filter_simple import BounceFilter, FirstOrderFilter @@ -353,8 +354,8 @@ class MiciKeyboard(Widget): # draw black circle behind selected key circle_alpha = int(self._selected_key_filter.x * 225) - rl.draw_circle_gradient(rl.Vector2(key_x + key.rect.width / 2, key_y + key.rect.height / 2), - SELECTED_CHAR_FONT_SIZE, rl.Color(0, 0, 0, circle_alpha), rl.BLANK) + draw_circle_gradient_compat(key_x + key.rect.width / 2, key_y + key.rect.height / 2, + SELECTED_CHAR_FONT_SIZE, rl.Color(0, 0, 0, circle_alpha), rl.BLANK) else: # move other keys away from selected key a bit dx = key.original_position.x - self._closest_key[0].original_position.x