mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-30 02:52:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

longcontrol/update: optimize capnp (#23382)

Browse Source 
old-commit-hash: 5dc631fa1bf471264fceab67edfe4c3962ac9831
This commit is contained in:
Dean Lee
2022-01-04 18:32:54 +08:00
committed by GitHub
parent 01c05dc66c
commit a55046c949
1 changed files with 8 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longcontrol.py
+8 -7
View File
@@ -65,15 +65,16 @@ class LongControl():
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
# TODO estimate car specific lag, use .15s for now
if len(long_plan.speeds) == CONTROL_N:
v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], long_plan.speeds)
a_target_lower = 2 * (v_target_lower - long_plan.speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0]
speeds = long_plan.speeds
if len(speeds) == CONTROL_N:
v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], speeds)
a_target_lower = 2 * (v_target_lower - speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0]
v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds)
a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], speeds)
a_target_upper = 2 * (v_target_upper - speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
a_target = min(a_target_lower, a_target_upper)
v_target_future = long_plan.speeds[-1]
v_target_future = speeds[-1]
else:
v_target_future = 0.0
a_target = 0.0
@@ -96,7 +97,7 @@ class LongControl():
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = long_plan.speeds[0]
self.v_pid = speeds[0]
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration