From c15a616ac44e238e6c5866bc57b5c6b524c765d6 Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Wed, 1 Sep 2021 15:00:52 -0700
Subject: [PATCH] Continuously update offset between TSS2 angle sensors
 (#21943)

* Continuously update offset between TSS2 angle sensors

* add comment

* less lines

* only when initialized

* init to None

* update ref
---
 common/filter_simple.py                  |  9 +++++++--
 selfdrive/car/toyota/carstate.py         | 24 +++++++++++++-----------
 selfdrive/test/process_replay/ref_commit |  2 +-
 3 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/common/filter_simple.py b/common/filter_simple.py
index f44bc9eae..0ec7a5156 100644
--- a/common/filter_simple.py
+++ b/common/filter_simple.py
@@ -1,13 +1,18 @@
 class FirstOrderFilter:
   # first order filter
-  def __init__(self, x0, rc, dt):
+  def __init__(self, x0, rc, dt, initialized=True):
     self.x = x0
     self.dt = dt
     self.update_alpha(rc)
+    self.initialized = initialized
 
   def update_alpha(self, rc):
     self.alpha = self.dt / (rc + self.dt)
 
   def update(self, x):
-    self.x = (1. - self.alpha) * self.x + self.alpha * x
+    if self.initialized:
+      self.x = (1. - self.alpha) * self.x + self.alpha * x
+    else:
+      self.initialized = True
+      self.x = x
     return self.x
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index a9d00b3ca..a7790bdf9 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -1,5 +1,7 @@
 from cereal import car
 from common.numpy_fast import mean
+from common.filter_simple import FirstOrderFilter
+from common.realtime import DT_CTRL
 from opendbc.can.can_define import CANDefine
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
@@ -18,7 +20,7 @@ class CarState(CarStateBase):
     # 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 = 0.
+    self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False)
 
     self.low_speed_lockout = False
     self.acc_type = 1
@@ -47,21 +49,21 @@ class CarState(CarStateBase):
 
     ret.standstill = ret.vEgoRaw < 0.001
 
+    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(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]) > 1e-3:
+    if abs(torque_sensor_angle_deg) > 1e-3:
       self.accurate_steer_angle_seen = True
 
     if self.accurate_steer_angle_seen:
-      ret.steeringAngleDeg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] - self.angle_offset
-      ret.steeringAngleOffsetDeg = self.angle_offset
+      # Offset seems to be invalid for large steering angles
+      if abs(ret.steeringAngleDeg) < 90:
+        self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg)
 
-      if self.needs_angle_offset:
-        angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
-        if abs(angle_wheel) > 1e-3:
-          self.needs_angle_offset = False
-          self.angle_offset = ret.steeringAngleDeg - angle_wheel
-    else:
-      ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
+      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"]
 
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index bc4db5fb6..c832d7012 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-f3e220d82db802c653777634228ce57576ac7aad
\ No newline at end of file
+3e8037d39f40c59a2b9e86539e0952645b2cb3ea
\ No newline at end of file