From 04b62555a09e943faadf0e2c8db63c1acdeadb11 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 15 Sep 2023 04:02:12 -0400 Subject: [PATCH] Gap Adjust Cruise: Fix carstate button press logic (#274) --- selfdrive/car/interfaces.py | 14 +++++--------- selfdrive/car/toyota/interface.py | 15 +++++++-------- 2 files changed, 12 insertions(+), 17 deletions(-) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index dedde94a2ae6f7..2e1156d849d430 100644 --- 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 index d0b01f2e98c216..4b4c66c36b4b29 100644 --- 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