mawei/opendbc-meb
1
0
Fork 0
mirror of https://github.com/infiniteCable2/opendbc.git synced 2026-02-18 13:03:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Slimming carState, part 6 (#2514)

Browse Source 
* rm wheel speeds

* this is still such a mess...

* ugh need for the body
This commit is contained in:
Adeeb Shihadeh
2025-07-21 16:40:58 -07:00
committed by GitHub
parent e7e929430b
commit de7d108f44
10 changed files with 27 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
opendbc/car/chrysler/carstate.py
View File

@@ -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),

9
opendbc/car/gm/carstate.py
View File

@@ -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")

12
opendbc/car/honda/carstate.py
View File

@@ -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))

14
opendbc/car/hyundai/carstate.py
View File

@@ -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"]

14
opendbc/car/interfaces.py
View File

@@ -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"""

4
opendbc/car/mazda/carstate.py
View File

@@ -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"]

14
opendbc/car/nissan/carstate.py
View File

@@ -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

4
opendbc/car/subaru/carstate.py
View File

@@ -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

5
opendbc/car/toyota/carstate.py
View File

@@ -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

13
opendbc/car/volkswagen/carstate.py
View File

@@ -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