diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 0914d6f45d..954b98de14 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -29,7 +29,6 @@ class CarState(CarStateBase): self.low_speed_lockout = False self.acc_type = 1 self.lkas_hud = {} - self.torque_sensor_angle_deg = 0 def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -63,20 +62,20 @@ class CarState(CarStateBase): ret.standstill = ret.vEgoRaw == 0 ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"] - self.torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] + torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # On some cars, the angle measurement is non-zero while initializing - if abs(self.torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE_INITIALIZING"]): + if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE_INITIALIZING"]): 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: - self.angle_offset.update(self.torque_sensor_angle_deg - ret.steeringAngleDeg) + self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg) if self.angle_offset.initialized: ret.steeringAngleOffsetDeg = self.angle_offset.x - ret.steeringAngleDeg = self.torque_sensor_angle_deg - self.angle_offset.x + ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]