From a1875ac6f090f372aa3f348f439f0589be16ca10 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 26 Feb 2022 00:54:35 -0800 Subject: [PATCH] Toyota: fix learning bad offsets for some models (#23810) * Update carstate.py * comments * use initializing * bump opendbc * bool * check both * bump opendbc * and on TSS-P the initializing bit isn't set --- opendbc | 2 +- selfdrive/car/toyota/carstate.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/opendbc b/opendbc index 46a942d6..f475c355 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 46a942d6790531cf5b94b14266140e43afcfda3e +Subproject commit f475c355a65eb8050730fe8a1af5ac4b319e1d7f diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index d73460ef..d48a21b5 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -19,7 +19,6 @@ class CarState(CarStateBase): # On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # the signal is zeroed to where the steering angle is at start. # Need to apply an offset as soon as the steering angle measurements are both received - self.needs_angle_offset = True self.accurate_steer_angle_seen = False self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False) @@ -58,8 +57,8 @@ class CarState(CarStateBase): ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"] torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] - # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero - if abs(torque_sensor_angle_deg) > 1e-3: + # On some cars, the angle measurement is non-zero while 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: @@ -151,6 +150,7 @@ class CarState(CarStateBase): ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"), ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR"), ("STEER_ANGLE", "STEER_TORQUE_SENSOR"), + ("STEER_ANGLE_INITIALIZING", "STEER_TORQUE_SENSOR"), ("TURN_SIGNALS", "BLINKERS_STATE"), ("LKA_STATE", "EPS_STATUS"), ("AUTO_HIGH_BEAM", "LIGHT_STALK"),