From 8531345bf9e71426e57ae7a57d0eccbcdde75c8d Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 14 Oct 2020 18:27:27 -0700 Subject: [PATCH] HKG fixups (#2342) * fix hyundai blinker state * consistency with update_speed * update refs * make hyundai resume reliable * update refs * 20 works * don't need that Co-authored-by: Greg Hogan old-commit-hash: 725e3b78ea1baaefa6795116c582a63c7a61686b --- selfdrive/car/hyundai/carcontroller.py | 6 +++--- selfdrive/car/hyundai/carstate.py | 6 ++---- selfdrive/car/interfaces.py | 10 +++++++++- selfdrive/car/subaru/carstate.py | 8 ++------ selfdrive/test/process_replay/ref_commit | 2 +- 5 files changed, 17 insertions(+), 15 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index c47545ef2..736435615 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -77,9 +77,9 @@ class CarController(): if pcm_cancel_cmd: can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL)) elif CS.out.cruiseState.standstill: - # send resume at a max freq of 5Hz - if (frame - self.last_resume_frame)*DT_CTRL > 0.2: - can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)) + # send resume at a max freq of 10Hz + if (frame - self.last_resume_frame)*DT_CTRL > 0.1: + can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 20) self.last_resume_frame = frame # 20 Hz LFA MFA message diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index fd53ffc8a..b799a13fe 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -29,8 +29,8 @@ class CarState(CarStateBase): ret.steeringAngle = cp.vl["SAS11"]['SAS_Angle'] ret.steeringRate = cp.vl["SAS11"]['SAS_Speed'] ret.yawRate = cp.vl["ESP12"]['YAW_RATE'] - ret.leftBlinker = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] != 0 - ret.rightBlinker = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] != 0 + ret.leftBlinker, ret.rightBlinker = self.update_blinker(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 @@ -151,9 +151,7 @@ class CarState(CarStateBase): ("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door ("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door ("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door - ("CF_Gway_TSigLHSw", "CGW1", 0), ("CF_Gway_TurnSigLh", "CGW1", 0), - ("CF_Gway_TSigRHSw", "CGW1", 0), ("CF_Gway_TurnSigRh", "CGW1", 0), ("CF_Gway_ParkBrakeSw", "CGW1", 0), diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index bd5c8eb4b..a4dcb905c 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -149,9 +149,12 @@ class CarStateBase: def __init__(self, CP): self.CP = CP self.car_fingerprint = CP.carFingerprint - self.cruise_buttons = 0 self.out = car.CarState.new_message() + self.cruise_buttons = 0 + self.left_blinker_cnt = 0 + self.right_blinker_cnt = 0 + # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], @@ -166,6 +169,11 @@ 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): + 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 + @staticmethod def parse_gear_shifter(gear): return {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral, diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 5b5e23599..3be516ed3 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -10,8 +10,6 @@ from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD, CAR, PREGLOBAL_CAR class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) - self.left_blinker_cnt = 0 - self.right_blinker_cnt = 0 can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = can_define.dv["Transmission"]['Gear'] @@ -36,10 +34,8 @@ class CarState(CarStateBase): ret.standstill = ret.vEgoRaw < 0.01 # continuous blinker signals for assisted lane change - self.left_blinker_cnt = 50 if cp.vl["Dashlights"]['LEFT_BLINKER'] else max(self.left_blinker_cnt - 1, 0) - ret.leftBlinker = self.left_blinker_cnt > 0 - self.right_blinker_cnt = 50 if cp.vl["Dashlights"]['RIGHT_BLINKER'] else max(self.right_blinker_cnt - 1, 0) - ret.rightBlinker = self.right_blinker_cnt > 0 + ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["Dashlights"]['LEFT_BLINKER'], + cp.vl["Dashlights"]['RIGHT_BLINKER']) ret.leftBlindspot = (cp.vl["BSD_RCTA"]['L_ADJACENT'] == 1) or (cp.vl["BSD_RCTA"]['L_APPROACHING'] == 1) ret.rightBlindspot = (cp.vl["BSD_RCTA"]['R_ADJACENT'] == 1) or (cp.vl["BSD_RCTA"]['R_APPROACHING'] == 1) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index af13a26f5..fb04e0953 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -fe7f0cbcead73a8ee5f91507238b933505bb53de \ No newline at end of file +2f3615c105ba553bea23fb349b0666a70f27d862 \ No newline at end of file