From d32b1f703ce9606c46177ba70e4de5082d588d06 Mon Sep 17 00:00:00 2001 From: martinl Date: Sat, 4 Sep 2021 02:53:49 +0300 Subject: [PATCH] Subaru: replace stock LKAS alerts with openpilot's (#21993) * Add stock LKAS alert filters and openpilot LKAS state indicator * Update ES_LKAS_State signals * bump opendbc Co-authored-by: Adeeb Shihadeh --- opendbc | 2 +- selfdrive/car/subaru/carcontroller.py | 2 +- selfdrive/car/subaru/carstate.py | 10 ++++------ selfdrive/car/subaru/subarucan.py | 21 ++++++++++++++++++--- 4 files changed, 24 insertions(+), 11 deletions(-) diff --git a/opendbc b/opendbc index 78c639f813..68bab3746f 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 78c639f813430d422bc04d06ac78242c3475bed6 +Subproject commit 68bab3746fa3aaa2b6191f3c7b8a51431557eed7 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 80ac2542ab..6163d7329c 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -69,7 +69,7 @@ class CarController(): self.es_distance_cnt = CS.es_distance_msg["Counter"] if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]: - can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart)) + can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] return can_sends diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 6e307562aa..8ff9dad38a 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -211,20 +211,18 @@ class CarState(CarStateBase): ("Signal6", "ES_Distance", 0), ("Counter", "ES_LKAS_State", 0), - ("Keep_Hands_On_Wheel", "ES_LKAS_State", 0), - ("Empty_Box", "ES_LKAS_State", 0), + ("LKAS_Alert_Msg", "ES_LKAS_State", 0), ("Signal1", "ES_LKAS_State", 0), ("LKAS_ACTIVE", "ES_LKAS_State", 0), + ("LKAS_Dash_State", "ES_LKAS_State", 0), ("Signal2", "ES_LKAS_State", 0), ("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0), - ("LKAS_ENABLE_3", "ES_LKAS_State", 0), + ("LKAS_Left_Line_Enable", "ES_LKAS_State", 0), ("LKAS_Left_Line_Light_Blink", "ES_LKAS_State", 0), - ("LKAS_ENABLE_2", "ES_LKAS_State", 0), + ("LKAS_Right_Line_Enable", "ES_LKAS_State", 0), ("LKAS_Right_Line_Light_Blink", "ES_LKAS_State", 0), ("LKAS_Left_Line_Visible", "ES_LKAS_State", 0), - ("LKAS_Left_Line_Green", "ES_LKAS_State", 0), ("LKAS_Right_Line_Visible", "ES_LKAS_State", 0), - ("LKAS_Right_Line_Green", "ES_LKAS_State", 0), ("LKAS_Alert", "ES_LKAS_State", 0), ("Signal3", "ES_LKAS_State", 0), ] diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 5a98ea5548..6485de7901 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -27,20 +27,35 @@ def create_es_distance(packer, es_distance_msg, pcm_cancel_cmd): return packer.make_can_msg("ES_Distance", 0, values) -def create_es_lkas(packer, es_lkas_msg, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): +def create_es_lkas(packer, es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): values = copy.copy(es_lkas_msg) + + # Filter the stock LKAS "Keep hands on wheel" alert + if values["LKAS_Alert_Msg"] == 1: + values["LKAS_Alert_Msg"] = 0 + + # Filter the stock LKAS sending an audible alert when it turns off LKAS + if values["LKAS_Alert"] == 27: + values["LKAS_Alert"] = 0 + + # Show Keep hands on wheel alert for openpilot steerRequired alert if visual_alert == VisualAlert.steerRequired: - values["Keep_Hands_On_Wheel"] = 1 + values["LKAS_Alert_Msg"] = 1 # Ensure we don't overwrite potentially more important alerts from stock (e.g. FCW) if visual_alert == VisualAlert.ldw and values["LKAS_Alert"] == 0: if left_lane_depart: values["LKAS_Alert"] = 12 # Left lane departure dash alert - elif right_lane_depart: values["LKAS_Alert"] = 11 # Right lane departure dash alert + if enabled: + values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines + values["LKAS_Dash_State"] = 2 # Green enabled indicator + else: + values["LKAS_Dash_State"] = 0 # LKAS Not enabled + values["LKAS_Left_Line_Visible"] = int(left_line) values["LKAS_Right_Line_Visible"] = int(right_line)