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

try upper/lower delay similar to longcontrol lag comp

This commit is contained in:
Shane Smiskol 2023-10-09 18:29:08 -07:00
parent 7cfc1c616c
commit 8e85333ee6
3 changed files with 13 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/car/toyota/interface.py
View File

@ -33,7 +33,7 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA
# LTA control can be more delayed and winds up more often
ret.steerActuatorDelay = 0.18
ret.steerActuatorDelay = 0.35
ret.steerLimitTimer = 0.8
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)

14
selfdrive/controls/controlsd.py
View File

@ -614,10 +614,16 @@ class Controls:
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
# Steering PID loop and lateral MPC
self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
lat_plan.psis,
lat_plan.curvatures,
lat_plan.curvatureRates)
desired_curvature_lower, desired_curvature_rate_lower = get_lag_adjusted_curvature(0.18, CS.vEgo,
lat_plan.psis,
lat_plan.curvatures,
lat_plan.curvatureRates)
desired_curvature_upper, desired_curvature_rate_upper = get_lag_adjusted_curvature(0.35, CS.vEgo,
lat_plan.psis,
lat_plan.curvatures,
lat_plan.curvatureRates)
self.desired_curvature = desired_curvature_lower if abs(desired_curvature_lower) < abs(desired_curvature_upper) else desired_curvature_upper
self.desired_curvature_rate = desired_curvature_rate_lower if abs(desired_curvature_rate_lower) < abs(desired_curvature_rate_upper) else desired_curvature_rate_upper
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.last_actuators, self.steer_limited, self.desired_curvature,
self.desired_curvature_rate, self.sm['liveLocationKalman'])

4
selfdrive/controls/lib/drive_helpers.py
View File

@ -163,7 +163,7 @@ def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
def get_lag_adjusted_curvature(steer_actuator_delay, v_ego, psis, curvatures, curvature_rates):
if len(psis) != CONTROL_N:
psis = [0.0]*CONTROL_N
curvatures = [0.0]*CONTROL_N
@ -171,7 +171,7 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
v_ego = max(MIN_SPEED, v_ego)
# TODO this needs more thought, use .2s extra for now to estimate other delays
delay = CP.steerActuatorDelay + .2
delay = steer_actuator_delay + .2
# MPC can plan to turn the wheel and turn back before t_delay. This means
# in high delay cases some corrections never even get commanded. So just use