mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-29 19:23:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

move-fast: v-tsc fix (#40)

Browse Source
This commit is contained in:
Jason Wen
2023-02-19 22:04:35 -05:00
committed by GitHub
parent c97b3b56a9
commit adf844eb30
2 changed files with 10 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/lateral_planner.py
+4
View File
@@ -41,6 +41,7 @@ class LateralPlanner:
self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE)
self.y_pts = np.zeros(TRAJECTORY_SIZE)
self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
self.lat_mpc = LateralMpc()
self.reset_mpc(np.zeros(4))
@@ -181,6 +182,9 @@ class LateralPlanner:
lateralPlan.dynamicLaneProfile = int(self.dynamic_lane_profile)
lateralPlan.dynamicLaneProfileStatus = bool(self.dynamic_lane_profile_status)
lateralPlan.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]]
lateralPlan.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]]
if self.standstill:
self.standstill_elapsed += DT_MDL
else:
selfdrive/controls/lib/vision_turn_controller.py
+6 -5
View File
@@ -6,7 +6,7 @@ from common.params import Params
from common.realtime import sec_since_boot
from common.conversions import Conversions as CV
from selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
_MIN_V = 5.6 # Do not operate under 20km/h
@@ -186,10 +186,11 @@ class VisionTurnController():
c_y = width_pts / 2 + lll_y
path_poly = np.polyfit(ll_x, c_y, 3)
# 2. If not polynomial derived from lanes, then derive it from driving path as provided by `lateralPlanner`.
if path_poly is None and lat_planner_data is not None and len(lat_planner_data.psis) == CONTROL_N \
and lat_planner_data.dPathPoints[0] > 0:
path_poly = np.polyfit(lat_planner_data.psis, lat_planner_data.dPathPoints, 3)
# 2. If not polynomial derived from lanes, then derive it from compensated driving path with lanes as
# provided by `lateralPlanner`.
if path_poly is None and lat_planner_data is not None and len(lat_planner_data.dPathWLinesX) > 0 \
and lat_planner_data.dPathWLinesX[0] > 0:
path_poly = np.polyfit(lat_planner_data.dPathWLinesX, lat_planner_data.dPathWLinesY, 3)
# 3. If no polynomial derived from lanes or driving path, then provide a straight line poly.
if path_poly is None: