diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index 5d2bd23d24..e5b2efbbe9 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/board/safety/safety_subaru.h @@ -116,6 +116,7 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return -1; } // global platform + // ES LKAS if (addr == 0x122) { return -1; } diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 98deb56467..9a098ea535 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -36,7 +36,7 @@ class CarController(object): print(DBC) self.packer = CANPacker(DBC[car_fingerprint]['pt']) - def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert): + def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line): """ Controls thread """ P = self.params @@ -70,7 +70,7 @@ class CarController(object): 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)) + can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line)) self.es_lkas_cnt = CS.es_lkas_msg["Counter"] sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 8de0760b4b..077f591a14 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -217,5 +217,6 @@ class CarInterface(object): def apply(self, c): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, - c.cruiseControl.cancel, c.hudControl.visualAlert) + c.cruiseControl.cancel, c.hudControl.visualAlert, + c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible) self.frame += 1 diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 5e17cd72c7..af2e2598e8 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -43,12 +43,22 @@ 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): +def create_es_lkas(packer, es_lkas_msg, visual_alert, left_line, right_line): values = copy.copy(es_lkas_msg) if visual_alert == VisualAlert.steerRequired: values["Keep_Hands_On_Wheel"] = 1 + if left_line: + values["LKAS_Left_Line_Visible"] = 1 + else: + values["LKAS_Left_Line_Visible"] = 0 + + if right_line: + values["LKAS_Right_Line_Visible"] = 1 + else: + values["LKAS_Right_Line_Visible"] = 0 + values["Checksum"] = subaru_checksum(packer, values, 802) return packer.make_can_msg("ES_LKAS_State", 0, values)