diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index ab7cd545a3..48ac84dd13 100644 --- a/selfdrive/car/__init__.py +++ b/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 \ No newline at end of file diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index da8f88ed02..6e0bef2068 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/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"] diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index cca1473ae6..701a0332e8 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -a2f54aa2f72dd63842ae15b3cf718bf17191599c \ No newline at end of file +5f0421f0167eaef5aa278e62d91f99e8fa0567d7 \ No newline at end of file