Honda: emulate convenience blinker from stalk (#21387)

* Honda: emulate convenience blinker from stalk

* comment

* light->lamp

* reset on rising edge

* update ref

selfdrive/car/honda/carstate.py

@@ -275,8 +275,8 @@ class CarState(CarStateBase):
     self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"]
     self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"]
 
-    ret.leftBlinker = cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"] != 0
-    ret.rightBlinker = cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"] != 0
+    ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
+      250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
     self.brake_hold = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"]
 
     if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,

selfdrive/car/hyundai/carstate.py

@@ -29,8 +29,8 @@ class CarState(CarStateBase):
     ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
     ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
     ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
-    ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"],
-                                                            cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
+    ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
+      50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
     ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
     ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
     ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD

selfdrive/car/interfaces.py

@@ -171,6 +171,8 @@ class CarStateBase:
     self.cruise_buttons = 0
     self.left_blinker_cnt = 0
     self.right_blinker_cnt = 0
+    self.left_blinker_prev = False
+    self.right_blinker_prev = False
 
     # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
     # R = 1e3
@@ -186,11 +188,37 @@ class CarStateBase:
     v_ego_x = self.v_ego_kf.update(v_ego_raw)
     return float(v_ego_x[0]), float(v_ego_x[1])
 
-  def update_blinker(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool):
+  def update_blinker_from_lamp(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool):
+    """Update blinkers from lights. Enable output when light was seen within the last `blinker_time`
+    iterations"""
+    # TODO: Handle case when switching direction. Now both blinkers can be on at the same time
     self.left_blinker_cnt = blinker_time if left_blinker_lamp else max(self.left_blinker_cnt - 1, 0)
     self.right_blinker_cnt = blinker_time if right_blinker_lamp else max(self.right_blinker_cnt - 1, 0)
     return self.left_blinker_cnt > 0, self.right_blinker_cnt > 0
 
+  def update_blinker_from_stalk(self, blinker_time: int, left_blinker_stalk: bool, right_blinker_stalk: bool):
+    """Update blinkers from stalk position. When stalk is seen the blinker will be on for at least blinker_time,
+    or until the stalk is turned off, whichever is longer. If the opposite stalk direction is seen the blinker
+    is forced to the other side. On a rising edge of the stalk the timeout is reset."""
+
+    if left_blinker_stalk:
+      self.right_blinker_cnt = 0
+      if not self.left_blinker_prev:
+        self.left_blinker_cnt = blinker_time
+
+    if right_blinker_stalk:
+      self.left_blinker_cnt = 0
+      if not self.right_blinker_prev:
+        self.right_blinker_cnt = blinker_time
+
+    self.left_blinker_cnt = max(self.left_blinker_cnt - 1, 0)
+    self.right_blinker_cnt = max(self.right_blinker_cnt - 1, 0)
+
+    self.left_blinker_prev = left_blinker_stalk
+    self.right_blinker_prev = right_blinker_stalk
+
+    return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0)
+
   @staticmethod
   def parse_gear_shifter(gear: str) -> car.CarState.GearShifter:
     d: Dict[str, car.CarState.GearShifter] = {

selfdrive/car/subaru/carstate.py

@@ -33,8 +33,8 @@ class CarState(CarStateBase):
     ret.standstill = ret.vEgoRaw < 0.01
 
     # continuous blinker signals for assisted lane change
-    ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["Dashlights"]["LEFT_BLINKER"],
-                                                            cp.vl["Dashlights"]["RIGHT_BLINKER"])
+    ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
+      50, cp.vl["Dashlights"]["LEFT_BLINKER"], cp.vl["Dashlights"]["RIGHT_BLINKER"])
 
     if self.CP.enableBsm:
       ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1)

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-6a9a7a9c4faf421ab9f7726274a9583f1eb2cb9c
+1ac9a43631a3d6c7316220897ab17f33a66bb05f