mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-21 14:02:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

tici: take into account frame timestamp (#2199)

Browse Source 
* compensate for frame processing time

* add rolling shutter time

* Add max frame delay of 250ms

* only delay on tici
This commit is contained in:
Willem Melching
2020-09-18 17:42:29 +02:00
committed by GitHub
parent 39fc171f91
commit 3a494e144b
1 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/pathplanner.py
+9 -2
View File
@@ -7,6 +7,7 @@ from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
from selfdrive.controls.lib.lane_planner import LanePlanner
from selfdrive.config import Conversions as CV
from common.params import Params
from common.hardware import TICI
import cereal.messaging as messaging
from cereal import log
@@ -164,8 +165,14 @@ class PathPlanner():
self.LP.r_prob *= self.lane_change_ll_prob
self.LP.update_d_poly(v_ego)
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay)
if TICI:
frame_delay = min((sm.logMonoTime['model'] - sm['model'].timestampEof) / 1e9, 0.250)
delay = frame_delay + CP.steerActuatorDelay
else:
delay = CP.steerActuatorDelay
# account for actuation + frame delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, delay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,