diff --git a/panda b/panda index 2f79621893..1f8b03666d 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 2f7962189329ce5da5d68cb3261982aaa84b0deb +Subproject commit 1f8b03666d8f8ed377e9af89cffc5b1bdd70e922 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index a6dbf4a39e..451f309662 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,7 +1,7 @@ from opendbc.can.packer import CANPacker from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.subaru import subarucan -from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CarControllerParams +from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CarControllerParams, SubaruFlags class CarController: @@ -13,6 +13,7 @@ class CarController: self.es_lkas_cnt = -1 self.es_distance_cnt = -1 self.es_dashstatus_cnt = -1 + self.infotainmentstatus_cnt = -1 self.cruise_button_prev = 0 self.last_cancel_frame = 0 @@ -85,6 +86,10 @@ class CarController: hud_control.leftLaneDepart, hud_control.rightLaneDepart)) self.es_lkas_cnt = CS.es_lkas_msg["COUNTER"] + if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT and self.infotainmentstatus_cnt != CS.es_infotainmentstatus_msg["COUNTER"]: + can_sends.append(subarucan.create_infotainmentstatus(self.packer, CS.es_infotainmentstatus_msg, hud_control.visualAlert)) + self.infotainmentstatus_cnt = CS.es_infotainmentstatus_msg["COUNTER"] + new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index ba873c48d7..0d92b4c162 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine from common.conversions import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser -from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS +from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags class CarState(CarStateBase): @@ -82,6 +82,8 @@ class CarState(CarStateBase): cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"]) + if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: + self.es_infotainmentstatus_msg = copy.copy(cp_cam.vl["INFOTAINMENT_STATUS"]) return ret @@ -301,6 +303,15 @@ class CarState(CarStateBase): signals += CarState.get_global_es_distance_signals()[0] checks += CarState.get_global_es_distance_signals()[1] + if CP.flags & SubaruFlags.SEND_INFOTAINMENT: + signals += [ + ("LKAS_State_Infotainment", "INFOTAINMENT_STATUS"), + ("LKAS_Blue_Lines", "INFOTAINMENT_STATUS"), + ("Signal1", "INFOTAINMENT_STATUS"), + ("Signal2", "INFOTAINMENT_STATUS"), + ] + checks.append(("INFOTAINMENT_STATUS", 10)) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) @staticmethod diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 733482ef82..6329070ef6 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -3,7 +3,7 @@ from cereal import car from panda import Panda from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase -from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS +from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags class CarInterface(CarInterfaceBase): @@ -15,6 +15,10 @@ class CarInterface(CarInterfaceBase): ret.dashcamOnly = candidate in PREGLOBAL_CARS ret.autoResumeSng = False + # Detect infotainment message sent from the camera + if candidate not in PREGLOBAL_CARS and 0x323 in fingerprint[2]: + ret.flags |= SubaruFlags.SEND_INFOTAINMENT.value + if candidate in PREGLOBAL_CARS: ret.enableBsm = 0x25c in fingerprint[0] ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruLegacy)] diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index d83b639a41..8615b60c62 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -71,11 +71,26 @@ def create_es_dashstatus(packer, dashstatus_msg): values = copy.copy(dashstatus_msg) # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts - if values["LKAS_State_Msg"] in [2, 3]: + if values["LKAS_State_Msg"] in (2, 3): values["LKAS_State_Msg"] = 0 return packer.make_can_msg("ES_DashStatus", 0, values) +def create_infotainmentstatus(packer, infotainmentstatus_msg, visual_alert): + # Filter stock LKAS disabled and Keep hands on steering wheel OFF alerts + if infotainmentstatus_msg["LKAS_State_Infotainment"] in (3, 4): + infotainmentstatus_msg["LKAS_State_Infotainment"] = 0 + + # Show Keep hands on wheel alert for openpilot steerRequired alert + if visual_alert == VisualAlert.steerRequired: + infotainmentstatus_msg["LKAS_State_Infotainment"] = 3 + + # Show Obstacle Detected for fcw + if visual_alert == VisualAlert.fcw: + infotainmentstatus_msg["LKAS_State_Infotainment"] = 2 + + return packer.make_can_msg("INFOTAINMENT_STATUS", 0, infotainmentstatus_msg) + # *** Subaru Pre-global *** def subaru_preglobal_checksum(packer, values, addr): diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index f77de3b34b..33b46c8c2b 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -1,5 +1,5 @@ from dataclasses import dataclass -from enum import Enum +from enum import Enum, IntFlag from typing import Dict, List, Union from cereal import car @@ -30,6 +30,10 @@ class CarControllerParams: self.STEER_MAX = 2047 +class SubaruFlags(IntFlag): + SEND_INFOTAINMENT = 1 + + class CAR: # Global platform ASCENT = "SUBARU ASCENT LIMITED 2019" diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index af63dab2f1..04aacbddf8 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -1bb3f665191e1b75c1b786f60e76d51b274f98ae +818a0f25985c95bb56e54a4ac3cfb3126e6044e2 \ No newline at end of file