diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 5157f52a01..5b5139eecc 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -53,8 +53,15 @@ class CarState(CarStateBase): def update(self, cp, cp_cam): if self.CP.carFingerprint in CANFD_CAR: - return self.update_canfd(cp, cp_cam) + ret = self.update_canfd(cp, cp_cam) + else: + ret = self.update_can(cp, cp_cam) + + ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD + return ret + + def update_can(self, cp, cp_cam): ret = car.CarState.new_message() cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0 @@ -73,7 +80,6 @@ class CarState(CarStateBase): ) ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD self.cluster_speed_counter += 1 if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE: @@ -192,7 +198,6 @@ class CarState(CarStateBase): ) ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"] ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1