commaai
/
opendbc
mirror of https://github.com/maweiyuweiwei/opendbc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

stash

This commit is contained in:
Shane Smiskol 2024-11-13 13:50:53 -08:00
parent 78b78eb257
commit 8bbd7ffd4c
2 changed files with 25 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
opendbc/car/toyota/carcontroller.py
View File

@ -1,6 +1,6 @@
import math
from opendbc.car import carlog, apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \
make_tester_present_msg, structs, ACCELERATION_DUE_TO_GRAVITY, DT_CTRL
make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY, DT_CTRL
from opendbc.car.can_definitions import CanData
from opendbc.car.common.filter_simple import FirstOrderFilter
from opendbc.car.common.numpy_fast import clip
@ -44,8 +44,13 @@ class CarController(CarControllerBase):
self.steer_rate_counter = 0
self.distance_button = 0
self.debug = 0
self.debug2 = 0
self.debug3 = 0
self.pitch = FirstOrderFilter(0, 2, DT_CTRL)
self.pcm_accel_compensation = FirstOrderFilter(0, 0.5, DT_CTRL * 3)
self.pcm_accel_compensation = 0.0
self.pcm_accel_compensation2 = FirstOrderFilter(0, 0.5, DT_CTRL * 3)
self.permit_braking = True
self.packer = CANPacker(dbc_name)
@ -179,11 +184,13 @@ class CarController(CarControllerBase):
pcm_accel_cmd = min(actuators.accel, self.accel + ACCEL_WINDUP_LIMIT / 3) if CC.longActive else 0.0
# calculate amount of acceleration PCM should apply to reach target, given pitch
accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY
# accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY
# accel_due_to_pitch = math.sin(math.radians(CS.aslp)) * ACCELERATION_DUE_TO_GRAVITY
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch
# For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking
if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill:
if CC.longActive and not CS.out.cruiseState.standstill:
# let PCM handle stopping for now
pcm_accel_compensation = 0.0
if not stopping:
@ -193,10 +200,16 @@ class CarController(CarControllerBase):
pcm_accel_compensation = clip(pcm_accel_compensation, pcm_accel_cmd - self.params.ACCEL_MAX,
pcm_accel_cmd - self.params.ACCEL_MIN)
pcm_accel_cmd = pcm_accel_cmd - self.pcm_accel_compensation.update(pcm_accel_compensation)
self.pcm_accel_compensation = rate_limit(pcm_accel_compensation, self.pcm_accel_compensation, -0.03, 0.03)
self.debug = self.pcm_accel_compensation
pcm_accel_cmd = pcm_accel_cmd - self.pcm_accel_compensation
# pcm_accel_cmd = pcm_accel_cmd - self.pcm_accel_compensation2.update(pcm_accel_compensation)
# self.debug2 = self.pcm_accel_compensation2.x
else:
self.pcm_accel_compensation.x = 0.0
self.pcm_accel_compensation2.x = 0.0
self.pcm_accel_compensation = 0.0
self.permit_braking = True
# Along with rate limiting positive jerk above, this greatly improves gas response time
@ -258,5 +271,9 @@ class CarController(CarControllerBase):
new_actuators.steeringAngleDeg = self.last_angle
new_actuators.accel = self.accel
new_actuators.debug = self.debug
new_actuators.debug2 = self.debug2
new_actuators.debug3 = self.debug3
self.frame += 1
return new_actuators, can_sends

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

@ -61,6 +61,7 @@ class CarState(CarStateBase):
# CLUTCH->ACCEL_NET is only accurate for gas, PCM_CRUISE->ACCEL_NET is only accurate for brake
# These signals only have meaning when ACC is active
if "CLUTCH" in cp.vl:
self.aslp = cp.vl["VSC1S07"]["ASLP"]
self.pcm_accel_net = max(cp.vl["CLUTCH"]["ACCEL_NET"], 0.0)
# Sometimes ACC_BRAKING can be 1 while showing we're applying gas already
@ -234,6 +235,7 @@ class CarState(CarStateBase):
if CP.carFingerprint in (TSS2_CAR - SECOC_CAR - {CAR.LEXUS_NX_TSS2, CAR.TOYOTA_ALPHARD_TSS2, CAR.LEXUS_IS_TSS2}):
messages.append(("CLUTCH", 15))
messages.append(("VSC1S07", 15))
if CP.carFingerprint in UNSUPPORTED_DSU_CAR:
messages.append(("DSU_CRUISE", 5))