From 342effe5dd918770a25ef0e6a1d7c89d2daa4fc6 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 15 Jun 2024 00:17:52 -0700 Subject: [PATCH 1/3] toyota low speed override --- common/params.cc | 2 ++ selfdrive/controls/controlsd.py | 6 ++++- selfdrive/controls/lib/drive_helpers.py | 30 ++++++++++++++++++++++--- system/manager/manager.py | 2 ++ 4 files changed, 36 insertions(+), 4 deletions(-) diff --git a/common/params.cc b/common/params.cc index 73906d35f4..4a450f2fad 100644 --- a/common/params.cc +++ b/common/params.cc @@ -317,6 +317,8 @@ std::unordered_map keys = { {"TorqueFriction", PERSISTENT | BACKUP}, {"TorqueMaxLatAccel", PERSISTENT | BACKUP}, {"TorquedOverride", PERSISTENT | BACKUP}, + {"ToyotaCruiseOverride", PERSISTENT | BACKUP}, + {"ToyotaCruiseOverrideSpeed", PERSISTENT | BACKUP}, {"ToyotaSnG", PERSISTENT | BACKUP}, {"ToyotaTSS2Long", PERSISTENT | BACKUP}, {"TrueVEgoUi", PERSISTENT | BACKUP}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 21bf73cf9a..5a98bcf50f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -181,6 +181,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.toyota_cruise_override = self.params.get_bool("ToyotaCruiseOverride") + self.toyota_cruise_override_speed = int(self.params.get("ToyotaCruiseOverrideSpeed", encoding="utf-8")) self.process_not_running = False self.custom_model, self.model_gen = get_model_generation(self.params) @@ -488,7 +490,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 - toyota low speed override + sp_override_speed = self.toyota_cruise_override_speed if self.toyota_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 7ea0b17834..05e6777eb2 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -84,11 +84,16 @@ class VCruiseHelper: self.slc_state_prev = SpeedLimitControlState.inactive self.slc_speed_limit_offsetted = 0 + #sp: toyota low 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 @@ -103,9 +108,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 f3a5b1b366..6ebf6d183a 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -111,6 +111,8 @@ def manager_init() -> None: ("CustomDrivingModel", "0"), ("DrivingModelGeneration", "4"), ("LastSunnylinkPingTime", "0"), + ("ToyotaCruiseOverride", "0"), + ("ToyotaCruiseOverrideSpeed", "30"), ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8'))) From 59d960addf2370a93d56f2c099c650142697ffde Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 2 Jul 2024 10:46:32 -0400 Subject: [PATCH 2/3] housekeeping --- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/drive_helpers.py | 2 +- system/manager/manager.py | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5eb690b098..8cb3c9ea9d 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -494,7 +494,7 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - #sp - toyota low speed override + # sp - toyota low speed override sp_override_speed = self.toyota_cruise_override_speed if self.toyota_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']) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 05e6777eb2..7c77f3013a 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -84,7 +84,7 @@ class VCruiseHelper: self.slc_state_prev = SpeedLimitControlState.inactive self.slc_speed_limit_offsetted = 0 - #sp: toyota low speed override + # sp: toyota low 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 diff --git a/system/manager/manager.py b/system/manager/manager.py index 434f4cb851..cfabbfd583 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -102,6 +102,8 @@ def manager_init() -> None: ("ToyotaAutoHold", "0"), ("ToyotaAutoLockBySpeed", "0"), ("ToyotaAutoUnlockByShifter", "0"), + ("ToyotaCruiseOverride", "0"), + ("ToyotaCruiseOverrideSpeed", "30"), ("ToyotaEnhancedBsm", "0"), ("TrueVEgoUi", "0"), ("TurnSpeedControl", "0"), @@ -117,8 +119,6 @@ def manager_init() -> None: ("CustomDrivingModel", "0"), ("DrivingModelGeneration", "4"), ("LastSunnylinkPingTime", "0"), - ("ToyotaCruiseOverride", "0"), - ("ToyotaCruiseOverrideSpeed", "30"), ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8'))) From 5d097fa5deffc6b954f9a6666dd1fa533dc14546 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 2 Jul 2024 10:57:40 -0400 Subject: [PATCH 3/3] rename to PCM Speed Override --- common/params.cc | 4 ++-- selfdrive/controls/controlsd.py | 8 ++++---- selfdrive/controls/lib/drive_helpers.py | 2 +- system/manager/manager.py | 4 ++-- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/common/params.cc b/common/params.cc index 66acd7c044..61e2ae8225 100644 --- a/common/params.cc +++ b/common/params.cc @@ -293,6 +293,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}, @@ -322,8 +324,6 @@ std::unordered_map keys = { {"ToyotaAutoHold", PERSISTENT | BACKUP}, {"ToyotaAutoLockBySpeed", PERSISTENT | BACKUP}, {"ToyotaAutoUnlockByShifter", PERSISTENT | BACKUP}, - {"ToyotaCruiseOverride", PERSISTENT | BACKUP}, - {"ToyotaCruiseOverrideSpeed", PERSISTENT | BACKUP}, {"ToyotaEnhancedBsm", PERSISTENT | BACKUP}, {"ToyotaSnG", PERSISTENT | BACKUP}, {"ToyotaTSS2Long", PERSISTENT | BACKUP}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8cb3c9ea9d..ebb44660ed 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -181,8 +181,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.toyota_cruise_override = self.params.get_bool("ToyotaCruiseOverride") - self.toyota_cruise_override_speed = int(self.params.get("ToyotaCruiseOverrideSpeed", encoding="utf-8")) + 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) @@ -494,8 +494,8 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - # sp - toyota low speed override - sp_override_speed = self.toyota_cruise_override_speed if self.toyota_cruise_override else False + # 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 diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 7c77f3013a..e9f78ef969 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -84,7 +84,7 @@ class VCruiseHelper: self.slc_state_prev = SpeedLimitControlState.inactive self.slc_speed_limit_offsetted = 0 - # sp: toyota low speed override + # 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 diff --git a/system/manager/manager.py b/system/manager/manager.py index cfabbfd583..eb6e956a1c 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"), @@ -102,8 +104,6 @@ def manager_init() -> None: ("ToyotaAutoHold", "0"), ("ToyotaAutoLockBySpeed", "0"), ("ToyotaAutoUnlockByShifter", "0"), - ("ToyotaCruiseOverride", "0"), - ("ToyotaCruiseOverrideSpeed", "30"), ("ToyotaEnhancedBsm", "0"), ("TrueVEgoUi", "0"), ("TurnSpeedControl", "0"),