From de7d108f443e91b7bf040e40ebf22fb4d86b8025 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 21 Jul 2025 16:40:58 -0700 Subject: [PATCH] Slimming `carState`, part 6 (#2514) * rm wheel speeds * this is still such a mess... * ugh need for the body --- opendbc/car/chrysler/carstate.py | 9 --------- opendbc/car/gm/carstate.py | 9 +++------ opendbc/car/honda/carstate.py | 12 +++++------- opendbc/car/hyundai/carstate.py | 14 +++++--------- opendbc/car/interfaces.py | 14 ++++---------- opendbc/car/mazda/carstate.py | 4 +--- opendbc/car/nissan/carstate.py | 14 ++++++-------- opendbc/car/subaru/carstate.py | 4 +--- opendbc/car/toyota/carstate.py | 5 +---- opendbc/car/volkswagen/carstate.py | 13 +------------ 10 files changed, 27 insertions(+), 71 deletions(-) diff --git a/opendbc/car/chrysler/carstate.py b/opendbc/car/chrysler/carstate.py index 65d7f7ae..51f54dea 100644 --- a/opendbc/car/chrysler/carstate.py +++ b/opendbc/car/chrysler/carstate.py @@ -58,13 +58,6 @@ class CarState(CarStateBase): ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["GEAR"]["PRNDL"], None)) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = not ret.vEgoRaw > 0.001 - ret.wheelSpeeds = self.get_wheel_speeds( - cp.vl["ESP_6"]["WHEEL_SPEED_FL"], - cp.vl["ESP_6"]["WHEEL_SPEED_FR"], - cp.vl["ESP_6"]["WHEEL_SPEED_RL"], - cp.vl["ESP_6"]["WHEEL_SPEED_RR"], - unit=1, - ) # button presses ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1, @@ -119,10 +112,8 @@ class CarState(CarStateBase): @staticmethod def get_can_parsers(CP): pt_messages = [ - # sig_address, frequency ("ESP_1", 50), ("EPS_2", 100), - ("ESP_6", 50), ("STEERING", 100), ("ECM_5", 50), ("CRUISE_BUTTONS", 50), diff --git a/opendbc/car/gm/carstate.py b/opendbc/car/gm/carstate.py index 21cf069a..1750f06f 100644 --- a/opendbc/car/gm/carstate.py +++ b/opendbc/car/gm/carstate.py @@ -1,5 +1,4 @@ import copy -import numpy as np from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from opendbc.car import Bus, create_button_events, structs @@ -68,16 +67,14 @@ class CarState(CarStateBase): # An Equinox has been seen with an unsupported status (3), so only check if either wheel is in reverse (2) left_whl_sign = -1 if pt_cp.vl["EBCMWheelSpdRear"]["RLWheelDir"] == 2 else 1 right_whl_sign = -1 if pt_cp.vl["EBCMWheelSpdRear"]["RRWheelDir"] == 2 else 1 - ret.wheelSpeeds = self.get_wheel_speeds( + self.parse_wheel_speeds(ret, left_whl_sign * pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"], right_whl_sign * pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"], left_whl_sign * pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"], right_whl_sign * pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"], ) - ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])) - ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - # sample rear wheel speeds, standstill=True if ECM allows engagement with brake - ret.standstill = abs(ret.wheelSpeeds.rl) <= STANDSTILL_THRESHOLD and abs(ret.wheelSpeeds.rr) <= STANDSTILL_THRESHOLD + # standstill=True if ECM allows engagement with brake + ret.standstill = abs(ret.vEgoRaw) <= STANDSTILL_THRESHOLD if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1: ret.gearShifter = self.parse_gear_shifter("T") diff --git a/opendbc/car/honda/carstate.py b/opendbc/car/honda/carstate.py index eac41cf9..7ec2cf5a 100644 --- a/opendbc/car/honda/carstate.py +++ b/opendbc/car/honda/carstate.py @@ -150,13 +150,11 @@ class CarState(CarStateBase): ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0 - ret.wheelSpeeds = self.get_wheel_speeds( - cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"], - cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"], - cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"], - cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"], - ) - v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.0 + fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"] + fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"] + rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"] + rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"] + v_wheel = (fl + fr + rl + rr) / 4.0 # blend in transmission speed at low speed, since it has more low speed accuracy v_weight = float(np.interp(v_wheel, v_weight_bp, v_weight_v)) diff --git a/opendbc/car/hyundai/carstate.py b/opendbc/car/hyundai/carstate.py index 191d6258..9a4ef16c 100644 --- a/opendbc/car/hyundai/carstate.py +++ b/opendbc/car/hyundai/carstate.py @@ -87,15 +87,13 @@ class CarState(CarStateBase): ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0 - ret.wheelSpeeds = self.get_wheel_speeds( + self.parse_wheel_speeds(ret, cp.vl["WHL_SPD11"]["WHL_SPD_FL"], cp.vl["WHL_SPD11"]["WHL_SPD_FR"], cp.vl["WHL_SPD11"]["WHL_SPD_RL"], cp.vl["WHL_SPD11"]["WHL_SPD_RR"], ) - ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. - ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD + ret.standstill = cp.vl["WHL_SPD11"]["WHL_SPD_FL"] <= STANDSTILL_THRESHOLD and cp.vl["WHL_SPD11"]["WHL_SPD_RR"] <= STANDSTILL_THRESHOLD self.cluster_speed_counter += 1 if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE: @@ -235,16 +233,14 @@ class CarState(CarStateBase): ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear)) # TODO: figure out positions - ret.wheelSpeeds = self.get_wheel_speeds( + self.parse_wheel_speeds(ret, cp.vl["WHEEL_SPEEDS"]["WHL_SpdFLVal"], cp.vl["WHEEL_SPEEDS"]["WHL_SpdFRVal"], cp.vl["WHEEL_SPEEDS"]["WHL_SpdRLVal"], cp.vl["WHEEL_SPEEDS"]["WHL_SpdRRVal"], ) - ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. - ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.fr <= STANDSTILL_THRESHOLD and \ - ret.wheelSpeeds.rl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD + ret.standstill = cp.vl["WHEEL_SPEEDS"]["WHL_SpdFLVal"] <= STANDSTILL_THRESHOLD and cp.vl["WHEEL_SPEEDS"]["WHL_SpdFRVal"] <= STANDSTILL_THRESHOLD and \ + cp.vl["WHEEL_SPEEDS"]["WHL_SpdRLVal"] <= STANDSTILL_THRESHOLD and cp.vl["WHEEL_SPEEDS"]["WHL_SpdRRVal"] <= STANDSTILL_THRESHOLD ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"] ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] diff --git a/opendbc/car/interfaces.py b/opendbc/car/interfaces.py index 50f903ce..2238d5f7 100644 --- a/opendbc/car/interfaces.py +++ b/opendbc/car/interfaces.py @@ -304,6 +304,10 @@ class CarStateBase(ABC): def update(self, can_parsers) -> structs.CarState: pass + def parse_wheel_speeds(self, cs, fl, fr, rl, rr, unit=CV.KPH_TO_MS): + cs.vEgoRaw = float(np.mean([fl, fr, rl, rr]) * unit * self.CP.wheelSpeedFactor) + cs.vEgo, cs.aEgo = self.update_speed_kf(cs.vEgoRaw) + def update_speed_kf(self, v_ego_raw): if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego_kf.set_x([[v_ego_raw], [0.0]]) @@ -311,16 +315,6 @@ class CarStateBase(ABC): v_ego_x = self.v_ego_kf.update(v_ego_raw) return float(v_ego_x[0]), float(v_ego_x[1]) - def get_wheel_speeds(self, fl, fr, rl, rr, unit=CV.KPH_TO_MS): - factor = unit * self.CP.wheelSpeedFactor - - wheelSpeeds = structs.CarState.WheelSpeeds() - wheelSpeeds.fl = fl * factor - wheelSpeeds.fr = fr * factor - wheelSpeeds.rl = rl * factor - wheelSpeeds.rr = rr * factor - return wheelSpeeds - def update_blinker_from_lamp(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool): """Update blinkers from lights. Enable output when light was seen within the last `blinker_time` iterations""" diff --git a/opendbc/car/mazda/carstate.py b/opendbc/car/mazda/carstate.py index c565091d..8c4643bc 100644 --- a/opendbc/car/mazda/carstate.py +++ b/opendbc/car/mazda/carstate.py @@ -30,14 +30,12 @@ class CarState(CarStateBase): prev_distance_button = self.distance_button self.distance_button = cp.vl["CRZ_BTNS"]["DISTANCE_LESS"] - ret.wheelSpeeds = self.get_wheel_speeds( + self.parse_wheel_speeds(ret, cp.vl["WHEEL_SPEEDS"]["FL"], cp.vl["WHEEL_SPEEDS"]["FR"], cp.vl["WHEEL_SPEEDS"]["RL"], cp.vl["WHEEL_SPEEDS"]["RR"], ) - ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. - ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) # Match panda speed reading speed_kph = cp.vl["ENGINE_DATA"]["SPEED"] diff --git a/opendbc/car/nissan/carstate.py b/opendbc/car/nissan/carstate.py index edbd68ef..2888946c 100644 --- a/opendbc/car/nissan/carstate.py +++ b/opendbc/car/nissan/carstate.py @@ -47,16 +47,14 @@ class CarState(CarStateBase): elif self.CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): ret.brakePressed = bool(cp.vl["CRUISE_THROTTLE"]["USER_BRAKE_PRESSED"]) - ret.wheelSpeeds = self.get_wheel_speeds( - cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"], - cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"], - cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"], - cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"], - ) + fl = cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"] + fr = cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"] + rl = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"] + rr = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"] # safety uses the rear wheel speeds for the speed measurement and angle limiting - ret.vEgoRaw = (ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 2.0 + ret.vEgoRaw = (rl + rr) / 2.0 - v_ego_raw_full = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.0 + v_ego_raw_full = (fl + fr + rl + rr) / 4.0 ret.vEgo, ret.aEgo = self.update_speed_kf(v_ego_raw_full) ret.standstill = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"] == 0.0 and cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"] == 0.0 diff --git a/opendbc/car/subaru/carstate.py b/opendbc/car/subaru/carstate.py index 5e4343c2..ab30d1fe 100644 --- a/opendbc/car/subaru/carstate.py +++ b/opendbc/car/subaru/carstate.py @@ -43,14 +43,12 @@ class CarState(CarStateBase): ret.accFaulted = eyesight_fault cp_wheels = cp_alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp - ret.wheelSpeeds = self.get_wheel_speeds( + self.parse_wheel_speeds(ret, cp_wheels.vl["Wheel_Speeds"]["FL"], cp_wheels.vl["Wheel_Speeds"]["FR"], cp_wheels.vl["Wheel_Speeds"]["RL"], cp_wheels.vl["Wheel_Speeds"]["RR"], ) - ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. - ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw == 0 # continuous blinker signals for assisted lane change diff --git a/opendbc/car/toyota/carstate.py b/opendbc/car/toyota/carstate.py index 35c58c2a..c7c1e09f 100644 --- a/opendbc/car/toyota/carstate.py +++ b/opendbc/car/toyota/carstate.py @@ -1,5 +1,4 @@ import copy -import numpy as np from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser @@ -82,14 +81,12 @@ class CarState(CarStateBase): if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5) - ret.wheelSpeeds = self.get_wheel_speeds( + self.parse_wheel_speeds(ret, cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"], cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"], cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"], cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"], ) - ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])) - ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.vEgoCluster = ret.vEgo * 1.015 # minimum of all the cars ret.standstill = abs(ret.vEgoRaw) < 1e-3 diff --git a/opendbc/car/volkswagen/carstate.py b/opendbc/car/volkswagen/carstate.py index 3940de69..1e86cb2a 100644 --- a/opendbc/car/volkswagen/carstate.py +++ b/opendbc/car/volkswagen/carstate.py @@ -1,4 +1,3 @@ -import numpy as np from opendbc.can.parser import CANParser from opendbc.car import Bus, structs from opendbc.car.interfaces import CarStateBase @@ -66,7 +65,7 @@ class CarState(CarStateBase): # MQB-specific self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) # Analog vs digital instrument cluster - ret.wheelSpeeds = self.get_wheel_speeds( + self.parse_wheel_speeds(ret, pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"], pt_cp.vl["ESP_19"]["ESP_VR_Radgeschw_02"], pt_cp.vl["ESP_19"]["ESP_HL_Radgeschw_02"], @@ -114,9 +113,6 @@ class CarState(CarStateBase): ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"]) # Shared logic - - ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])) - ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.vEgoCluster = pt_cp.vl["Kombi_01"]["KBI_angez_Geschw"] * CV.KPH_TO_MS ret.steeringAngleDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradwinkel"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradwinkel"])] @@ -149,13 +145,6 @@ class CarState(CarStateBase): def update_pq(self, pt_cp, cam_cp, ext_cp) -> structs.CarState: ret = structs.CarState() - # Update vehicle speed and acceleration from ABS wheel speeds. - ret.wheelSpeeds = self.get_wheel_speeds( - pt_cp.vl["Bremse_3"]["Radgeschw__VL_4_1"], - pt_cp.vl["Bremse_3"]["Radgeschw__VR_4_1"], - pt_cp.vl["Bremse_3"]["Radgeschw__HL_4_1"], - pt_cp.vl["Bremse_3"]["Radgeschw__HR_4_1"], - ) # vEgo obtained from Bremse_1 vehicle speed rather than Bremse_3 wheel speeds because Bremse_3 isn't present on NSF ret.vEgoRaw = pt_cp.vl["Bremse_1"]["Geschwindigkeit_neu__Bremse_1_"] * CV.KPH_TO_MS