diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index a332d06765..db5b24ef38 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -17,7 +17,7 @@ V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors MIN_SPEED = 1.0 -LAT_MPC_N = 16 +LAT_MPC_N = 32 LON_MPC_N = 32 CONTROL_N = 17 CAR_ROTATION_RADIUS = 0.0 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index aac7a78ccd..38d4c54abe 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,5 +1,8 @@ #!/usr/bin/env python3 from cereal import car +import numpy as np +from selfdrive.modeld.constants import T_IDXS +from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_LONG from common.params import Params from common.realtime import Priority, config_realtime_process from system.swaglog import cloudlog @@ -32,7 +35,7 @@ def plannerd_thread(sm=None, pm=None): if sm.updated['modelV2']: longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) - lateral_planner.update(sm, longitudinal_planner.mpc.v_solution) + lateral_planner.update(sm, np.interp(T_IDXS, T_IDXS_LONG, longitudinal_planner.mpc.v_solution)) lateral_planner.publish(sm, pm)