From 3e9de4099a68e53be4ae775e70ee42a600737fde Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 27 Feb 2024 16:34:47 -0700 Subject: [PATCH] User set steer ratio --- common/params.cc | 2 ++ selfdrive/controls/controlsd.py | 5 +++- selfdrive/frogpilot/ui/control_settings.cc | 27 ++++++++++++++++++++++ selfdrive/frogpilot/ui/control_settings.h | 4 +++- selfdrive/locationd/paramsd.py | 5 ++++ 5 files changed, 41 insertions(+), 2 deletions(-) diff --git a/common/params.cc b/common/params.cc index ff99a78c..da8a36e5 100644 --- a/common/params.cc +++ b/common/params.cc @@ -394,6 +394,8 @@ std::unordered_map keys = { {"SpeedLimitChangedAlert", PERSISTENT}, {"StandardFollow", PERSISTENT}, {"StandardJerk", PERSISTENT}, + {"SteerRatio", PERSISTENT}, + {"SteerRatioStock", PERSISTENT}, {"StockTune", PERSISTENT}, {"StoppingDistance", PERSISTENT}, {"StorageParamsSet", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 21a6070d..386c0062 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -812,7 +812,7 @@ class Controls: # Update VehicleModel lp = self.sm['liveParameters'] x = max(lp.stiffnessFactor, 0.1) - sr = max(lp.steerRatio, 0.1) + sr = max(self.steer_ratio, 0.1) if self.use_custom_steer_ratio else max(lp.steerRatio, 0.1) self.VM.update_params(x, sr) # Update Torque Params @@ -1188,6 +1188,9 @@ class Controls: lateral_tune = self.params.get_bool("LateralTune") self.force_auto_tune = lateral_tune and self.params.get_float("ForceAutoTune") + stock_steer_ratio = self.params.get_float("SteerRatioStock") + self.steer_ratio = self.params.get_float("SteerRatio") if lateral_tune else stock_steer_ratio + self.use_custom_steer_ratio = self.steer_ratio != stock_steer_ratio self.frogpilot_variables.lock_doors = self.params.get_bool("LockDoors") self.frogpilot_variables.long_pitch = self.params.get_bool("LongPitch") diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 22a0e67f..25e4ca8a 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -25,6 +25,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"ForceAutoTune", "Force Auto Tune", "Forces comma's auto lateral tuning for unsupported vehicles.", ""}, {"NNFF", "NNFF - Neural Network Feedforward", "Use Twilsonco's Neural Network Feedforward for enhanced precision in lateral control.", ""}, + {"SteerRatio", steerRatioStock != 0 ? QString("Steer Ratio (Default: %1)").arg(steerRatioStock, 0, 'f', 2) : "Steer Ratio", "Set a custom steer ratio for your vehicle controls.", ""}, {"UseLateralJerk", "Use Lateral Jerk", "Include steer torque necessary to achieve desired steer rate (lateral jerk).", ""}, {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, @@ -222,6 +223,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } }); toggle = lateralTuneToggle; + } else if (param == "SteerRatio") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, steerRatioStock * 0.75, steerRatioStock * 1.25, std::map(), this, false, "", 1, 0.01); } else if (param == "LongitudinalTune") { FrogPilotParamManageControl *longitudinalTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); @@ -505,6 +508,30 @@ void FrogPilotControlsPanel::updateToggles() { } void FrogPilotControlsPanel::updateCarToggles() { + steerRatioStock = params.getFloat("SteerRatioStock"); + + if (steerRatioStock == 0.0) { + QTimer *timer = new QTimer(this); + timer->setInterval(1000); + connect(timer, &QTimer::timeout, this, [this, timer]() { + steerRatioStock = params.getFloat("SteerRatioStock"); + if (steerRatioStock != 0.0) { + timer->stop(); + timer->deleteLater(); + + FrogPilotParamValueControl *steerRatioToggle = static_cast(toggles["SteerRatio"]); + steerRatioToggle->setTitle(QString("Steer Ratio (Default: %1)").arg(steerRatioStock, 0, 'f', 2)); + steerRatioToggle->updateControl(steerRatioStock * 0.75, steerRatioStock * 1.25, "", 0.01); + steerRatioToggle->refresh(); + } + }); + timer->start(); + } else { + FrogPilotParamValueControl *steerRatioToggle = static_cast(toggles["SteerRatio"]); + steerRatioToggle->setTitle(QString("Steer Ratio (Default: %1)").arg(steerRatioStock, 0, 'f', 2)); + steerRatioToggle->updateControl(steerRatioStock * 0.75, steerRatioStock * 1.25, "", 0.01); + steerRatioToggle->refresh(); + } } void FrogPilotControlsPanel::updateMetric() { diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 1eb389a8..a42355ee 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -42,7 +42,7 @@ private: std::set conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CESlowerLead", "CENavigation", "CEStopLights", "CESignal"}; std::set fireTheBabysitterKeys = {"NoLogging", "MuteOverheated", "NoUploads", "OfflineMode"}; std::set laneChangeKeys = {"LaneChangeTime", "LaneDetection", "LaneDetectionWidth", "OneLaneChange"}; - std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "UseLateralJerk"}; + std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "SteerRatio", "UseLateralJerk"}; std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"}; std::set qolKeys = {"DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset"}; @@ -59,4 +59,6 @@ private: bool isMetric = params.getBool("IsMetric"); bool started = false; + + float steerRatioStock = params.getFloat("SteerRatioStock"); }; diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 56a3a550..de2578fa 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -134,6 +134,11 @@ def main(): CP = msg cloudlog.info("paramsd got CarParams") + steer_ratio_stock = params_reader.get_float("SteerRatioStock") + if steer_ratio_stock != CP.steerRatio: + params_reader.put_float("SteerRatio", CP.steerRatio) + params_reader.put_float("SteerRatioStock", CP.steerRatio) + min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio params = params_reader.get("LiveParameters")