mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-10-01 03:43:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

add CarParams.wheelSpeedFactor (#23079)

Browse Source 
* add CarParams.wheelSpeedFactor

* Fudge speed for Lexus RX

* should have been the TSS2 model

* bump cereal

* refactor into function

* update refs
This commit is contained in:
Willem Melching
2021-12-03 14:57:53 +01:00
committed by GitHub
parent 5b0e10ea33
commit e6180738fd
16 changed files with 85 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal
+1 -1
Submodule cereal updated: 596d34e77a...c4a1c9fa00
selfdrive/car/chrysler/carstate.py
+7 -4
View File
@@ -31,10 +31,13 @@ class CarState(CarStateBase):
ret.espDisabled = (cp.vl["TRACTION_BUTTON"]["TRACTION_OFF"] == 1)
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"]
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"]
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"]
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"]
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"],
unit=1,
)
ret.vEgoRaw = (cp.vl["SPEED_1"]["SPEED_LEFT"] + cp.vl["SPEED_1"]["SPEED_RIGHT"]) / 2.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
selfdrive/car/ford/carstate.py
+8 -4
View File
@@ -10,10 +10,14 @@ WHEEL_RADIUS = 0.33
class CarState(CarStateBase):
def update(self, cp):
ret = car.CarState.new_message()
ret.wheelSpeeds.rr = cp.vl["WheelSpeed_CG1"]["WhlRr_W_Meas"] * WHEEL_RADIUS
ret.wheelSpeeds.rl = cp.vl["WheelSpeed_CG1"]["WhlRl_W_Meas"] * WHEEL_RADIUS
ret.wheelSpeeds.fr = cp.vl["WheelSpeed_CG1"]["WhlFr_W_Meas"] * WHEEL_RADIUS
ret.wheelSpeeds.fl = cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"] * WHEEL_RADIUS
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"],
cp.vl["WheelSpeed_CG1"]["WhlFr_W_Meas"],
cp.vl["WheelSpeed_CG1"]["WhlRl_W_Meas"],
cp.vl["WheelSpeed_CG1"]["WhlRr_W_Meas"],
unit=WHEEL_RADIUS,
)
ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
selfdrive/car/gm/carstate.py
+6 -5
View File
@@ -1,6 +1,5 @@
from cereal import car
from common.numpy_fast import mean
from selfdrive.config import Conversions as CV
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase
@@ -21,10 +20,12 @@ class CarState(CarStateBase):
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"] * CV.KPH_TO_MS
ret.wheelSpeeds = self.get_wheel_speeds(
pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"],
pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"],
pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"],
pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"],
)
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
selfdrive/car/honda/carstate.py
+9 -8
View File
@@ -5,7 +5,7 @@ from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
TransmissionType = car.CarParams.TransmissionType
@@ -213,16 +213,17 @@ class CarState(CarStateBase):
self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"]
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
speed_factor = SPEED_FACTOR.get(self.CP.carFingerprint, 1.)
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"] * CV.KPH_TO_MS * speed_factor
v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr)/4.
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
# blend in transmission speed at low speed, since it has more low speed accuracy
v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor + v_weight * v_wheel
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"]
selfdrive/car/honda/interface.py
+4
View File
@@ -143,6 +143,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.CRV_5G:
stop_and_go = True
@@ -160,6 +161,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
tire_stiffness_factor = 0.677
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.CRV_HYBRID:
stop_and_go = True
@@ -170,6 +172,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.677
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.FIT:
stop_and_go = False
@@ -201,6 +204,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.ACURA_RDX:
stop_and_go = False
selfdrive/car/honda/values.py
-10
View File
@@ -1428,16 +1428,6 @@ STEER_THRESHOLD = {
CAR.CRV_EU: 400,
}
# TODO: is this real?
SPEED_FACTOR = {
# default is 1, overrides go here
CAR.CRV: 1.025,
CAR.CRV_5G: 1.025,
CAR.CRV_EU: 1.025,
CAR.CRV_HYBRID: 1.025,
CAR.HRV: 1.025,
}
HONDA_NIDEC_ALT_PCM_ACCEL = set([CAR.ODYSSEY])
HONDA_NIDEC_ALT_SCM_MESSAGES = set([CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN,
CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE])
selfdrive/car/hyundai/carstate.py
+6 -4
View File
@@ -28,10 +28,12 @@ class CarState(CarStateBase):
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]["WHL_SPD_FL"] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]["WHL_SPD_FR"] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]["WHL_SPD_RL"] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]["WHL_SPD_RR"] * CV.KPH_TO_MS
ret.wheelSpeeds = self.get_wheel_speeds(
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)
selfdrive/car/interfaces.py
+11
View File
@@ -78,6 +78,7 @@ class CarInterfaceBase():
ret.steerMaxBP = [0.]
ret.steerMaxV = [1.]
ret.minSteerSpeed = 0.
ret.wheelSpeedFactor = 1.0
ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
@@ -210,6 +211,16 @@ class CarStateBase:
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 = car.CarState.WheelSpeeds.new_message()
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"""
selfdrive/car/mazda/carstate.py
+6 -4
View File
@@ -20,10 +20,12 @@ class CarState(CarStateBase):
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["FL"] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["FR"] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["RL"] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["RR"] * CV.KPH_TO_MS
ret.wheelSpeeds = self.get_wheel_speeds(
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)
selfdrive/car/nissan/carstate.py
+6 -5
View File
@@ -35,11 +35,12 @@ class CarState(CarStateBase):
elif self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]:
ret.brakePressed = bool(cp.vl["CRUISE_THROTTLE"]["USER_BRAKE_PRESSED"])
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"] * CV.KPH_TO_MS
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"],
)
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)
selfdrive/car/subaru/carstate.py
+6 -4
View File
@@ -23,10 +23,12 @@ class CarState(CarStateBase):
else:
ret.brakePressed = cp.vl["Brake_Status"]["Brake"] == 1
ret.wheelSpeeds.fl = cp.vl["Wheel_Speeds"]["FL"] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["Wheel_Speeds"]["FR"] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["Wheel_Speeds"]["RL"] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["Wheel_Speeds"]["RR"] * CV.KPH_TO_MS
ret.wheelSpeeds = self.get_wheel_speeds(
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.
# Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
selfdrive/car/toyota/carstate.py
+6 -4
View File
@@ -41,10 +41,12 @@ class CarState(CarStateBase):
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"]
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"] * CV.KPH_TO_MS
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"],
)
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
selfdrive/car/toyota/interface.py
+2 -1
View File
@@ -65,7 +65,6 @@ class CarInterface(CarInterfaceBase):
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_B)
elif candidate == CAR.LEXUS_RXH:
stop_and_go = True
ret.wheelbase = 2.79
@@ -81,6 +80,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.5533 # not optimized yet
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
ret.wheelSpeedFactor = 1.035
elif candidate == CAR.LEXUS_RXH_TSS2:
stop_and_go = True
@@ -89,6 +89,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4481.0 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
set_lat_tune(ret.lateralTuning, LatTunes.PID_E)
ret.wheelSpeedFactor = 1.035
elif candidate in [CAR.CHR, CAR.CHRH]:
stop_and_go = True
selfdrive/car/volkswagen/carstate.py
+6 -4
View File
@@ -20,10 +20,12 @@ class CarState(CarStateBase):
def update(self, pt_cp, cam_cp, ext_cp, trans_type):
ret = car.CarState.new_message()
# Update vehicle speed and acceleration from ABS wheel speeds.
ret.wheelSpeeds.fl = pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = pt_cp.vl["ESP_19"]["ESP_VR_Radgeschw_02"] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = pt_cp.vl["ESP_19"]["ESP_HL_Radgeschw_02"] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = pt_cp.vl["ESP_19"]["ESP_HR_Radgeschw_02"] * CV.KPH_TO_MS
ret.wheelSpeeds = self.get_wheel_speeds(
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"],
pt_cp.vl["ESP_19"]["ESP_HR_Radgeschw_02"],
)
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)
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
6ab94cfcfe183a09e75e9f8b67a7ee2a2d3630df
4d55b3974e099f2bd6e77ea12e0e0830af051041