diff --git a/cereal/car.capnp b/cereal/car.capnp index 69daf1db980eee..3398fb70c0ec15 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -501,6 +501,8 @@ struct CarParams { wheelSpeedFactor @63 :Float32; # Multiplier on wheels speeds to computer actual speeds # FrogPilot CarParams + lateralTune @78 :Bool; + pfeiferjDesiredCurvatures @80 :Bool; struct SafetyConfig { safetyModel @0 :SafetyModel; diff --git a/cereal/log.capnp b/cereal/log.capnp index 99cd96614421a2..ba4454837a67b5 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -996,6 +996,7 @@ struct LongitudinalPlan @0xe00b5b3eba12876c { accels @32 :List(Float32); speeds @33 :List(Float32); jerks @34 :List(Float32); + distances @37 :List(Float32); solverExecutionTime @35 :Float32; personality @36 :LongitudinalPersonality; diff --git a/common/params.cc b/common/params.cc index 98310657f87651..4b5c99ad9ca38a 100644 --- a/common/params.cc +++ b/common/params.cc @@ -91,6 +91,7 @@ std::unordered_map keys = { {"AssistNowToken", PERSISTENT}, {"AthenadPid", PERSISTENT}, {"AthenadUploadQueue", PERSISTENT}, + {"AverageDesiredCurvature", PERSISTENT}, {"BlindSpotPath", PERSISTENT}, {"CalibrationParams", PERSISTENT}, {"CameraDebugExpGain", CLEAR_ON_MANAGER_START}, @@ -171,6 +172,7 @@ std::unordered_map keys = { {"LastSystemShutdown", CLEAR_ON_MANAGER_START}, {"LastUpdateException", CLEAR_ON_MANAGER_START}, {"LastUpdateTime", PERSISTENT}, + {"LateralTuning", PERSISTENT}, {"LiveParameters", PERSISTENT}, {"LiveTorqueCarParams", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, diff --git a/selfdrive/assets/offroad/icon_lateral_tune.png b/selfdrive/assets/offroad/icon_lateral_tune.png new file mode 100644 index 00000000000000..ba83e3a3b22914 Binary files /dev/null and b/selfdrive/assets/offroad/icon_lateral_tune.png differ diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index c4f90cb570a6e9..77500c20be5ecb 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -183,6 +183,8 @@ def get_std_params(candidate): # FrogPilot variables params = Params() + ret.lateralTune = params.get_bool("LateralTuning") + ret.pfeiferjDesiredCurvatures = ret.lateralTune and params.get_bool("AverageDesiredCurvature") return ret @staticmethod diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 71585441a5f94b..641158158da2fa 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -118,6 +118,7 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None): # FrogPilot variables fire_the_babysitter = self.params.get_bool("FireTheBabysitter") frog_theme = self.params.get_bool("FrogTheme") + self.average_desired_curvature = self.CP.pfeiferjDesiredCurvatures self.frog_sounds = frog_theme and self.params.get_bool("FrogSounds") self.mute_dm = fire_the_babysitter and self.params.get_bool("MuteDM") self.mute_overheat = fire_the_babysitter and self.params.get_bool("MuteSystemOverheat") @@ -630,7 +631,9 @@ def state_control(self, CS): self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, - lat_plan.curvatureRates) + lat_plan.curvatureRates, + long_plan.distances, + self.average_desired_curvature) actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.last_actuators, self.steer_limited, self.desired_curvature, self.desired_curvature_rate, self.sm['liveLocationKalman']) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ab65da2e1d9328..9d2621ab055e64 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -16,6 +16,7 @@ V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors +MIN_DIST = 0.001 MIN_SPEED = 1.0 CONTROL_N = 17 CAR_ROTATION_RADIUS = 0.0 @@ -163,11 +164,12 @@ def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) -def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): - if len(psis) != CONTROL_N: +def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates, distances, average_desired_curvature): + if len(psis) != CONTROL_N or len(distances) != CONTROL_N: psis = [0.0]*CONTROL_N curvatures = [0.0]*CONTROL_N curvature_rates = [0.0]*CONTROL_N + distances = [0.0]*CONTROL_N v_ego = max(MIN_SPEED, v_ego) # TODO this needs more thought, use .2s extra for now to estimate other delays @@ -178,7 +180,10 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): # psi to calculate a simple linearization of desired curvature current_curvature_desired = curvatures[0] psi = interp(delay, T_IDXS[:CONTROL_N], psis) - average_curvature_desired = psi / (v_ego * delay) + # Pfeiferj's #28118 PR - https://github.com/commaai/openpilot/pull/28118 + distance = interp(delay, T_IDXS[:CONTROL_N], distances) + distance = max(MIN_DIST, distance) + average_curvature_desired = psi / distance if average_desired_curvature else psi / (v_ego * delay) desired_curvature = 2 * average_curvature_desired - current_curvature_desired # This is the "desired rate of the setpoint" not an actual desired rate diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 269eac650c9f43..5e4d199faa2ed0 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -51,6 +51,7 @@ def __init__(self, CP, debug=False): self.reset_mpc(np.zeros(4)) # FrogPilot variables + self.average_desired_curvature = CP.pfeiferjDesiredCurvatures def reset_mpc(self, x0=None): if x0 is None: @@ -71,7 +72,10 @@ def update(self, sm): self.plan_yaw = np.array(md.orientation.z) self.plan_yaw_rate = np.array(md.orientationRate.z) self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z]) - car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car) + if self.average_desired_curvature: + car_speed = np.array(md.velocity.x) - get_speed_error(md, v_ego_car) + else: + car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car) self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf) self.v_ego = self.v_plan[0] diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index a81cb33a5668de..1b22443e08ee91 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -234,6 +234,7 @@ def reset(self): # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.solver.reset() # self.solver.options_set('print_level', 2) + self.x_solution = np.zeros(N+1) self.v_solution = np.zeros(N+1) self.a_solution = np.zeros(N+1) self.prev_a = np.array(self.a_solution) @@ -440,6 +441,7 @@ def run(self): for i in range(N): self.u_sol[i] = self.solver.get(i, 'u') + self.x_solution = self.x_sol[:,0] self.v_solution = self.x_sol[:,1] self.a_solution = self.x_sol[:,2] self.j_solution = self.u_sol[:,0] diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 648bdf74e6a4c8..82caf03b546e19 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -56,6 +56,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0): self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL) self.v_model_error = 0.0 + self.x_desired_trajectory = np.zeros(CONTROL_N) self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) @@ -146,8 +147,10 @@ def update(self, sm): x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality) + self.x_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.x_solution) self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution) + self.x_desired_trajectory = self.x_desired_trajectory_full[:CONTROL_N] self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N] self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N] self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) @@ -171,6 +174,7 @@ def publish(self, sm, pm): longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] + longitudinalPlan.distances = self.x_desired_trajectory.tolist() longitudinalPlan.speeds = self.v_desired_trajectory.tolist() longitudinalPlan.accels = self.a_desired_trajectory.tolist() longitudinalPlan.jerks = self.j_desired_trajectory.tolist() diff --git a/selfdrive/ui/qt/offroad/frogpilot_settings.cc b/selfdrive/ui/qt/offroad/frogpilot_settings.cc index 5dc2d81b79e408..09cb4b3e6b45df 100644 --- a/selfdrive/ui/qt/offroad/frogpilot_settings.cc +++ b/selfdrive/ui/qt/offroad/frogpilot_settings.cc @@ -19,6 +19,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(QWidget *parent) : FrogPilotPanel static const std::vector> toggles = { {"DeviceShutdownTimer", "Device Shutdown Timer", "Set the timer for when the device turns off after being offroad to reduce energy waste and prevent battery drain.", "../assets/offroad/icon_time.png"}, {"FireTheBabysitter", "Fire the Babysitter", "Disable some of openpilot's 'Babysitter Protocols'.", "../assets/offroad/icon_babysitter.png"}, + {"LateralTuning", "Lateral Tuning", "Change the way openpilot steers.", "../assets/offroad/icon_lateral_tune.png"}, {"NudgelessLaneChange", "Nudgeless Lane Change", "Switch lanes without having to nudge the steering wheel.", "../assets/offroad/icon_lane.png"} }; @@ -37,6 +38,10 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(QWidget *parent) : FrogPilotPanel {"MuteSeatbelt", "Mute Seatbelt"}, {"MuteSystemOverheat", "Mute Overheat"} }, mainLayout); + } else if (key == "LateralTuning") { + createSubControl(key, label, desc, icon, {}, { + {"AverageDesiredCurvature", "Average Desired Curvature", "Use Pfeiferj's distance based curvature adjustment for smoother handling of curves."} + }); } else if (key == "NudgelessLaneChange") { createSubControl(key, label, desc, icon, { new LaneChangeTimer(),