Gap Adjust Cruise: Fix carstate button press logic (commaai#274)

opgm · Sep 15, 2023 · 04b6255 · 04b6255
1 parent a809a7f
commit 04b6255
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 17 deletions.
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
@@ -470,25 +470,21 @@ def get_sp_distance(self, gac_tr, gac_max, gac_dict=None):
       gac_dict = GAC_DICT
     return next((key for key, value in gac_dict.items() if value == gac_tr), gac_max)
 
-  def get_sp_gac_mpc(self, gac_tr: int):
-    gac_tr = clip(gac_tr, 1, 3)
-    return str(gac_tr - 1)
-
   def toggle_gac(self, cs_out, CS, gac_button, gac_min, gac_max, gac_default, inc_dec):
     if not self.CP.openpilotLongitudinalControl:
-      CS.gac_tr = gac_default
+      CS.gac_tr = 2
       CS.gac_tr_cluster = gac_default
-      put_nonblocking("LongitudinalPersonality", self.get_sp_gac_mpc(3))
+      put_nonblocking("LongitudinalPersonality", 2)
       return cs_out, CS
     if gac_button:
       self.gac_button_counter += 1
     elif self.prev_gac_button and not gac_button and self.gac_button_counter < 50:
       self.gac_button_counter = 0
-      CS.gac_tr = self.get_sp_gac_state(CS.gac_tr, gac_min, gac_max, inc_dec)
-      put_nonblocking("LongitudinalPersonality", self.get_sp_gac_mpc(CS.gac_tr))
+      CS.gac_tr = self.get_sp_gac_state(CS.gac_tr, 0, 2, inc_dec)
+      put_nonblocking("LongitudinalPersonality", str(CS.gac_tr))
     else:
       self.gac_button_counter = 0
-    CS.gac_tr_cluster = clip(CS.gac_tr, gac_min, gac_max)
+    CS.gac_tr_cluster = clip(CS.gac_tr + 1, gac_min, gac_max)  # always 1 higher
     self.prev_gac_button = gac_button
     return cs_out, CS
 

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
@@ -316,26 +316,25 @@ def _update(self, c):
               self.CS.madsEnabled = not self.CS.madsEnabled
         self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
       if not self.CP.openpilotLongitudinalControl:
-        cluster_gap_display = 3
         self.CS.gac_tr_cluster = 3
-        put_nonblocking("LongitudinalPersonality", self.get_sp_gac_mpc(3))
+        put_nonblocking("LongitudinalPersonality", 2)
       else:
         gap_dist_button = bool(self.CS.gap_dist_button)
         if gap_dist_button:
           self.gac_button_counter += 1
         elif self.prev_gac_button and not gap_dist_button and self.gac_button_counter < 50:
           self.gac_button_counter = 0
-          follow_distance_converted = self.get_sp_gac_state(self.CS.follow_distance, GAC_MIN, GAC_MAX, "+")
-          gac_tr = self.get_sp_distance(follow_distance_converted, GAC_MAX, gac_dict=GAC_DICT)
+          pre_calculated_distance = 3 if self.CS.gac_tr == 3 else self.CS.follow_distance
+          follow_distance_converted = self.get_sp_gac_state(pre_calculated_distance, GAC_MIN, GAC_MAX, "+")
+          gac_tr = self.get_sp_distance(follow_distance_converted, GAC_MAX, gac_dict=GAC_DICT) - 1  # always 1 lower
           if gac_tr != self.CS.gac_tr:
-            put_nonblocking("LongitudinalPersonality", self.get_sp_gac_mpc(gac_tr))
+            put_nonblocking("LongitudinalPersonality", str(gac_tr))
             self.CS.gac_tr = gac_tr
         else:
           self.gac_button_counter = 0
         self.prev_gac_button = gap_dist_button
-        cluster_gap_display = self.CS.gac_tr
-      self.CS.gac_tr_cluster = clip(cluster_gap_display, GAC_MIN, GAC_MAX)
-      gap_distance = self.get_sp_distance(cluster_gap_display, GAC_MAX, gac_dict=GAC_DICT)
+        self.CS.gac_tr_cluster = clip(self.CS.gac_tr + 1, GAC_MIN, GAC_MAX)  # always 1 higher
+      gap_distance = self.get_sp_distance(self.CS.gac_tr_cluster, GAC_MAX, gac_dict=GAC_DICT)
       if self.CS.gac_send_counter < 10 and gap_distance != self.CS.follow_distance:
         self.CS.gac_send_counter += 1
         self.CS.gac_send = 1