diff --git a/common/params.cc b/common/params.cc index dbf2c1d7f..35afa1290 100644 --- a/common/params.cc +++ b/common/params.cc @@ -420,6 +420,7 @@ std::unordered_map keys = { {"StandbyMode", PERSISTENT}, {"StockTune", PERSISTENT}, {"StoppingDistance", PERSISTENT}, + {"TacoTune", PERSISTENT}, {"TetheringEnabled", PERSISTENT}, {"ToyotaDoors", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py old mode 100755 new mode 100644 index 67381ebd1..8f7d9ad5d --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,6 +16,7 @@ from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC, LEAD_ACCEL_TAU from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error +from openpilot.system.version import get_short_branch from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS @@ -137,10 +138,17 @@ class LongitudinalPlanner: self.solverExecutionTime = 0.0 # FrogPilot variables + self.params = Params() + self.params_memory = Params("/dev/shm/params") + self.radarless_model = self.params.get("Model", block=True, encoding='utf-8') in RADARLESS_MODELS + self.release = get_short_branch() == "FrogPilot" + + self.update_frogpilot_params() + @staticmethod - def parse_model(model_msg, model_error): + def parse_model(model_msg, model_error, v_ego, taco_tune): if (len(model_msg.position.x) == 33 and len(model_msg.velocity.x) == 33 and len(model_msg.acceleration.x) == 33): @@ -153,6 +161,13 @@ class LongitudinalPlanner: v = np.zeros(len(T_IDXS_MPC)) a = np.zeros(len(T_IDXS_MPC)) j = np.zeros(len(T_IDXS_MPC)) + + if taco_tune: + max_lat_accel = interp(v_ego, [5, 10, 20], [1.5, 2.0, 3.0]) + curvatures = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.orientationRate.z) / np.clip(v, 0.3, 100.0) + max_v = np.sqrt(max_lat_accel / (np.abs(curvatures) + 1e-3)) - 2.0 + v = np.minimum(max_v, v) + return x, v, a, j def update(self, sm): @@ -210,7 +225,7 @@ class LongitudinalPlanner: self.mpc.set_weights(sm['frogpilotPlan'].jerk, prev_accel_constraint, personality=sm['controlsState'].personality) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) + x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, self.taco_tune) self.mpc.update(self.lead_one, self.lead_two, sm['frogpilotPlan'].vCruise, x, v, a, j, self.radarless_model, sm['frogpilotPlan'].tFollow, sm['frogpilotCarControl'].trafficModeActive, personality=sm['controlsState'].personality) @@ -230,6 +245,13 @@ class LongitudinalPlanner: self.a_desired = float(interp(self.dt, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + + def update_frogpilot_params(self): + lateral_tune = self.params.get_bool("LateralTune") + self.taco_tune = lateral_tune and self.params.get_bool("TacoTune") and not self.release + def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index 0bea61d3c..0fe6e7b62 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -80,6 +80,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"ForceAutoTune", tr("Force Auto Tune"), tr("Forces comma's auto lateral tuning for unsupported vehicles."), ""}, {"NNFF", tr("NNFF"), tr("Use Twilsonco's Neural Network Feedforward for enhanced precision in lateral control."), ""}, {"NNFFLite", tr("NNFF-Lite"), tr("Use Twilsonco's Neural Network Feedforward for enhanced precision in lateral control for cars without available NNFF logs."), ""}, + {"TacoTune", tr("Taco Tune"), tr("Use comma's 'Taco Tune' designed for handling left and right turns."), ""}, {"LongitudinalTune", tr("Longitudinal Tuning"), tr("Modify openpilot's acceleration and braking behavior."), "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, {"AccelerationProfile", tr("Acceleration Profile"), tr("Change the acceleration rate to be either sporty or eco-friendly."), ""}, @@ -283,6 +284,10 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil modifiedLateralTuneKeys.erase("NNFF"); } + if (isRelease ) { + modifiedLateralTuneKeys.erase("TacoTune"); + } + toggle->setVisible(modifiedLateralTuneKeys.find(key.c_str()) != modifiedLateralTuneKeys.end()); } }); diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index 0a9ee5512..8d82a0ca7 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -43,7 +43,7 @@ private: std::set deviceManagementKeys = {"DeviceShutdown", "HigherBitrate", "IncreaseThermalLimits", "LowVoltageShutdown", "NoLogging", "NoUploads", "OfflineMode"}; std::set experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap"}; std::set laneChangeKeys = {"LaneChangeTime", "LaneDetectionWidth", "OneLaneChange"}; - std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite"}; + std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite", "TacoTune"}; std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "SmoothBraking", "StoppingDistance"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"}; std::set qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise", "SetSpeedOffset"};