diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 3d6bd5e22a..e85735b1a6 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -19,7 +19,6 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = -math.inf ret.maxLateralAccel = math.inf # TODO: set to a reasonable value ret.steerRatio = 0.5 - ret.steerRateCost = 0.5 ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0. diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 55672dd4ab..817a5b387e 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -20,7 +20,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 - ret.steerRateCost = 0.7 ret.steerLimitTimer = 0.4 if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index ec7c723669..c608cc08d9 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -59,7 +59,6 @@ class CarInterface(CarInterfaceBase): # LCA can steer down to zero ret.minSteerSpeed = 0. - ret.steerRateCost = 1.0 ret.centerToFront = ret.wheelbase * 0.44 ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 498f4b3347..9c1744dfb4 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -63,7 +63,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 - ret.steerRateCost = 1.0 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.longitudinalTuning.kpBP = [5., 35.] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index ea7deab5d7..994152608e 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -319,7 +319,6 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor=tire_stiffness_factor) ret.steerActuatorDelay = 0.1 - ret.steerRateCost = 0.5 ret.steerLimitTimer = 0.8 return ret diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6fd75ebfab..497d6b3f30 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -43,7 +43,6 @@ class CarInterface(CarInterfaceBase): ret.dashcamOnly = not os.path.exists('/data/enable-ev6') ret.steerActuatorDelay = 0.1 # Default delay - ret.steerRateCost = 0.5 ret.steerLimitTimer = 0.4 tire_stiffness_factor = 1. diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 0ef573c6b6..cbeb910de2 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -25,7 +25,6 @@ class CarInterface(CarInterfaceBase): ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021) ret.steerActuatorDelay = 0.1 - ret.steerRateCost = 1.0 ret.steerLimitTimer = 0.8 tire_stiffness_factor = 0.70 # not optimized yet diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 3bf34e8266..436cef68bf 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -14,7 +14,6 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] ret.steerLimitTimer = 1.0 - ret.steerRateCost = 0.5 ret.steerActuatorDelay = 0.1 diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index d0d8e91ce1..8c5cab86e8 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -22,7 +22,6 @@ class CarInterface(CarInterfaceBase): ret.dashcamOnly = candidate in PREGLOBAL_CARS - ret.steerRateCost = 0.7 ret.steerLimitTimer = 0.4 if candidate == CAR.ASCENT: diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 71594cecb6..d4eda38e64 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -42,7 +42,6 @@ class CarInterface(CarInterfaceBase): ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0.25 - ret.steerRateCost = 0.5 if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS): ret.mass = 2100. + STD_CARGO_KG diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 52c89440c7..15df1aafef 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -33,7 +33,6 @@ class TestCarInterfaces(unittest.TestCase): assert car_interface self.assertGreater(car_params.mass, 1) - self.assertGreater(car_params.steerRateCost, 1e-3) if car_params.steerControlType != car.CarParams.SteerControlType.angle: tuning = car_params.lateralTuning.which() diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 9ab881279b..27dc4ba776 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -121,7 +121,6 @@ class TestCarModelBase(unittest.TestCase): # make sure car params are within a valid range self.assertGreater(self.CP.mass, 1) - self.assertGreater(self.CP.steerRateCost, 1e-3) if self.CP.steerControlType != car.CarParams.SteerControlType.angle: tuning = self.CP.lateralTuning.which() diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 7969775a89..d0f4789775 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -213,7 +213,6 @@ class CarInterface(CarInterfaceBase): ret.mass = 4305. * CV.LB_TO_KG + STD_CARGO_KG set_lat_tune(ret.lateralTuning, LatTunes.PID_J) - ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 # TODO: get actual value, for now starting with reasonable value for diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 3a0d7c8ce8..6782105c16 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -45,7 +45,6 @@ class CarInterface(CarInterfaceBase): # Global lateral tuning defaults, can be overridden per-vehicle ret.steerActuatorDelay = 0.1 - ret.steerRateCost = 1.0 ret.steerLimitTimer = 0.4 ret.steerRatio = 15.6 # Let the params learner figure this out tire_stiffness_factor = 1.0 # Let the params learner figure this out diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index ec21c16e91..019a19fb1d 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -11,13 +11,12 @@ from cereal import log class LateralPlanner: - def __init__(self, CP, use_lanelines=True, wide_camera=False): + def __init__(self, use_lanelines=True, wide_camera=False): self.use_lanelines = use_lanelines self.LP = LanePlanner(wide_camera) self.DH = DesireHelper() self.last_cloudlog_t = 0 - self.steer_rate_cost = CP.steerRateCost self.solution_invalid_cnt = 0 self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) @@ -59,12 +58,12 @@ class LateralPlanner: # Calculate final driving path and set MPC costs if self.use_lanelines: d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) - self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost) + self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, MPC_COST_LAT.STEER_RATE) else: d_path_xyz = self.path_xyz # Heading cost is useful at low speed, otherwise end of plan can be off-heading heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15]) - self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, self.steer_rate_cost) + self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE) y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) @@ -79,7 +78,7 @@ class LateralPlanner: y_pts, heading_pts) # init state for next - # mpc.u_sol is the desired curvature rate given x0 curv state. + # mpc.u_sol is the desired curvature rate given x0 curv state. # with x0[3] = measured_curvature, this would be the actual desired rate. # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 06807b2a5c..9356a55d84 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -22,7 +22,7 @@ def plannerd_thread(sm=None, pm=None): cloudlog.event("e2e mode", on=use_lanelines) longitudinal_planner = Planner(CP) - lateral_planner = LateralPlanner(CP, use_lanelines=use_lanelines, wide_camera=wide_camera) + lateral_planner = LateralPlanner(use_lanelines=use_lanelines, wide_camera=wide_camera) if sm is None: sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2'], diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 97e65fbc0e..a691051636 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -245,7 +245,7 @@ struct ModelOutput { constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float); #ifdef TEMPORAL - constexpr int TEMPORAL_SIZE = 512; + constexpr int TEMPORAL_SIZE = 512+256; #else constexpr int TEMPORAL_SIZE = 0; #endif diff --git a/selfdrive/modeld/models/supercombo.dlc b/selfdrive/modeld/models/supercombo.dlc index 23f6d904fb..90f7a2e65b 100644 --- a/selfdrive/modeld/models/supercombo.dlc +++ b/selfdrive/modeld/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:027cbb1fabae369878271cb0e3505071a8bdaa07473fad9a0b2e8d695c5dc1ff -size 76725611 +oid sha256:4c2cb3a3054f3292bbe538d6b793908dc2e234c200802d41b6766d3cb51b0b44 +size 101662751 diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 9023c18dd7..0493398560 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:484976ea5bd4ddcabc82e95faf30d7311a27802c1e337472558699fa2395a499 -size 77472267 +oid sha256:96b60d0bfd1386c93b4f79195aa1c5e77b23e0250578a308ee2c58857ed5eb49 +size 102570834 diff --git a/selfdrive/modeld/thneed/compile.cc b/selfdrive/modeld/thneed/compile.cc index f76c63b2b9..a2f55ffd97 100644 --- a/selfdrive/modeld/thneed/compile.cc +++ b/selfdrive/modeld/thneed/compile.cc @@ -5,7 +5,7 @@ #include "selfdrive/modeld/thneed/thneed.h" #include "system/hardware/hw.h" -#define TEMPORAL_SIZE 512 +#define TEMPORAL_SIZE 512+256 #define DESIRE_LEN 8 #define TRAFFIC_CONVENTION_LEN 2 diff --git a/selfdrive/modeld/thneed/optimizer.cc b/selfdrive/modeld/thneed/optimizer.cc index 03d20ff386..39737d3d76 100644 --- a/selfdrive/modeld/thneed/optimizer.cc +++ b/selfdrive/modeld/thneed/optimizer.cc @@ -9,7 +9,7 @@ extern map g_program_source; -static int is_same_size_image(cl_mem a, cl_mem b) { +/*static int is_same_size_image(cl_mem a, cl_mem b) { size_t a_width, a_height, a_depth, a_array_size, a_row_pitch, a_slice_pitch; clGetImageInfo(a, CL_IMAGE_WIDTH, sizeof(a_width), &a_width, NULL); clGetImageInfo(a, CL_IMAGE_HEIGHT, sizeof(a_height), &a_height, NULL); @@ -29,7 +29,7 @@ static int is_same_size_image(cl_mem a, cl_mem b) { return (a_width == b_width) && (a_height == b_height) && (a_depth == b_depth) && (a_array_size == b_array_size) && (a_row_pitch == b_row_pitch) && (a_slice_pitch == b_slice_pitch); -} +}*/ static cl_mem make_image_like(cl_context context, cl_mem val) { cl_image_format format; @@ -138,7 +138,7 @@ int Thneed::optimize() { // delete useless copy layers // saves ~0.7 ms - if (kq[i]->name == "concatenation" || kq[i]->name == "flatten") { + /*if (kq[i]->name == "concatenation" || kq[i]->name == "flatten") { string in = kq[i]->args[kq[i]->get_arg_num("input")]; string out = kq[i]->args[kq[i]->get_arg_num("output")]; if (is_same_size_image(*(cl_mem*)in.data(), *(cl_mem*)out.data())) { @@ -148,7 +148,7 @@ int Thneed::optimize() { kq.erase(kq.begin()+i); --i; } - } + }*/ // NOTE: if activations/accumulation are done in the wrong order, this will be wrong diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index a7a909d2a4..54c2ed54ad 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -512a9d4596c8faba304d6f7ded2ce77837357b65 +629eaa7b26d1721a71547f9de880f99732cb27f3 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index b8976e5f38..8ed68d5bdd 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -1d66eed104dbc124c4e5679f5dddf40197b86ce9 +ed1dfb8b155ebcd8fdad4e06462b3bb7869fc67b