diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 6f54aba6ed..3a89d655da 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -74,6 +74,7 @@ class CarState(CarStateBase): ret.standstill = ret.vEgoRaw == 0 ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"] + ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"] torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # On some cars, the angle measurement is non-zero while initializing @@ -81,16 +82,14 @@ class CarState(CarStateBase): self.accurate_steer_angle_seen = True if self.accurate_steer_angle_seen: - # Offset seems to be invalid for large steering angles - if abs(ret.steeringAngleDeg) < 90 and cp.can_valid: + # Offset seems to be invalid for large steering angles and high angle rates + if abs(ret.steeringAngleDeg) < 90 and abs(ret.steeringRateDeg) < 100 and cp.can_valid: self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg) if self.angle_offset.initialized: ret.steeringAngleOffsetDeg = self.angle_offset.x ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x - ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"] - can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1