mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-23 20:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'dynamic_personality' into master-dev-c3

Browse Source 
# Conflicts:
#	cereal/custom.capnp
#	selfdrive/controls/controlsd.py
This commit is contained in:
Jason Wen
2024-07-01 16:24:36 -04:00
parent a268ba67c2 9ee63438ef
commit 2a3a533d2b
8 changed files with 42 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/custom.capnp
+2 -1
View File
@@ -28,7 +28,8 @@ enum AccelerationProfile {
struct ControlsStateSP @0x81c2f05a394cf4af {
lateralState @0 :Text;
personality @8 :LongitudinalPersonalitySP;
accelProfile @9 :AccelerationProfile;
dynamicPersonality @9 :Bool;
accelProfile @10 :AccelerationProfile;
lateralControlState :union {
indiState @1 :LateralINDIState;
common/params.cc
+1
View File
@@ -245,6 +245,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"DrivingModelUrl", PERSISTENT},
{"DynamicExperimentalControl", PERSISTENT | BACKUP},
{"DynamicLaneProfile", PERSISTENT | BACKUP},
{"DynamicPersonality", PERSISTENT | BACKUP},
{"EnableAmap", PERSISTENT | BACKUP},
{"EnableGmap", PERSISTENT | BACKUP},
{"EnableMads", PERSISTENT | BACKUP},
selfdrive/controls/controlsd.py
+4
View File
@@ -188,6 +188,8 @@ class Controls:
model_capabilities = ModelCapabilities.get_by_gen(self.model_gen)
self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution
self.dynamic_personality = self.params.get_bool("DynamicPersonality")
self.accel_profile = self.read_accel_profile_param()
self.can_log_mono_time = 0
@@ -861,6 +863,7 @@ class Controls:
controlsStateSP.lateralState = lat_tuning
controlsStateSP.personality = self.personality
controlsStateSP.dynamicPersonality = self.dynamic_personality
controlsStateSP.accelProfile = self.accel_profile
if self.enable_nnff and lat_tuning == 'torque':
@@ -922,6 +925,7 @@ class Controls:
self.experimental_mode = self.params.get_bool("ExperimentalMode") and (self.CP.openpilotLongitudinalControl or
(not self.CP.pcmCruiseSpeed and self.custom_stock_planner_speed))
self.personality = self.read_personality_param()
self.dynamic_personality = self.params.get_bool("DynamicPersonality")
self.accel_profile = self.read_accel_profile_param()
if self.CP.notCar:
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+24 -4
View File
@@ -63,9 +63,9 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
elif personality==custom.LongitudinalPersonalitySP.standard:
return 1.0
elif personality==custom.LongitudinalPersonalitySP.moderate:
return 0.5
return 0.8
elif personality==custom.LongitudinalPersonalitySP.aggressive:
return 0.222
return 0.6
else:
raise NotImplementedError("Longitudinal personality not supported")
@@ -82,6 +82,26 @@ def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard):
else:
raise NotImplementedError("Longitudinal personality not supported")
def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.standard):
if personality==custom.LongitudinalPersonalitySP.relaxed:
x_vel = [0, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01]
y_dist = [1.5, 1.5, 1.5, 1.6, 1.76, 1.76, 1.78, 1.78, 1.8, 1.8]
elif personality==custom.LongitudinalPersonalitySP.standard:
x_vel = [0, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01]
y_dist = [1.40, 1.40, 1.40, 1.50, 1.60, 1.76, 1.76, 1.78, 1.8, 1.8]
elif personality==custom.LongitudinalPersonalitySP.moderate:
x_vel = [0, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01]
y_dist = [1.3, 1.3, 1.3, 1.35, 1.35, 1.385, 1.385, 1.4, 1.4, 1.45]
elif personality==custom.LongitudinalPersonalitySP.aggressive:
x_vel = [0, 5, 5.01, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01]
y_dist = [1.12, 1.12, 1.12, 1.12, 1.12, 1.105, 1.105, 1.15, 1.15, 1.18, 1.20, 1.23]
else:
raise NotImplementedError("Dynamic personality not supported")
return np.interp(v_ego, x_vel, y_dist)
def get_stopped_equivalence_factor(v_lead):
return (v_lead**2) / (2 * COMFORT_BRAKE)
@@ -338,9 +358,9 @@ class LongitudinalMpc:
self.cruise_min_a = min_a
self.max_a = max_a
def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard):
t_follow = get_T_FOLLOW(personality)
def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard, dynamic_personality=False):
v_ego = self.x0[1]
t_follow = get_dynamic_personality(v_ego, personality) if dynamic_personality else get_T_FOLLOW(personality)
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
lead_xv_0 = self.process_lead(radarstate.leadOne)
selfdrive/controls/lib/longitudinal_planner.py
+1 -1
View File
@@ -195,7 +195,7 @@ class LongitudinalPlanner:
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)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsStateSP'].personality)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsStateSP'].personality, dynamic_personality=sm['controlsStateSP'].dynamicPersonality)
self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
selfdrive/ui/qt/offroad/settings.cc
+8 -2
View File
@@ -61,6 +61,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
tr("Enable toggle to allow the model to determine when to use openpilot ACC or openpilot End to End Longitudinal."),
"../assets/offroad/icon_blank.png",
},
{
"DynamicPersonality",
tr("Enable Dynamic Personality"),
tr("Enable this to allow sunnypilot to dynamically adjust following distance and reaction based on your \"Driving Personality\" setting."),
"../assets/offroad/icon_blank.png",
},
{
"DisengageOnAccelerator",
tr("Disengage on Accelerator Pedal"),
@@ -114,9 +120,9 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
};
std::vector<QString> longi_button_texts{tr("Maniac"), tr("Aggressive"), tr("Stock"), tr("Relaxed")};
std::vector<QString> longi_button_texts{tr("Aggressive"), tr("Moderate"), tr("Standard"), tr("Relaxed")};
long_personality_setting = new ButtonParamControl("LongitudinalPersonality", tr("Driving Personality"),
tr("Stock is recommended. In aggressive/maniac mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. "
tr("Standard is recommended. In aggressive/moderate mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. "
"In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with "
"your steering wheel distance button."),
"../assets/offroad/icon_blank.png",
selfdrive/ui/ui.h
+1 -1
View File
@@ -27,7 +27,7 @@ const int UI_ROAD_NAME_MARGIN_X = 14;
struct FeatureStatusText {
const QStringList dlp_list_text = { "Laneful", "Laneless", "Auto" };
const QStringList gac_list_text = { "Maniac", "Aggressive", "Standard", "Relaxed" };
const QStringList gac_list_text = { "Aggressive", "Moderate", "Standard", "Relaxed" };
const QStringList slc_list_text = { "Inactive", "Temp Off", "Adapting", "Active", "Pre Active" };
};
system/manager/manager.py
+1
View File
@@ -64,6 +64,7 @@ def manager_init() -> None:
("DisengageLateralOnBrake", "0"),
("DrivingModelGeneration", "0"),
("DynamicLaneProfile", "1"),
("DynamicPersonality", "0"),
("EnableMads", "1"),
("EnhancedScc", "0"),
("FeatureStatus", "1"),