mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-12 21:02:13 +08:00
feat: Add speed-dependent following distance settings
- Introduce 'High' following distance parameters for Aggressive, Standard, and Relaxed driving personalities. - Implement logic to interpolate between low and high speed following distances based on vehicle speed (20-90 kph). - Update UI to include new toggles for configuring high speed following distances. - Refactor longitudinal settings UI code for better readability and structure.
This commit is contained in:
Woohyun Rho
@@ -224,6 +224,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"AdvancedLateralTune", PERSISTENT},
|
||||
{"AdvancedLongitudinalTune", PERSISTENT},
|
||||
{"AggressiveFollow", PERSISTENT},
|
||||
{"AggressiveFollowHigh", PERSISTENT},
|
||||
{"AggressiveJerkAcceleration", PERSISTENT},
|
||||
{"AggressiveJerkDanger", PERSISTENT},
|
||||
{"AggressiveJerkDeceleration", PERSISTENT},
|
||||
@@ -456,6 +457,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"RandomThemes", PERSISTENT},
|
||||
{"RefuseVolume", PERSISTENT},
|
||||
{"RelaxedFollow", PERSISTENT},
|
||||
{"RelaxedFollowHigh", PERSISTENT},
|
||||
{"RelaxedJerkAcceleration", PERSISTENT},
|
||||
{"RelaxedJerkDanger", PERSISTENT},
|
||||
{"RelaxedJerkDeceleration", PERSISTENT},
|
||||
@@ -515,6 +517,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"SpeedLimitsFiltered", PERSISTENT | DONT_LOG},
|
||||
{"SpeedLimitSources", PERSISTENT},
|
||||
{"StandardFollow", PERSISTENT},
|
||||
{"StandardFollowHigh", PERSISTENT},
|
||||
{"StandardJerkAcceleration", PERSISTENT},
|
||||
{"StandardJerkDanger", PERSISTENT},
|
||||
{"StandardJerkDeceleration", PERSISTENT},
|
||||
|
||||
@@ -129,6 +129,7 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [
|
||||
("AdvancedLateralTune", "0", 2, "0"),
|
||||
("AdvancedLongitudinalTune", "0", 3, "0"),
|
||||
("AggressiveFollow", "1.25", 2, "1.25"),
|
||||
("AggressiveFollowHigh", "1.25", 2, "1.25"),
|
||||
("AggressiveJerkAcceleration", "50", 3, "50"),
|
||||
("AggressiveJerkDanger", "100", 3, "100"),
|
||||
("AggressiveJerkDeceleration", "50", 3, "50"),
|
||||
@@ -329,6 +330,7 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [
|
||||
("RecordFront", "0", 0, "0"),
|
||||
("RefuseVolume", "101", 2, "101"),
|
||||
("RelaxedFollow", "1.75", 2, "1.75"),
|
||||
("RelaxedFollowHigh", "2.25", 2, "2.25"),
|
||||
("RelaxedJerkAcceleration", "50", 3, "50"),
|
||||
("RelaxedJerkDanger", "100", 3, "100"),
|
||||
("RelaxedJerkDeceleration", "50", 3, "50"),
|
||||
@@ -386,6 +388,7 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [
|
||||
("StartupMessageBottom", "Human-tested, frog-approved 🐸", 0, "Always keep hands on wheel and eyes on road"),
|
||||
("StartupMessageTop", "Hop in and buckle up!", 0, "Be ready to take over at any time"),
|
||||
("StandardFollow", "1.45", 2, "1.45"),
|
||||
("StandardFollowHigh", "2.15", 2, "2.15"),
|
||||
("StandardJerkAcceleration", "50", 3, "50"),
|
||||
("StandardJerkDanger", "100", 3, "100"),
|
||||
("StandardJerkDeceleration", "50", 3, "50"),
|
||||
@@ -676,28 +679,31 @@ class FrogPilotVariables:
|
||||
toggle.aggressive_jerk_danger = np.clip(params.get_int("AggressiveJerkDanger") / 100, 0.25, 2) if aggressive_profile and tuning_level >= level["AggressiveJerkDanger"] else default.get_int("AggressiveJerkDanger") / 100
|
||||
toggle.aggressive_jerk_speed = np.clip(params.get_int("AggressiveJerkSpeed") / 100, 0.25, 2) if aggressive_profile and tuning_level >= level["AggressiveJerkSpeed"] else default.get_int("AggressiveJerkSpeed") / 100
|
||||
toggle.aggressive_jerk_speed_decrease = np.clip(params.get_int("AggressiveJerkSpeedDecrease") / 100, 0.25, 2) if aggressive_profile and tuning_level >= level["AggressiveJerkSpeedDecrease"] else default.get_int("AggressiveJerkSpeedDecrease") / 100
|
||||
toggle.aggressive_follow = np.clip(params.get_float("AggressiveFollow"), 1, MAX_T_FOLLOW) if aggressive_profile and tuning_level >= level["AggressiveFollow"] else default.get_float("AggressiveFollow")
|
||||
toggle.aggressive_follow = [np.clip(params.get_float("AggressiveFollow"), 1, MAX_T_FOLLOW) if aggressive_profile and tuning_level >= level["AggressiveFollow"] else default.get_float("AggressiveFollow"),
|
||||
np.clip(params.get_float("AggressiveFollowHigh"), 1, MAX_T_FOLLOW) if aggressive_profile and tuning_level >= level["AggressiveFollowHigh"] else default.get_float("AggressiveFollowHigh")]
|
||||
standard_profile = toggle.custom_personalities and (params.get_bool("StandardPersonalityProfile") if tuning_level >= level["StandardPersonalityProfile"] else default.get_bool("StandardPersonalityProfile"))
|
||||
toggle.standard_jerk_acceleration = np.clip(params.get_int("StandardJerkAcceleration") / 100, 0.25, 2) if standard_profile and tuning_level >= level["StandardJerkAcceleration"] else default.get_int("StandardJerkAcceleration") / 100
|
||||
toggle.standard_jerk_deceleration = np.clip(params.get_int("StandardJerkDeceleration") / 100, 0.25, 2) if standard_profile and tuning_level >= level["StandardJerkDeceleration"] else default.get_int("StandardJerkDeceleration") / 100
|
||||
toggle.standard_jerk_danger = np.clip(params.get_int("StandardJerkDanger") / 100, 0.25, 2) if standard_profile and tuning_level >= level["StandardJerkDanger"] else default.get_int("StandardJerkDanger") / 100
|
||||
toggle.standard_jerk_speed = np.clip(params.get_int("StandardJerkSpeed") / 100, 0.25, 2) if standard_profile and tuning_level >= level["StandardJerkSpeed"] else default.get_int("StandardJerkSpeed") / 100
|
||||
toggle.standard_jerk_speed_decrease = np.clip(params.get_int("StandardJerkSpeedDecrease") / 100, 0.25, 2) if standard_profile and tuning_level >= level["StandardJerkSpeedDecrease"] else default.get_int("StandardJerkSpeedDecrease") / 100
|
||||
toggle.standard_follow = np.clip(params.get_float("StandardFollow"), 1, MAX_T_FOLLOW) if standard_profile and tuning_level >= level["StandardFollow"] else default.get_float("StandardFollow")
|
||||
toggle.standard_follow = [np.clip(params.get_float("StandardFollow"), 1, MAX_T_FOLLOW) if standard_profile and tuning_level >= level["StandardFollow"] else default.get_float("StandardFollow"),
|
||||
np.clip(params.get_float("StandardFollowHigh"), 1, MAX_T_FOLLOW) if standard_profile and tuning_level >= level["StandardFollowHigh"] else default.get_float("StandardFollowHigh")]
|
||||
relaxed_profile = toggle.custom_personalities and (params.get_bool("RelaxedPersonalityProfile") if tuning_level >= level["RelaxedPersonalityProfile"] else default.get_bool("RelaxedPersonalityProfile"))
|
||||
toggle.relaxed_jerk_acceleration = np.clip(params.get_int("RelaxedJerkAcceleration") / 100, 0.25, 2) if relaxed_profile and tuning_level >= level["RelaxedJerkAcceleration"] else default.get_int("RelaxedJerkAcceleration") / 100
|
||||
toggle.relaxed_jerk_deceleration = np.clip(params.get_int("RelaxedJerkDeceleration") / 100, 0.25, 2) if relaxed_profile and tuning_level >= level["RelaxedJerkDeceleration"] else default.get_int("RelaxedJerkDeceleration") / 100
|
||||
toggle.relaxed_jerk_danger = np.clip(params.get_int("RelaxedJerkDanger") / 100, 0.25, 2) if relaxed_profile and tuning_level >= level["RelaxedJerkDanger"] else default.get_int("RelaxedJerkDanger") / 100
|
||||
toggle.relaxed_jerk_speed = np.clip(params.get_int("RelaxedJerkSpeed") / 100, 0.25, 2) if relaxed_profile and tuning_level >= level["RelaxedJerkSpeed"] else default.get_int("RelaxedJerkSpeed") / 100
|
||||
toggle.relaxed_jerk_speed_decrease = np.clip(params.get_int("RelaxedJerkSpeedDecrease") / 100, 0.25, 2) if relaxed_profile and tuning_level >= level["RelaxedJerkSpeedDecrease"] else default.get_int("RelaxedJerkSpeedDecrease") / 100
|
||||
toggle.relaxed_follow = np.clip(params.get_float("RelaxedFollow"), 1, MAX_T_FOLLOW) if relaxed_profile and tuning_level >= level["RelaxedFollow"] else default.get_float("RelaxedFollow")
|
||||
toggle.relaxed_follow = [np.clip(params.get_float("RelaxedFollow"), 1, MAX_T_FOLLOW) if relaxed_profile and tuning_level >= level["RelaxedFollow"] else default.get_float("RelaxedFollow"),
|
||||
np.clip(params.get_float("RelaxedFollowHigh"), 1, MAX_T_FOLLOW) if relaxed_profile and tuning_level >= level["RelaxedFollowHigh"] else default.get_float("RelaxedFollowHigh")]
|
||||
traffic_profile = toggle.custom_personalities and (params.get_bool("TrafficPersonalityProfile") if tuning_level >= level["TrafficPersonalityProfile"] else default.get_bool("TrafficPersonalityProfile"))
|
||||
toggle.traffic_mode_jerk_acceleration = [np.clip(params.get_int("TrafficJerkAcceleration") / 100, 0.25, 2) if traffic_profile and tuning_level >= level["TrafficJerkAcceleration"] else default.get_int("TrafficJerkAcceleration") / 100, toggle.aggressive_jerk_acceleration]
|
||||
toggle.traffic_mode_jerk_deceleration = [np.clip(params.get_int("TrafficJerkDeceleration") / 100, 0.25, 2) if traffic_profile and tuning_level >= level["TrafficJerkDeceleration"] else default.get_int("TrafficJerkDeceleration") / 100, toggle.aggressive_jerk_deceleration]
|
||||
toggle.traffic_mode_jerk_danger = [np.clip(params.get_int("TrafficJerkDanger") / 100, 0.25, 2) if traffic_profile and tuning_level >= level["TrafficJerkDanger"] else default.get_int("TrafficJerkDanger") / 100, toggle.aggressive_jerk_danger]
|
||||
toggle.traffic_mode_jerk_speed = [np.clip(params.get_int("TrafficJerkSpeed") / 100, 0.25, 2) if traffic_profile and tuning_level >= level["TrafficJerkSpeed"] else default.get_int("TrafficJerkSpeed") / 100, toggle.aggressive_jerk_speed]
|
||||
toggle.traffic_mode_jerk_speed_decrease = [np.clip(params.get_int("TrafficJerkSpeedDecrease") / 100, 0.25, 2) if traffic_profile and tuning_level >= level["TrafficJerkSpeedDecrease"] else default.get_int("TrafficJerkSpeedDecrease") / 100, toggle.aggressive_jerk_speed_decrease]
|
||||
toggle.traffic_mode_follow = [np.clip(params.get_float("TrafficFollow"), 0.5, MAX_T_FOLLOW) if traffic_profile and tuning_level >= level["TrafficFollow"] else default.get_float("TrafficFollow"), toggle.aggressive_follow]
|
||||
toggle.traffic_mode_follow = [np.clip(params.get_float("TrafficFollow"), 0.5, MAX_T_FOLLOW) if traffic_profile and tuning_level >= level["TrafficFollow"] else default.get_float("TrafficFollow"), toggle.aggressive_follow[0]]
|
||||
|
||||
custom_ui = params.get_bool("CustomUI") if tuning_level >= level["CustomUI"] else default.get_bool("CustomUI")
|
||||
toggle.acceleration_path = toggle.openpilot_longitudinal and (custom_ui and (params.get_bool("AccelerationPath") if tuning_level >= level["AccelerationPath"] else default.get_bool("AccelerationPath")) or toggle.debug_mode)
|
||||
|
||||
@@ -1,11 +1,13 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
|
||||
|
||||
from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT
|
||||
|
||||
TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
|
||||
PERSONALITY_BP = [20. * CV.KPH_TO_MS, 90. * CV.KPH_TO_MS]
|
||||
|
||||
class FrogPilotFollowing:
|
||||
def __init__(self, FrogPilotPlanner):
|
||||
@@ -48,12 +50,16 @@ class FrogPilotFollowing:
|
||||
frogpilot_toggles.custom_personalities, sm["controlsState"].personality
|
||||
)
|
||||
|
||||
self.t_follow = get_T_FOLLOW(
|
||||
t_follow_param = get_T_FOLLOW(
|
||||
frogpilot_toggles.aggressive_follow,
|
||||
frogpilot_toggles.standard_follow,
|
||||
frogpilot_toggles.relaxed_follow,
|
||||
frogpilot_toggles.custom_personalities, sm["controlsState"].personality
|
||||
)
|
||||
if isinstance(t_follow_param, list):
|
||||
self.t_follow = float(np.interp(v_ego, PERSONALITY_BP, t_follow_param))
|
||||
else:
|
||||
self.t_follow = float(t_follow_param)
|
||||
else:
|
||||
self.base_acceleration_jerk = 0
|
||||
self.base_danger_jerk = 0
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -41,18 +41,18 @@ private:
|
||||
std::map<QString, AbstractControl*> toggles;
|
||||
|
||||
QSet<QString> advancedLongitudinalTuneKeys = {"LongitudinalActuatorDelay", "StartAccel", "StopAccel", "StoppingDecelRate", "VEgoStarting", "VEgoStopping"};
|
||||
QSet<QString> aggressivePersonalityKeys = {"AggressiveFollow", "AggressiveJerkAcceleration", "AggressiveJerkDeceleration", "AggressiveJerkDanger", "AggressiveJerkSpeed", "AggressiveJerkSpeedDecrease", "ResetAggressivePersonality"};
|
||||
QSet<QString> aggressivePersonalityKeys = {"AggressiveFollow", "AggressiveFollowHigh", "AggressiveJerkAcceleration", "AggressiveJerkDeceleration", "AggressiveJerkDanger", "AggressiveJerkSpeed", "AggressiveJerkSpeedDecrease", "ResetAggressivePersonality"};
|
||||
QSet<QString> conditionalExperimentalKeys = {"CESpeed", "CESpeedLead", "CECurves", "CELead", "CEModelStopTime", "CENavigation", "CESignalSpeed", "ShowCEMStatus"};
|
||||
QSet<QString> curveSpeedKeys = {"CalibratedLateralAcceleration", "CalibrationProgress", "ResetCurveData", "ShowCSCStatus"};
|
||||
QSet<QString> customDrivingPersonalityKeys = {"AggressivePersonalityProfile", "RelaxedPersonalityProfile", "StandardPersonalityProfile", "TrafficPersonalityProfile"};
|
||||
QSet<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "HumanFollowing", "LeadDetectionThreshold", "MaxDesiredAcceleration", "TacoTune"};
|
||||
QSet<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset"};
|
||||
QSet<QString> relaxedPersonalityKeys = {"RelaxedFollow", "RelaxedJerkAcceleration", "RelaxedJerkDeceleration", "RelaxedJerkDanger", "RelaxedJerkSpeed", "RelaxedJerkSpeedDecrease", "ResetRelaxedPersonality"};
|
||||
QSet<QString> relaxedPersonalityKeys = {"RelaxedFollow", "RelaxedFollowHigh", "RelaxedJerkAcceleration", "RelaxedJerkDeceleration", "RelaxedJerkDanger", "RelaxedJerkSpeed", "RelaxedJerkSpeedDecrease", "ResetRelaxedPersonality"};
|
||||
QSet<QString> speedLimitControllerKeys = {"SLCOffsets", "SLCFallback", "SLCOverride", "SLCPriority", "SLCQOL", "SLCVisuals"};
|
||||
QSet<QString> speedLimitControllerOffsetsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "Offset5", "Offset6", "Offset7"};
|
||||
QSet<QString> speedLimitControllerQOLKeys = {"ForceMPHDashboard", "SetSpeedLimit", "SLCConfirmation", "SLCLookaheadHigher", "SLCLookaheadLower", "SLCMapboxFiller"};
|
||||
QSet<QString> speedLimitControllerVisualKeys = {"ShowSLCOffset", "SpeedLimitSources"};
|
||||
QSet<QString> standardPersonalityKeys = {"StandardFollow", "StandardJerkAcceleration", "StandardJerkDeceleration", "StandardJerkDanger", "StandardJerkSpeed", "StandardJerkSpeedDecrease", "ResetStandardPersonality"};
|
||||
QSet<QString> standardPersonalityKeys = {"StandardFollow", "StandardFollowHigh", "StandardJerkAcceleration", "StandardJerkDeceleration", "StandardJerkDanger", "StandardJerkSpeed", "StandardJerkSpeedDecrease", "ResetStandardPersonality"};
|
||||
QSet<QString> trafficPersonalityKeys = {"TrafficFollow", "TrafficJerkAcceleration", "TrafficJerkDeceleration", "TrafficJerkDanger", "TrafficJerkSpeed", "TrafficJerkSpeedDecrease", "ResetTrafficPersonality"};
|
||||
|
||||
QSet<QString> parentKeys;
|
||||
|
||||
Reference in New Issue
Block a user