From 0f8cba29b463315aec17ccd603c313dc7b1c6eab Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 21 Feb 2022 02:34:28 -0600 Subject: [PATCH 1/8] Update carstate.py --- selfdrive/car/toyota/carstate.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index d73460ef32dd2f..e6e6152c05d268 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -19,8 +19,8 @@ def __init__(self, CP): # 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.prev_accurate_steer_angle = 0.0 self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False) self.low_speed_lockout = False @@ -58,9 +58,10 @@ def update(self, cp, cp_cam): 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: + # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use when updated + if abs(torque_sensor_angle_deg - self.prev_accurate_steer_angle) > 1e-3: self.accurate_steer_angle_seen = True + self.prev_accurate_steer_angle = torque_sensor_angle_deg if self.accurate_steer_angle_seen: # Offset seems to be invalid for large steering angles From 29cc3c6fe70bca892903775395b3adc49e825bca Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 21 Feb 2022 02:51:48 -0600 Subject: [PATCH 2/8] comments --- selfdrive/car/toyota/carstate.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index e6e6152c05d268..5c413fb92bcdd3 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -16,9 +16,9 @@ def __init__(self, CP): self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"] self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. - # 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 + # On most cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] + # the signal is zeroed to where the steering angle starts. + # However, on some cars this signal is absolute and requires no offset self.accurate_steer_angle_seen = False self.prev_accurate_steer_angle = 0.0 self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False) @@ -58,7 +58,7 @@ def update(self, cp, cp_cam): 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 when updated + # Wait until an update has been seen to avoid learning an incorrect offset if abs(torque_sensor_angle_deg - self.prev_accurate_steer_angle) > 1e-3: self.accurate_steer_angle_seen = True self.prev_accurate_steer_angle = torque_sensor_angle_deg From 3620cf588415c059820a7b2239b1fd3a2e53f443 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 21 Feb 2022 11:33:47 -0800 Subject: [PATCH 3/8] use initializing --- selfdrive/car/toyota/carstate.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 5c413fb92bcdd3..656ffdd3c46d5b 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -20,7 +20,6 @@ def __init__(self, CP): # the signal is zeroed to where the steering angle starts. # However, on some cars this signal is absolute and requires no offset self.accurate_steer_angle_seen = False - self.prev_accurate_steer_angle = 0.0 self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False) self.low_speed_lockout = False @@ -59,9 +58,8 @@ def update(self, cp, cp_cam): torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # Wait until an update has been seen to avoid learning an incorrect offset - if abs(torque_sensor_angle_deg - self.prev_accurate_steer_angle) > 1e-3: + if not cp.vl["STEER_TORQUE_SENSOR"]["INITIALIZING"]: self.accurate_steer_angle_seen = True - self.prev_accurate_steer_angle = torque_sensor_angle_deg if self.accurate_steer_angle_seen: # Offset seems to be invalid for large steering angles @@ -152,6 +150,7 @@ def get_can_parser(CP): ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"), ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR"), ("STEER_ANGLE", "STEER_TORQUE_SENSOR"), + ("INITIALIZING", "STEER_TORQUE_SENSOR"), ("TURN_SIGNALS", "BLINKERS_STATE"), ("LKA_STATE", "EPS_STATUS"), ("AUTO_HIGH_BEAM", "LIGHT_STALK"), From bb7aadfd074b780bb00adacc8d208bbd9fd67d1a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 21 Feb 2022 11:34:29 -0800 Subject: [PATCH 4/8] bump opendbc --- opendbc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc b/opendbc index 46a942d6790531..3bddb8403b7684 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 46a942d6790531cf5b94b14266140e43afcfda3e +Subproject commit 3bddb8403b7684c9ca2f8396d2baf5cc75cc1c1b From d86ec2bdbbdf869a857758bfe5a99f4707386659 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 21 Feb 2022 11:37:28 -0800 Subject: [PATCH 5/8] bool --- selfdrive/car/toyota/carstate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 656ffdd3c46d5b..14f8319207ecb2 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -58,7 +58,7 @@ def update(self, cp, cp_cam): torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # Wait until an update has been seen to avoid learning an incorrect offset - if not cp.vl["STEER_TORQUE_SENSOR"]["INITIALIZING"]: + if not bool(cp.vl["STEER_TORQUE_SENSOR"]["INITIALIZING"]): self.accurate_steer_angle_seen = True if self.accurate_steer_angle_seen: From 586b0fef30d4c10fce987c33db2338cb24de9412 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 22 Feb 2022 16:41:59 -0800 Subject: [PATCH 6/8] check both --- selfdrive/car/toyota/carstate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 14f8319207ecb2..a27224e549cf5d 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -58,7 +58,7 @@ def update(self, cp, cp_cam): torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # Wait until an update has been seen to avoid learning an incorrect offset - if not bool(cp.vl["STEER_TORQUE_SENSOR"]["INITIALIZING"]): + if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["INITIALIZING"]): self.accurate_steer_angle_seen = True if self.accurate_steer_angle_seen: From fbe06455acdd21f7fff78e599b070113ee50619e Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 22 Feb 2022 16:53:37 -0800 Subject: [PATCH 7/8] bump opendbc --- opendbc | 2 +- selfdrive/car/toyota/carstate.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/opendbc b/opendbc index 3bddb8403b7684..f475c355a65eb8 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 3bddb8403b7684c9ca2f8396d2baf5cc75cc1c1b +Subproject commit f475c355a65eb8050730fe8a1af5ac4b319e1d7f diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index a27224e549cf5d..40ac45f7e4b8f4 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -58,7 +58,7 @@ def update(self, cp, cp_cam): torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # Wait until an update has been seen to avoid learning an incorrect offset - if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["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: @@ -150,7 +150,7 @@ def get_can_parser(CP): ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"), ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR"), ("STEER_ANGLE", "STEER_TORQUE_SENSOR"), - ("INITIALIZING", "STEER_TORQUE_SENSOR"), + ("STEER_ANGLE_INITIALIZING", "STEER_TORQUE_SENSOR"), ("TURN_SIGNALS", "BLINKERS_STATE"), ("LKA_STATE", "EPS_STATUS"), ("AUTO_HIGH_BEAM", "LIGHT_STALK"), From 6ebf18eca97218f5c6c15b317087021bd31553c0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 26 Feb 2022 00:53:42 -0800 Subject: [PATCH 8/8] and on TSS-P the initializing bit isn't set --- selfdrive/car/toyota/carstate.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 40ac45f7e4b8f4..d48a21b51b4510 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -16,9 +16,9 @@ def __init__(self, CP): self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"] self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. - # On most cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] - # the signal is zeroed to where the steering angle starts. - # However, on some cars this signal is absolute and requires no offset + # 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.accurate_steer_angle_seen = False self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False) @@ -57,7 +57,7 @@ def update(self, cp, cp_cam): 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"] - # Wait until an update has been seen to avoid learning an incorrect offset + # 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