mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-07 00:05:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

Lexus ES TSS2: improve start from stop for ICE variants #1248

Browse Source
This commit is contained in:
rav4kumar
2024-09-17 21:15:29 -07:00
parent 21d4bd4779
commit f6831f420e
2 changed files with 26 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/toyota/carcontroller.py
+14 -25
View File
@@ -5,7 +5,7 @@ from openpilot.selfdrive.controls.lib.pid import PIDController
from common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip, interp
from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg, make_tester_present_msg, \
create_gas_interceptor_command
create_gas_interceptor_command, rate_limit
from openpilot.selfdrive.car.interfaces import CarControllerBase
from openpilot.selfdrive.car.toyota import toyotacan
from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
@@ -48,7 +48,7 @@ class CarController(CarControllerBase):
self.last_standstill = False
self.standstill_req = False
self.steer_rate_counter = 0
self.pcm_accel_comp = 0
self.pcm_accel_compensation = 0.0
self.distance_button = 0
self.pid = PIDController(k_p=1.0, k_i=0.25, k_f=0)
@@ -151,31 +151,20 @@ class CarController(CarControllerBase):
# When sp_tss2_long_tune is True and CC.longActive
if sp_tss2_long_tune:
# we will throw out PCM's compensations, but that may be a good thing. for example:
# we lose things like pitch compensation, gas to maintain speed, brake to compensate for creeping, etc.
# but also remove undesirable "snap to standstill" behavior when not requesting enough accel at low speeds,
# lag to start moving, lag to start braking, etc.
# PI should compensate for lack of the desirable behaviors, but might be worse than the PCM doing them
# For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot
# TODO: validate PCM_CRUISE->ACCEL_NET for braking requests and compensate for imprecise braking as well
if CC.longActive:
pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - actuators.accel) if actuators.accel > 0 else 0.0
# FIXME? neutral force will only be positive under ~5 mph, which messes up stopping control considerably
# not sure why this isn't captured in the PCM accel net, maybe that just ignores creep force + high speed deceleration
# it also doesn't seem to capture slightly more braking on downhills (VSC1S07->ASLP (pitch, deg.) might have some clues)
offset = min(CS.pcm_neutral_force / self.CP.mass, 0.0)
pitch_offset = math.sin(math.radians(CS.vsc_slope_angle)) * 9.81 # downhill is negative
# TODO: these limits are too slow to prevent a jerk when engaging, ramp down on engage?
# self.pcm_accel_comp = clip(actuators.accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.05, self.pcm_accel_comp + 0.05)
pcm_accel_comp = self.pid.update(actuators.accel - CS.pcm_calc_accel_net)
self.pcm_accel_comp = clip(pcm_accel_comp, self.pcm_accel_comp - 0.005, self.pcm_accel_comp + 0.005)
if CS.out.cruiseState.standstill or actuators.longControlState == LongCtrlState.stopping:
self.pcm_accel_comp = 0.0
self.pid.reset()
pcm_accel_cmd = actuators.accel + self.pcm_accel_comp # + offset
# pcm_accel_cmd = actuators.accel - pitch_offset
# prevent compensation windup
if actuators.accel - pcm_accel_compensation > self.params.ACCEL_MAX:
pcm_accel_compensation = actuators.accel - self.params.ACCEL_MAX
if not CC.longActive:
self.pid.reset()
self.pcm_accel_comp = 0.0
pcm_accel_cmd = 0.0
self.pcm_accel_compensation = rate_limit(pcm_accel_compensation, self.pcm_accel_compensation, -0.01, 0.01)
pcm_accel_cmd = actuators.accel - self.pcm_accel_compensation
else:
self.pcm_accel_compensation = 0.0
pcm_accel_cmd = actuators.accel
pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
else:
selfdrive/car/toyota/carstate.py
+12 -12
View File
@@ -58,10 +58,10 @@ class CarState(CarStateBase):
self.acc_type = 1
self.lkas_hud = {}
self.pcm_accel_net = 0.0
self.pcm_true_accel_net = 0.0
self.pcm_calc_accel_net = 0.0
self.pcm_neutral_force = 0.0
self.vsc_slope_angle = 0.0
#self.pcm_true_accel_net = 0.0
#self.pcm_calc_accel_net = 0.0
#self.pcm_neutral_force = 0.0
#self.vsc_slope_angle = 0.0
self.lkas_enabled = None
self.prev_lkas_enabled = None
@@ -133,11 +133,11 @@ class CarState(CarStateBase):
# thought to be the gas/brake as issued by the pcm (0=coasting)
self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"] # this is only accurate for braking * 43
self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"] # this is only accurate for acceleration * 78
self.pcm_calc_accel_net = cp.vl["GEAR_PACKET_HYBRID"]["CAR_MOVEMENT"] / 78 - cp.vl["BRAKE"]["BRAKE_PEDAL"] / 43
self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"]
self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
self.vsc_slope_angle = cp.vl["VSC1S07"]["ASLP"]
#self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"] # this is only accurate for acceleration * 78
#self.pcm_calc_accel_net = cp.vl["GEAR_PACKET_HYBRID"]["CAR_MOVEMENT"] / 78 - cp.vl["BRAKE"]["BRAKE_PEDAL"] / 43
#self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"]
#self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
#self.vsc_slope_angle = cp.vl["VSC1S07"]["ASLP"]
ret.standstill = abs(ret.vEgoRaw) < 1e-3
@@ -457,14 +457,14 @@ class CarState(CarStateBase):
("BODY_CONTROL_STATE_2", 2),
("ESP_CONTROL", 3),
("EPS_STATUS", 25),
("GEAR_PACKET_HYBRID", 60),
("BRAKE", 80),
#("GEAR_PACKET_HYBRID", 60),
#("BRAKE", 80),
("BRAKE_MODULE", 40),
("WHEEL_SPEEDS", 80),
("STEER_ANGLE_SENSOR", 80),
("PCM_CRUISE", 33),
("PCM_CRUISE_SM", 1),
("VSC1S07", 20),
#("VSC1S07", 20),
("STEER_TORQUE_SENSOR", 50),
("CLUTCH", 16),
]