diff --git a/common/params.cc b/common/params.cc index a4811bed5..befabfba5 100644 --- a/common/params.cc +++ b/common/params.cc @@ -212,6 +212,8 @@ std::unordered_map keys = { {"AccelerationPath", PERSISTENT}, {"AccelerationProfile", PERSISTENT}, {"AggressiveAcceleration", PERSISTENT}, + {"AggressiveFollow", PERSISTENT}, + {"AggressiveJerk", PERSISTENT}, {"AlertVolumeControl", PERSISTENT}, {"AlwaysOnLateral", PERSISTENT}, {"AlwaysOnLateralMain", PERSISTENT}, @@ -238,6 +240,7 @@ std::unordered_map keys = { {"CustomColors", PERSISTENT}, {"CustomIcons", PERSISTENT}, {"CustomPaths", PERSISTENT}, + {"CustomPersonalities", PERSISTENT}, {"CustomUI", PERSISTENT}, {"CustomSignals", PERSISTENT}, {"CustomSounds", PERSISTENT}, @@ -265,7 +268,11 @@ std::unordered_map keys = { {"QOLControls", PERSISTENT}, {"QOLVisuals", PERSISTENT}, {"RefuseVolume", PERSISTENT}, + {"RelaxedFollow", PERSISTENT}, + {"RelaxedJerk", PERSISTENT}, {"RoadEdgesWidth", PERSISTENT}, + {"StandardFollow", PERSISTENT}, + {"StandardJerk", PERSISTENT}, {"StockTune", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"WarningImmediateVolume", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 1902b08a2..87b5f59ce 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -58,26 +58,46 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 -def get_jerk_factor(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: - return 1.0 - elif personality==log.LongitudinalPersonality.standard: - return 1.0 - elif personality==log.LongitudinalPersonality.aggressive: - return 0.5 +def get_jerk_factor(custom_personalities=False, aggressive_jerk=0.5, standard_jerk=1.0, relaxed_jerk=1.0, personality=log.LongitudinalPersonality.standard): + if custom_personalities: + if personality==log.LongitudinalPersonality.relaxed: + return relaxed_jerk + elif personality==log.LongitudinalPersonality.standard: + return standard_jerk + elif personality==log.LongitudinalPersonality.aggressive: + return aggressive_jerk + else: + raise NotImplementedError("Longitudinal personality not supported") else: - raise NotImplementedError("Longitudinal personality not supported") + if personality==log.LongitudinalPersonality.relaxed: + return 1.0 + elif personality==log.LongitudinalPersonality.standard: + return 1.0 + elif personality==log.LongitudinalPersonality.aggressive: + return 0.5 + else: + raise NotImplementedError("Longitudinal personality not supported") -def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: - return 1.75 - elif personality==log.LongitudinalPersonality.standard: - return 1.45 - elif personality==log.LongitudinalPersonality.aggressive: - return 1.25 +def get_T_FOLLOW(custom_personalities=False, aggressive_follow=1.25, standard_follow=1.45, relaxed_follow=1.75, personality=log.LongitudinalPersonality.standard): + if custom_personalities: + if personality==log.LongitudinalPersonality.relaxed: + return relaxed_follow + elif personality==log.LongitudinalPersonality.standard: + return standard_follow + elif personality==log.LongitudinalPersonality.aggressive: + return aggressive_follow + else: + raise NotImplementedError("Longitudinal personality not supported") else: - raise NotImplementedError("Longitudinal personality not supported") + if personality==log.LongitudinalPersonality.relaxed: + return 1.75 + elif personality==log.LongitudinalPersonality.standard: + return 1.45 + elif personality==log.LongitudinalPersonality.aggressive: + return 1.25 + else: + raise NotImplementedError("Longitudinal personality not supported") def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_custom.png b/selfdrive/frogpilot/assets/toggle_icons/icon_custom.png new file mode 100644 index 000000000..f1c7f4d91 Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_custom.png differ diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 2008d93e1..17d021dfc 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -90,8 +90,8 @@ class FrogPilotPlanner: road_curvature = calculate_road_curvature(modelData, v_ego) if radarState.leadOne.status and self.CP.openpilotLongitudinalControl: - base_jerk = get_jerk_factor(controlsState.personality) - base_t_follow = get_T_FOLLOW(controlsState.personality) + base_jerk = get_jerk_factor(self.custom_personalities, self.aggressive_jerk, self.standard_jerk, self.relaxed_jerk, controlsState.personality) + base_t_follow = get_T_FOLLOW(self.custom_personalities, self.aggressive_follow, self.standard_follow, self.relaxed_follow, controlsState.personality) self.jerk, self.t_follow = self.update_follow_values(base_jerk, radarState, base_t_follow, v_ego, v_lead) else: self.t_follow = 1.45 @@ -158,6 +158,14 @@ class FrogPilotPlanner: custom_alerts = self.params.get_bool("CustomAlerts") + self.custom_personalities = self.params.get_bool("CustomPersonalities") + self.aggressive_jerk = self.params.get_float("AggressiveJerk") + self.aggressive_follow = self.params.get_float("AggressiveFollow") + self.standard_jerk = self.params.get_float("StandardJerk") + self.standard_follow = self.params.get_float("StandardFollow") + self.relaxed_jerk = self.params.get_float("RelaxedJerk") + self.relaxed_follow = self.params.get_float("RelaxedFollow") + custom_ui = self.params.get_bool("CustomUI") self.blind_spot_path = custom_ui and self.params.get_bool("BlindSpotPath") diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index aa04731d0..3f64fec4d 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -18,6 +18,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"CESignal", tr("Turn Signal When Below Highway Speeds"), tr("Switch to 'Experimental Mode' when using turn signals below highway speeds to help assist with turns."), ""}, {"HideCEMStatusBar", tr("Hide the Status Bar"), tr("Don't use the status bar for 'Conditional Experimental Mode'."), ""}, + {"CustomPersonalities", tr("Custom Driving Personalities"), tr("Customize the driving personality profiles to your driving style."), "../frogpilot/assets/toggle_icons/icon_custom.png"}, + {"LateralTune", tr("Lateral Tuning"), tr("Modify openpilot's steering behavior."), "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"LongitudinalTune", tr("Longitudinal Tuning"), tr("Modify openpilot's acceleration and braking behavior."), "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, @@ -81,6 +83,58 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil std::vector stopLightToggleNames{tr("With Lead")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, stopLightToggles, stopLightToggleNames); + } else if (param == "CustomPersonalities") { + FrogPilotParamManageControl *customPersonalitiesToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(customPersonalitiesToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + openParentToggle(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(false); + } + aggressiveProfile->setVisible(true); + standardProfile->setVisible(true); + relaxedProfile->setVisible(true); + }); + toggle = customPersonalitiesToggle; + + FrogPilotParamValueControl *aggressiveFollow = new FrogPilotParamValueControl("AggressiveFollow", tr("Follow"), + tr("Set the 'Aggressive' personality' following distance. " + "Represents seconds to follow behind the lead vehicle.\n\nStock: 1.25 seconds."), + "../frogpilot/assets/other_images/aggressive.png", + 1, 5, std::map(), this, false, tr(" sec"), 1, 0.01); + FrogPilotParamValueControl *aggressiveJerk = new FrogPilotParamValueControl("AggressiveJerk", tr("Jerk"), + tr("Configure brake/gas pedal responsiveness for the 'Aggressive' personality. " + "Higher jerk value = Less likely to use the gas/brake.\nLower jerk value = More likely.\n\nStock: 0.5."), + "", + 0.01, 5, std::map(), this, false, "", 1, 0.01); + aggressiveProfile = new FrogPilotDualParamControl(aggressiveFollow, aggressiveJerk, this, true); + addItem(aggressiveProfile); + + FrogPilotParamValueControl *standardFollow = new FrogPilotParamValueControl("StandardFollow", tr("Follow"), + tr("Set the 'Standard' personality following distance. " + "Represents seconds to follow behind the lead vehicle.\n\nStock: 1.45 seconds."), + "../frogpilot/assets/other_images/standard.png", + 1, 5, std::map(), this, false, tr(" sec"), 1, 0.01); + FrogPilotParamValueControl *standardJerk = new FrogPilotParamValueControl("StandardJerk", tr("Jerk"), + tr("Adjust brake/gas pedal responsiveness for the 'Standard' personality. " + "Higher jerk value = Less likely to use the gas/brake.\nLower jerk value = More likely.\n\nStock: 1.0."), + "", + 0.01, 5, std::map(), this, false, "", 1, 0.01); + standardProfile = new FrogPilotDualParamControl(standardFollow, standardJerk, this, true); + addItem(standardProfile); + + FrogPilotParamValueControl *relaxedFollow = new FrogPilotParamValueControl("RelaxedFollow", tr("Follow"), + tr("Set the 'Relaxed' personality following distance. " + "Represents seconds to follow behind the lead vehicle.\n\nStock: 1.75 seconds."), + "../frogpilot/assets/other_images/relaxed.png", + 1, 5, std::map(), this, false, tr(" sec"), 1, 0.01); + FrogPilotParamValueControl *relaxedJerk = new FrogPilotParamValueControl("RelaxedJerk", tr("Jerk"), + tr("Set brake/gas pedal responsiveness for the 'Relaxed' personality. " + "Higher jerk value = Less likely to use the gas/brake.\nLower jerk value = More likely.\n\nStock: 1.0."), + "", + 0.01, 5, std::map(), this, false, "", 1, 0.01); + relaxedProfile = new FrogPilotDualParamControl(relaxedFollow, relaxedJerk, this, true); + addItem(relaxedProfile); + } else if (param == "LateralTune") { FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { @@ -252,10 +306,13 @@ void FrogPilotControlsPanel::updateMetric() { } void FrogPilotControlsPanel::hideToggles() { + aggressiveProfile->setVisible(false); conditionalSpeedsImperial->setVisible(false); conditionalSpeedsMetric->setVisible(false); + standardProfile->setVisible(false); + relaxedProfile->setVisible(false); - std::set longitudinalKeys = {"ConditionalExperimental", "LongitudinalTune"}; + std::set longitudinalKeys = {"ConditionalExperimental", "CustomPersonalities", "LongitudinalTune"}; for (auto &[key, toggle] : toggles) { toggle->setVisible(false); diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index 923b2cad7..348f434c0 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -25,8 +25,11 @@ private: void updateState(); void updateToggles(); + FrogPilotDualParamControl *aggressiveProfile; FrogPilotDualParamControl *conditionalSpeedsImperial; FrogPilotDualParamControl *conditionalSpeedsMetric; + FrogPilotDualParamControl *standardProfile; + FrogPilotDualParamControl *relaxedProfile; std::set aolKeys = {"AlwaysOnLateralMain", "HideAOLStatusBar", "PauseAOLOnBrake"}; std::set conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CENavigation", "CESignal", "CESlowerLead", "CEStopLights", "HideCEMStatusBar"};