diff --git a/common/params_keys.h b/common/params_keys.h index fbbf9305a..a157672ab 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -482,6 +482,8 @@ inline static std::unordered_map keys = { {"SNGHack", {PERSISTENT, BOOL, "1", "0", 2}}, {"SoundPack", {PERSISTENT, STRING, "frog", "stock", 0}}, {"SoundToDownload", {CLEAR_ON_MANAGER_START, STRING, "", ""}}, + {"SLCAdoptSpeedLimit", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, + {"SLCForceCruiseSpeed", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}}, {"SpeedLimitAccepted", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, {"SpeedLimitChangedAlert", {PERSISTENT, BOOL, "0", "0", 0}}, {"SpeedLimitController", {PERSISTENT, BOOL, "0", "0", 0}}, diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 54275c608..46a35c283 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -19,7 +19,8 @@ from opendbc.car.car_helpers import get_car, interfaces from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp -from openpilot.selfdrive.car.cruise import VCruiseHelper +from openpilot.common.constants import CV +from openpilot.selfdrive.car.cruise import VCruiseHelper, IMPERIAL_INCREMENT, V_CRUISE_MAX, V_CRUISE_MIN from openpilot.selfdrive.car.car_specific import MockCarState from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles, update_starpilot_toggles @@ -83,6 +84,7 @@ class Car: self.last_actuators_output = structs.CarControl.Actuators() self.params = Params() + self.params_memory = Params(memory=True) self.can_callbacks = can_comm_callbacks(self.can_sock, self.pm.sock['sendcan']) @@ -225,6 +227,16 @@ class Car: self.sm['starpilotPlan'].speedLimitChanged, self.starpilot_toggles, ) + slc_force_speed = self.params_memory.get_float("SLCForceCruiseSpeed") + if slc_force_speed > 0: + if self.is_metric: + new_cruise_kph = round(slc_force_speed * CV.MS_TO_KPH) + else: + new_cruise_kph = round(slc_force_speed * CV.MS_TO_MPH) * IMPERIAL_INCREMENT + self.v_cruise_helper.v_cruise_kph = max(min(new_cruise_kph, V_CRUISE_MAX), V_CRUISE_MIN) + self.v_cruise_helper.v_cruise_cluster_kph = self.v_cruise_helper.v_cruise_kph + self.params_memory.remove("SLCForceCruiseSpeed") + if self.sm['carControl'].enabled and not self.CC_prev.enabled: # Use CarState w/ buttons from the step selfdrived enables on desired_speed_limit = self.sm['starpilotPlan'].slcSpeedLimit + self.sm['starpilotPlan'].slcSpeedLimitOffset diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index 2bd845aec..f9c4ca23b 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/starpilot/controls/lib/speed_limit_controller.py @@ -57,6 +57,8 @@ class SpeedLimitController: self.previous_source = "None" self.source = "None" + self._slc_adopt_counter = 0 + mapbox_requests_raw = self.starpilot_planner.params.get("MapBoxRequests", encoding="utf-8") try: self.mapbox_requests = json.loads(mapbox_requests_raw or "{}") @@ -382,6 +384,21 @@ class SpeedLimitController: self.speed_limit_changed_timer = 0 self.unconfirmed_speed_limit = 0 + self._slc_adopt_counter += 1 + if self._slc_adopt_counter % 4 == 0 and self.starpilot_planner.params_memory.get_bool("SLCAdoptSpeedLimit"): + self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit") + if desired_target > 0: + self.overridden_speed = 0 + self.denied_target = 0 + self.source = desired_source + self.target = desired_target + self.previous_source = desired_source + self.previous_target = desired_target + self.speed_limit_changed_timer = 0 + self.unconfirmed_speed_limit = 0 + self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", self.target) + self.starpilot_planner.params_memory.put_float("SLCForceCruiseSpeed", self.target + self.offset) + def update_map_speed_limit(self, v_ego, sm): next_speed_limit_distance = sm["mapdOut"].nextSpeedLimitDistance diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index eb9ad119a..268830efa 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -84,8 +84,11 @@ class StarPilotCard: for be in carState.buttonEvents: if be.type == ButtonType.lkas and be.pressed and starpilot_toggles.always_on_lateral_lkas: self.always_on_lateral_allowed = not self.always_on_lateral_allowed - elif be.type == ButtonType.mainCruise and be.pressed and starpilot_toggles.always_on_lateral_main: - self.always_on_lateral_allowed = not self.always_on_lateral_allowed + elif be.type == ButtonType.mainCruise and be.pressed: + if starpilot_toggles.always_on_lateral_main: + self.always_on_lateral_allowed = not self.always_on_lateral_allowed + elif starpilot_toggles.speed_limit_controller: + self.params_memory.put_bool("SLCAdoptSpeedLimit", True) elif starpilot_toggles.always_on_lateral_main: self.always_on_lateral_allowed = carState.cruiseState.available