diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index b4d48be88f..3067c5e29e 100644 --- a/selfdrive/car/honda/carstate.py +++ b/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, diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index ae18dbaa57..9c6b1275ed 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/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 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 4415e9440e..9bceff63f4 100644 --- a/selfdrive/car/interfaces.py +++ b/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] = { diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 888c370eb1..d2fa0c7958 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/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) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index aafe28438d..d43667d632 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -6a9a7a9c4faf421ab9f7726274a9583f1eb2cb9c \ No newline at end of file +1ac9a43631a3d6c7316220897ab17f33a66bb05f \ No newline at end of file