Subaru: infotainment status intercept and rewrite (#27829)

* wip

* wip

* wip

* wip

* wip

* wip

* wip

* wip

* wip

* remove compiler flag stuff

* force error for testing

* bump panda and fix mistake

* wip

* bump panda

* wip

* wip

* forgot checksum and stuff

* add to signals

* rename

* test

* test

* bump panda and opendbc

* bump cereal

* add blank filler

* test

* add steer required to infotainment

* fix missing bit

* fix missing bit

* cleanup for PR

* cleanup for PR

* bump submodules

* wip

* wip

* i wonder what 1 is

* seemed to also be obstacle detected?

* bumppanda

* only if global

* only global gen 2

* fix order

* detect and send

* detect in rest of places

* update refs

* bumpopendbc

* copy whole message (camera sometimes sets these signals)

* bumppanda

* update refs!

* bumppanda

* let's not change this

* same as all the other functions

* Revert "same as all the other functions"

This reverts commit e86dbb695ef84a18f3123720fed37a6b81391cc4.

---------

Co-authored-by: Shane Smiskol <shane@smiskol.com>
old-commit-hash: d59bfa5519

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

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

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)]

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):

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"

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-1bb3f665191e1b75c1b786f60e76d51b274f98ae
+818a0f25985c95bb56e54a4ac3cfb3126e6044e2