Subaru: calculate steering angle rate (#29409)

* calculate rate

* added adeeb's comment

* move to selfdrive/car

* that makes way more sense

* much simpler

* fix rate

* preglobal doesn't have a counter

* update ref

* merge

* update ref after merging

selfdrive/car/__init__.py

@@ -213,3 +213,24 @@ class CanBusBase:
     else:
       num = len(CP.safetyConfigs)
     self.offset = 4 * (num - 1)
+
+
+class CanSignalRateCalculator:
+  """
+  Calculates the instantaneous rate of a CAN signal by using the counter
+  variable and the known frequency of the CAN message that contains it.
+  """
+  def __init__(self, frequency):
+    self.frequency = frequency
+    self.previous_counter = 0
+    self.previous_value = 0
+    self.rate = 0
+
+  def update(self, current_value, current_counter):
+    if current_counter != self.previous_counter:
+      self.rate = (current_value - self.previous_value) * self.frequency
+
+    self.previous_counter = current_counter
+    self.previous_value = current_value
+
+    return self.rate

selfdrive/car/subaru/carstate.py

@@ -5,6 +5,7 @@ from common.conversions import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, SubaruFlags
+from selfdrive.car import CanSignalRateCalculator
 
 
 class CarState(CarStateBase):
@@ -13,6 +14,8 @@ class CarState(CarStateBase):
     can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
     self.shifter_values = can_define.dv["Transmission"]["Gear"]
 
+    self.angle_rate_calulator = CanSignalRateCalculator(50)
+
   def update(self, cp, cp_cam, cp_body):
     ret = car.CarState.new_message()
 
@@ -47,6 +50,11 @@ class CarState(CarStateBase):
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
 
     ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
+
+    if self.car_fingerprint not in PREGLOBAL_CARS:
+      # ideally we get this from the car, but unclear if it exists. diagnostic software doesn't even have it
+      ret.steeringRateDeg = self.angle_rate_calulator.update(ret.steeringAngleDeg, cp.vl["Steering_Torque"]["COUNTER"])
+
     ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"]
     ret.steeringTorqueEps = cp.vl["Steering_Torque"]["Steer_Torque_Output"]
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-a2f54aa2f72dd63842ae15b3cf718bf17191599c
+5f0421f0167eaef5aa278e62d91f99e8fa0567d7