diff --git a/common/params.cc b/common/params.cc index 4f02b6f17e..8f8ca0fc2b 100644 --- a/common/params.cc +++ b/common/params.cc @@ -295,6 +295,8 @@ std::unordered_map keys = { {"OsmDownloadedDate", PERSISTENT}, {"PathOffset", PERSISTENT | BACKUP}, {"PauseLateralSpeed", PERSISTENT | BACKUP}, + {"PCMVCruiseOverride", PERSISTENT | BACKUP}, + {"PCMVCruiseOverrideSpeed", PERSISTENT | BACKUP}, {"QuietDrive", PERSISTENT | BACKUP}, {"RoadEdge", PERSISTENT | BACKUP}, {"ReverseAccChange", PERSISTENT | BACKUP}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5ad4be0040..bda4d57eb8 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -182,6 +182,8 @@ class Controls: self.enable_mads = self.params.get_bool("EnableMads") self.mads_disengage_lateral_on_brake = self.params.get_bool("DisengageLateralOnBrake") self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake + self.pcm_v_cruise_override = self.params.get_bool("PCMVCruiseOverride") + self.pcm_v_cruise_override_speed = int(self.params.get("PCMVCruiseOverrideSpeed", encoding="utf-8")) self.process_not_running = False self.custom_model, self.model_gen = get_model_generation(self.params) @@ -493,7 +495,9 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric, self.reverse_acc_change, self.sm['longitudinalPlanSP']) + # sp - PCM speed override + sp_override_speed = self.pcm_v_cruise_override_speed if self.pcm_v_cruise_override else False + self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric, self.reverse_acc_change, sp_override_speed, self.sm['longitudinalPlanSP']) # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index d5697dd530..3f62e8bf08 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -88,11 +88,16 @@ class VCruiseHelper: self.slc_state_prev = SpeedLimitControlState.inactive self.slc_speed_limit_offsetted = 0 + # sp: PCM speed override + self.sp_override_v_cruise_kph = V_CRUISE_UNSET + self.sp_override_cruise_speed_last = V_CRUISE_UNSET + self.sp_override_enabled_last = False + @property def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET - def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, long_plan_sp): + def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, sp_override_speed, long_plan_sp): self.v_cruise_kph_last = self.v_cruise_kph self.slc_state = long_plan_sp.speedLimitControlState @@ -107,9 +112,28 @@ class VCruiseHelper: self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled) else: - self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH - self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH + if enabled and sp_override_speed and CS.cruiseState.speed * CV.MS_TO_KPH < sp_override_speed: + if self.sp_override_v_cruise_kph == V_CRUISE_UNSET: + self.sp_override_v_cruise_kph = max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_MIN) + else: + self.sp_override_v_cruise_kph = V_CRUISE_UNSET + + # when we have an override_speed, use it + if self.sp_override_v_cruise_kph != V_CRUISE_UNSET: + self.v_cruise_kph = self.sp_override_v_cruise_kph + self.v_cruise_cluster_kph = self.sp_override_v_cruise_kph + else: + self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH + + #print("sp_override_v_cruise_kph:", self.sp_override_v_cruise_kph) + #print("v_cruise_kph:", self.v_cruise_kph) + #print("v_cruise_cluster_kph:", self.v_cruise_cluster_kph) + + self.sp_override_cruise_speed_last = CS.cruiseState.speed + self.sp_override_enabled_last = enabled else: + self.sp_override_v_cruise_kph = V_CRUISE_UNSET self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET diff --git a/system/manager/manager.py b/system/manager/manager.py index be931353e0..3dd8d75c47 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -84,6 +84,8 @@ def manager_init() -> None: ("OnroadSettings", "1"), ("PathOffset", "0"), ("PauseLateralSpeed", "0"), + ("PCMVCruiseOverride", "0"), + ("PCMVCruiseOverrideSpeed", "30"), ("ReverseAccChange", "0"), ("ScreenRecorder", "1"), ("ShowDebugUI", "1"),