Merge branch 'toyota-lowspeed-override' into master-dev-c3

common/params.cc

@@ -295,6 +295,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"OsmDownloadedDate", PERSISTENT},
     {"PathOffset", PERSISTENT | BACKUP},
     {"PauseLateralSpeed", PERSISTENT | BACKUP},
+    {"PCMVCruiseOverride", PERSISTENT | BACKUP},
+    {"PCMVCruiseOverrideSpeed", PERSISTENT | BACKUP},
     {"QuietDrive", PERSISTENT | BACKUP},
     {"RoadEdge", PERSISTENT | BACKUP},
     {"ReverseAccChange", PERSISTENT | BACKUP},

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

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
 

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"),