spacecruft/nopenpilot
spacecruft/nopenpilot
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Different lag comp (#20024)

Browse source 
* more sensible

* different names for different segs

* new ref

* this is checked somewhere
This commit is contained in:
HaraldSchafer 2021-02-05 14:01:22 -08:00 committed by GitHub
parent a7fe4560a6
commit c3707328f5
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 9 additions and 11 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

14
selfdrive/controls/lib/lateral_planner.py
View file

@ -186,16 +186,14 @@ class LateralPlanner():
# TODO this needs more thought, use .2s extra for now to estimate other delays
delay = CP.steerActuatorDelay + .2
next_curvature = interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.curvature)
next_curvature = interp(delay, self.t_idxs[:MPC_N+1], self.mpc_solution.curvature)
psi = interp(delay, self.t_idxs[:MPC_N+1], self.mpc_solution.psi)
next_curvature_rate = self.mpc_solution.curvature_rate[0]
# TODO This gets around the fact that MPC can plan to turn and turn back in the
# time between now and delay, need better abstraction between planner and controls
plan_ahead_idx = sum(self.t_idxs < delay)
if next_curvature_rate > 0:
next_curvature = max(list(self.mpc_solution.curvature)[:plan_ahead_idx] + [next_curvature])
next_curvature_from_psi = psi/(v_ego*delay)
if psi > self.mpc_solution.curvature[0] * delay * v_ego:
next_curvature = max(next_curvature_from_psi, next_curvature)
else:
next_curvature = min(list(self.mpc_solution.curvature)[:plan_ahead_idx] + [next_curvature])
next_curvature = min(next_curvature_from_psi, next_curvature)
# reset to current steer angle if not active or overriding
if active:

2
selfdrive/test/process_replay/ref_commit
View file

@ -1 +1 @@
c1740f64ad8a8e789023e062bc1ab125438e6a0a
defb3dc7205a3d501829f4a780241dfb97132025

4
selfdrive/test/process_replay/test_processes.py
View file

@ -14,9 +14,9 @@ INJECT_MODEL = 0
segments = [
("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.SONATA
("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS (INDI)
("TOYOTA", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR)
("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR)
("HONDA", "0982d79ebb0de295|2021-01-08--10-13-10--6"), # HONDA.CIVIC (NIDEC)
("HONDA", "a8e8bf6a3864361b|2021-01-04--03-01-18--2"), # HONDA.ACCORD (BOSCH)
("HONDA2", "a8e8bf6a3864361b|2021-01-04--03-01-18--2"), # HONDA.ACCORD (BOSCH)
("CHRYSLER", "52d86230ee29aa84|2021-01-10--17-16-34--30"), # CHRYSLER.PACIFICA
("SUBARU", "4d70bc5e608678be|2021-01-15--17-02-04--5"), # SUBARU.IMPREZA
("GM", "ae3ed0eb20960a20|2021-01-15--15-04-06--8"), # GM.VOLT