This commit is contained in:
rav4kumar
2025-07-21 21:26:32 -07:00
parent 7e03277962
commit 1cd9c8cd3c
12 changed files with 454 additions and 6 deletions
+6
View File
@@ -150,6 +150,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
aTarget @5 :Float32;
events @6 :List(OnroadEventSP.Event);
e2eAlerts @7 :E2eAlerts;
accelPersonality @8 :AccelerationPersonality;
struct DynamicExperimentalControl {
state @0 :DynamicExperimentalControlState;
@@ -252,6 +253,11 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
greenLightAlert @0 :Bool;
leadDepartAlert @1 :Bool;
}
enum AccelerationPersonality {
sport @0;
normal @1;
eco @2;
}
}
struct OnroadEventSP @0xda96579883444c35 {
+3
View File
@@ -171,6 +171,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
{"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
{"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
{"VibePersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
{"VibeAccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
{"VibeFollowPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
// MADS params
{"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -10,6 +10,8 @@ from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.modeld.constants import index_function
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
from openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality import VibePersonalityController
if __name__ == '__main__': # generating code
from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
else:
@@ -228,6 +230,7 @@ class LongitudinalMpc:
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.reset()
self.source = SOURCES[2]
self.vibe_controller = VibePersonalityController()
def reset(self):
# self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
@@ -328,10 +331,31 @@ class LongitudinalMpc:
return lead_xv
def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
t_follow = get_T_FOLLOW(personality)
v_ego = self.x0[1]
# Get following distance
if self.vibe_controller.is_follow_enabled():
t_follow = self.vibe_controller.get_follow_distance_multiplier(v_ego)
if t_follow is None:
# Fallback to stock behavior when vibe controller can't provide a value
t_follow = get_T_FOLLOW(personality)
else:
t_follow = get_T_FOLLOW(personality)
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
# Get acceleration limits
if self.vibe_controller.is_accel_enabled():
accel_limits = self.vibe_controller.get_accel_limits(v_ego)
if accel_limits is not None:
min_accel = accel_limits[0]
else:
min_accel = CRUISE_MIN_ACCEL
else:
min_accel = CRUISE_MIN_ACCEL
a_cruise_min = min_accel
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
@@ -350,7 +374,7 @@ class LongitudinalMpc:
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
v_lower = v_ego + (T_IDXS * a_cruise_min * 1.05)
# TODO does this make sense when max_a is negative?
v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
+16 -3
View File
@@ -124,9 +124,22 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
if mode == 'acc':
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
if self.vibe_controller.is_accel_enabled():
# Only get max acceleration from vibe controller, use default ACCEL_MIN for minimum
accel_limits = self.vibe_controller.get_accel_limits(v_ego)
if accel_limits is not None:
max_accel = accel_limits[1]
accel_clip = [ACCEL_MIN, max_accel]
else:
# Fallback to stock if vibe controller returns None
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
# Recalculate limit turn according to the new max limit
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
else:
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
else:
accel_clip = [ACCEL_MIN, ACCEL_MAX]
+19 -1
View File
@@ -92,7 +92,15 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
"your steering wheel distance button."),
"../assets/icons/speed_limit.png",
longi_button_texts);
// accel controller
std::vector<QString> accel_personality_texts{tr("Sport"), tr("Normal"), tr("Eco")};
accel_personality_setting = new ButtonParamControlSP("AccelPersonality", tr("Acceleration Personality"),
tr("Normal is recommended. In sport mode, sunnypilot will provide aggressive acceleration for a dynamic driving experience. "
"In eco mode, sunnypilot will apply smoother and more relaxed acceleration. On supported cars, you can cycle through these "
"acceleration personality within Onroad Settings on the driving screen."),
"",
accel_personality_texts);
accel_personality_setting->showDescription();
// set up uiState update for personality setting
QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState);
@@ -120,6 +128,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
// insert longitudinal personality after NDOG toggle
if (param == "DisengageOnAccelerator") {
addItem(long_personality_setting);
addItem(accel_personality_setting);
}
}
@@ -140,6 +149,13 @@ void TogglesPanel::updateState(const UIState &s) {
}
uiState()->scene.personality = personality;
}
if (sm.updated("longitudinalPlanSP")) {
auto accel_personality = sm["longitudinalPlanSP"].getLongitudinalPlanSP().getAccelPersonality();
if (accel_personality != s.scene.accel_personality && s.scene.started && isVisible()) {
accel_personality_setting->setCheckedButton(static_cast<int>(accel_personality));
}
uiState()->scene.accel_personality = accel_personality;
}
}
void TogglesPanel::expandToggleDescription(const QString &param) {
@@ -186,10 +202,12 @@ void TogglesPanel::updateToggles() {
experimental_mode_toggle->setEnabled(true);
experimental_mode_toggle->setDescription(e2e_description);
long_personality_setting->setEnabled(true);
accel_personality_setting->setEnabled(true);
} else {
// no long for now
experimental_mode_toggle->setEnabled(false);
long_personality_setting->setEnabled(false);
accel_personality_setting->setEnabled(true);
params.remove("ExperimentalMode");
const QString unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.");
+1
View File
@@ -88,6 +88,7 @@ protected:
Params params;
std::map<std::string, ParamControl*> toggles;
ButtonParamControl *long_personality_setting;
ButtonParamControl *accel_personality_setting;
virtual void updateToggles();
};
@@ -75,6 +75,35 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
main_layout->setCurrentWidget(cruisePanelScreen);
});
// Vibe Personality Controller
vibePersonalityControl = new ParamControlSP("VibePersonalityEnabled",
tr("Vibe Personality Controller"),
tr("Advanced driving personality system with separate controls for acceleration behavior (Eco/Normal/Sport) and following distance/braking (Relaxed/Standard/Aggressive). "
"Customize your driving experience with independent acceleration and distance personalities."),
"../assets/offroad/icon_shell.png");
list->addItem(vibePersonalityControl);
connect(vibePersonalityControl, &ParamControlSP::toggleFlipped, [=]() {
refresh(offroad);
});
// Vibe Acceleration Personality
vibeAccelPersonalityControl = new ParamControlSP("VibeAccelPersonalityEnabled",
tr("Acceleration Personality"),
tr("Controls acceleration behavior: Eco (efficient), Normal (balanced), Sport (responsive). "
"Adjust how aggressively the vehicle accelerates while maintaining smooth operation."),
"../assets/offroad/icon_shell.png");
list->addItem(vibeAccelPersonalityControl);
// Vibe Following Distance Personality
vibeFollowPersonalityControl = new ParamControlSP("VibeFollowPersonalityEnabled",
tr("Following Distance Personality"),
tr("Controls following distance and braking behavior: Relaxed (longer distance, gentler braking), Standard (balanced), Aggressive (shorter distance, firmer braking). "
"Fine-tune your comfort level in traffic situations."),
"../assets/offroad/icon_shell.png");
list->addItem(vibeFollowPersonalityControl);
main_layout->addWidget(cruisePanelScreen);
main_layout->addWidget(speedLimitScreen);
main_layout->setCurrentWidget(cruisePanelScreen);
@@ -135,6 +164,14 @@ void LongitudinalPanel::refresh(bool _offroad) {
intelligentCruiseButtonManagement->toggleFlipped(false);
}
}
bool vibePersonalityEnabled = params.getBool("VibePersonalityEnabled");
if (vibePersonalityEnabled) {
vibeAccelPersonalityControl->setVisible(true);
vibeFollowPersonalityControl->setVisible(true);
} else {
vibeAccelPersonalityControl->setVisible(false);
vibeFollowPersonalityControl->setVisible(false);
}
bool icbm_allowed = intelligent_cruise_button_management_available && !has_longitudinal_control;
intelligentCruiseButtonManagement->setEnabled(icbm_allowed && offroad);
@@ -146,6 +183,13 @@ void LongitudinalPanel::refresh(bool _offroad) {
SmartCruiseControlVision->setEnabled(has_longitudinal_control || icbm_allowed);
SmartCruiseControlMap->setEnabled(has_longitudinal_control || icbm_allowed);
// Vibe Personality controls - always enabled for toggling
vibePersonalityControl->setEnabled(true);
vibeAccelPersonalityControl->setEnabled(true);
vibeFollowPersonalityControl->setEnabled(true);
vibePersonalityControl->refresh();
vibeAccelPersonalityControl->refresh();
vibeFollowPersonalityControl->refresh();
offroad = _offroad;
}
@@ -37,4 +37,8 @@ private:
ParamControl *intelligentCruiseButtonManagement = nullptr;
SpeedLimitSettings *speedLimitScreen;
PushButtonSP *speedLimitSettings;
ParamControlSP *vibePersonalityControl;
ParamControlSP *vibeAccelPersonalityControl;
ParamControlSP *vibeFollowPersonalityControl;
};
+1
View File
@@ -60,6 +60,7 @@ typedef struct UIScene {
cereal::PandaState::PandaType pandaType;
cereal::LongitudinalPersonality personality;
cereal::LongitudinalPlanSP::AccelerationPersonality accel_personality;
float light_sensor = -1;
bool started, ignition, is_metric, recording_audio;
@@ -17,6 +17,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolve
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
from openpilot.sunnypilot.models.helpers import get_active_bundle
from openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality import VibePersonalityController
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
@@ -29,6 +30,7 @@ class LongitudinalPlannerSP:
self.scc = SmartCruiseControl()
self.resolver = SpeedLimitResolver()
self.sla = SpeedLimitAssist(CP)
self.vibe_controller = VibePersonalityController()
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
self.source = LongitudinalPlanSource.cruise
self.e2e_alerts_helper = E2EAlertsHelper()
@@ -81,6 +83,7 @@ class LongitudinalPlannerSP:
self.events_sp.clear()
self.dec.update(sm)
self.e2e_alerts_helper.update(sm, self.events_sp)
self.vibe_controller.update()
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
plan_sp_send = messaging.new_message('longitudinalPlanSP')
@@ -0,0 +1,331 @@
"""
Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from cereal import log, custom
import numpy as np
from openpilot.common.realtime import DT_MDL
from openpilot.common.params import Params
LongPersonality = log.LongitudinalPersonality
AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
# Acceleration Profiles mapped to AccelPersonality (eco/normal/sport)
MAX_ACCEL_PROFILES = {
AccelPersonality.eco: [2.00, 2.00, 1.32, 0.85, .58, .46, .365, .317, .089], # eco
AccelPersonality.normal: [2.00, 2.00, 1.42, 1.10, .65, .56, .43, .36, .12], # normal
AccelPersonality.sport: [2.00, 2.00, 1.52, 1.40, .80, .70, .53, .46, .20], # sport
}
MAX_ACCEL_BREAKPOINTS = [0., 6., 9., 11., 16., 20., 25., 30., 55.]
# Braking profiles mapped to LongPersonality (relaxed/standard/aggressive)
MIN_ACCEL_PROFILES = {
LongPersonality.relaxed: [-1.20, -1.20], # gentler braking
LongPersonality.standard: [-1.30, -1.30], # normal braking
LongPersonality.aggressive: [-1.40, -1.40], # more aggressive braking
}
MIN_ACCEL_BREAKPOINTS = [0., 50.]
# Following Distance Profiles mapped to LongPersonality (relaxed/standard/aggressive)
FOLLOW_DISTANCE_PROFILES = {
LongPersonality.relaxed: {
'x_vel': [0., 19.7, 22.2, 40.],
'y_dist': [1.40, 1.40, 1.65, 1.65] # longer following distance
},
LongPersonality.standard: {
'x_vel': [0., 19.7, 22.2, 40.],
'y_dist': [1.35, 1.35, 1.40, 1.40] # normal following distance
},
LongPersonality.aggressive: {
'x_vel': [0., 19.7, 22.2, 40.],
'y_dist': [1.20, 1.20, 1.30, 1.30] # shorter following distance
}
}
class VibePersonalityController:
"""
Controller for managing separated acceleration and distance controls:
- AccelPersonality controls acceleration behavior (eco, normal, sport)
- LongPersonality controls braking and following distance (relaxed, standard, aggressive)
"""
def __init__(self):
self.params = Params()
self.frame = 0
# Separate personalities for acceleration and distance control
self.accel_personality = AccelPersonality.normal
self.long_personality = LongPersonality.standard
# Parameter keys
self.param_keys = {
'accel_personality': 'AccelPersonality', # eco=0, normal=1, sport=2
'long_personality': 'LongitudinalPersonality', # relaxed=0, standard=1, aggressive=2
'enabled': 'VibePersonalityEnabled',
'accel_enabled': 'VibeAccelPersonalityEnabled',
'follow_enabled': 'VibeFollowPersonalityEnabled'
}
print(f"[VIBE_DEBUG] Initializing VibePersonalityController - accel_personality: {self.accel_personality}, long_personality: {self.long_personality}")
# Precompute slopes for all personalities
self._precompute_slopes()
def _precompute_slopes(self):
"""Precompute all interpolation slopes for efficiency"""
self.max_accel_slopes = {}
self.min_accel_slopes = {}
self.follow_distance_slopes = {}
# Precompute for AccelPersonality (acceleration)
for personality in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
if personality in MAX_ACCEL_PROFILES:
self.max_accel_slopes[personality] = self._compute_slopes(MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[personality])
# Precompute for LongPersonality (braking and following)
for personality in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
if personality in MIN_ACCEL_PROFILES:
self.min_accel_slopes[personality] = self._compute_slopes(MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[personality])
if personality in FOLLOW_DISTANCE_PROFILES:
profile = FOLLOW_DISTANCE_PROFILES[personality]
self.follow_distance_slopes[personality] = self._compute_slopes(profile['x_vel'], profile['y_dist'])
def _update_from_params(self):
"""Update personalities from params (rate limited)"""
if self.frame % int(1. / DT_MDL) != 0:
return
# Update AccelPersonality
try:
accel_personality_str = self.params.get(self.param_keys['accel_personality'], encoding='utf-8')
if accel_personality_str:
accel_personality_int = int(accel_personality_str)
if accel_personality_int in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
if accel_personality_int != self.accel_personality:
print(f"[VIBE_DEBUG] AccelPersonality changed from {self.accel_personality} to {accel_personality_int}")
self.accel_personality = accel_personality_int
except (ValueError, TypeError):
pass
# Update LongPersonality
try:
long_personality_str = self.params.get(self.param_keys['long_personality'], encoding='utf-8')
if long_personality_str:
long_personality_int = int(long_personality_str)
if long_personality_int in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
if long_personality_int != self.long_personality:
print(f"[VIBE_DEBUG] LongPersonality changed from {self.long_personality} to {long_personality_int}")
self.long_personality = long_personality_int
except (ValueError, TypeError):
pass
def _get_toggle_state(self, key: str, default: bool = True) -> bool:
"""Get toggle state with default fallback"""
return self.params.get_bool(self.param_keys.get(key, key)) if key in self.param_keys else default
def _set_toggle_state(self, key: str, value: bool):
"""Set toggle state in params"""
if key in self.param_keys:
self.params.put_bool(self.param_keys[key], value)
# AccelPersonality Management (for acceleration)
def set_accel_personality(self, personality: int) -> bool:
"""Set AccelPersonality (eco=0, normal=1, sport=2)"""
if personality in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
old_personality = self.accel_personality
self.accel_personality = personality
self.params.put(self.param_keys['accel_personality'], str(personality))
print(f"[VIBE_DEBUG] AccelPersonality set: {old_personality} -> {personality}")
return True
return False
def cycle_accel_personality(self) -> int:
"""Cycle through AccelPersonality: eco -> normal -> sport -> eco"""
personalities = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]
current_idx = personalities.index(self.accel_personality)
next_personality = personalities[(current_idx + 1) % len(personalities)]
self.set_accel_personality(next_personality)
return next_personality
def get_accel_personality(self) -> int:
"""Get current AccelPersonality"""
self._update_from_params()
return self.accel_personality
# LongPersonality Management (for braking and following distance)
def set_long_personality(self, personality: int) -> bool:
"""Set LongPersonality (relaxed=0, standard=1, aggressive=2)"""
if personality in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
old_personality = self.long_personality
self.long_personality = personality
self.params.put(self.param_keys['long_personality'], str(personality))
print(f"[VIBE_DEBUG] LongPersonality set: {old_personality} -> {personality}")
return True
return False
def cycle_long_personality(self) -> int:
"""Cycle through LongPersonality: relaxed -> standard -> aggressive -> relaxed"""
personalities = [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]
current_idx = personalities.index(self.long_personality)
next_personality = personalities[(current_idx + 1) % len(personalities)]
self.set_long_personality(next_personality)
return next_personality
def get_long_personality(self) -> int:
"""Get current LongPersonality"""
self._update_from_params()
return self.long_personality
# Toggle Functions
def toggle_personality(self): return self._toggle_flag('enabled')
def toggle_accel_personality(self): return self._toggle_flag('accel_enabled')
def toggle_follow_distance_personality(self): return self._toggle_flag('follow_enabled')
def _toggle_flag(self, key):
current = self._get_toggle_state(key)
self._set_toggle_state(key, not current)
return not current
def set_personality_enabled(self, enabled: bool): self._set_toggle_state('enabled', enabled)
# Feature-specific enable checks
def is_accel_enabled(self) -> bool:
self._update_from_params()
enabled = self._get_toggle_state('enabled') and self._get_toggle_state('accel_enabled')
if enabled:
print(f"[VIBE_DEBUG] is_accel_enabled: TRUE using AccelPersonality {self.accel_personality}")
return enabled
def is_follow_enabled(self) -> bool:
self._update_from_params()
enabled = self._get_toggle_state('enabled') and self._get_toggle_state('follow_enabled')
if enabled:
print(f"[VIBE_DEBUG] is_follow_enabled: TRUE using LongPersonality {self.long_personality}")
return enabled
def is_enabled(self) -> bool:
self._update_from_params()
return (self._get_toggle_state('enabled') and
(self._get_toggle_state('accel_enabled') or self._get_toggle_state('follow_enabled')))
def get_accel_limits(self, v_ego: float) -> tuple[float, float] | None:
"""
Get acceleration limits based on current personalities.
- Max acceleration from AccelPersonality (eco/normal/sport)
- Min acceleration (braking) from LongPersonality (relaxed/standard/aggressive)
Returns None if controller is disabled.
"""
self._update_from_params()
if not self.is_accel_enabled():
return None
try:
print(f"[VIBE_DEBUG] get_accel_limits: AccelPersonality={self.accel_personality}, LongPersonality={self.long_personality}, speed={v_ego:.2f}")
# Max acceleration from AccelPersonality
max_a = self._interpolate(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality],
self.max_accel_slopes[self.accel_personality])
# Min acceleration (braking) from LongPersonality
min_a = self._interpolate(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.long_personality],
self.min_accel_slopes[self.long_personality])
print(f"[VIBE_DEBUG] get_accel_limits: min_a={min_a:.3f} (from LongPersonality), max_a={max_a:.3f} (from AccelPersonality)")
return float(min_a), float(max_a)
except (KeyError, IndexError):
print(f"[VIBE_DEBUG] get_accel_limits: ERROR - KeyError/IndexError")
return None
def get_follow_distance_multiplier(self, v_ego: float) -> float | None:
"""Get following distance multiplier based on LongPersonality only"""
self._update_from_params()
if not self.is_follow_enabled():
return None
try:
print(f"[VIBE_DEBUG] get_follow_distance_multiplier: LongPersonality={self.long_personality}, speed={v_ego:.2f}")
profile = FOLLOW_DISTANCE_PROFILES[self.long_personality]
multiplier = float(self._interpolate(v_ego, profile['x_vel'], profile['y_dist'],
self.follow_distance_slopes[self.long_personality]))
print(f"[VIBE_DEBUG] get_follow_distance_multiplier: multiplier={multiplier:.3f}")
return multiplier
except (KeyError, IndexError):
print(f"[VIBE_DEBUG] get_follow_distance_multiplier: ERROR - KeyError/IndexError")
return None
def get_personality_info(self) -> dict:
"""Get comprehensive info about current personalities and settings"""
self._update_from_params()
accel_names = {AccelPersonality.eco: "Eco", AccelPersonality.normal: "Normal", AccelPersonality.sport: "Sport"}
long_names = {LongPersonality.relaxed: "Relaxed", LongPersonality.standard: "Standard", LongPersonality.aggressive: "Aggressive"}
info = {
"accel_personality": accel_names.get(self.accel_personality, "Unknown"),
"accel_personality_int": self.accel_personality,
"long_personality": long_names.get(self.long_personality, "Unknown"),
"long_personality_int": self.long_personality,
"enabled": self._get_toggle_state('enabled'),
"accel_enabled": self._get_toggle_state('accel_enabled'),
"follow_enabled": self._get_toggle_state('follow_enabled'),
"accel_description": f"Acceleration: {accel_names.get(self.accel_personality, 'Unknown')}",
"long_description": f"Following/Braking: {long_names.get(self.long_personality, 'Unknown')}",
}
print(f"[VIBE_DEBUG] get_personality_info: {info}")
return info
def get_min_accel(self, v_ego: float) -> float | None:
"""Get minimum acceleration (braking) from distance mode"""
limits = self.get_accel_limits(v_ego)
return limits[0] if limits else None
def get_max_accel(self, v_ego: float) -> float | None:
"""Get maximum acceleration from drive mode"""
limits = self.get_accel_limits(v_ego)
return limits[1] if limits else None
def reset(self):
"""Reset to default modes"""
print(f"[VIBE_DEBUG] Reset to default personalities - accel: {self.accel_personality}, long: {self.long_personality}")
self.accel_personality = AccelPersonality.normal
self.long_personality = LongPersonality.standard
self.frame = 0
def update(self):
"""Update frame counter"""
self.frame = (self.frame + 1) % 1000000
def _compute_slopes(self, x, y):
"""Compute slopes for Hermite interpolation using symmetric difference method."""
n = len(x)
if n < 2:
raise ValueError("At least two points required")
m = np.zeros(n)
for i in range(n):
if i == 0:
m[i] = (y[1] - y[0]) / (x[1] - x[0])
elif i == n-1:
m[i] = (y[i] - y[i-1]) / (x[i] - x[i-1])
else:
m[i] = ((y[i+1] - y[i]) / (x[i+1] - x[i]) + (y[i] - y[i-1]) / (x[i] - x[i-1])) / 2
return m
def _interpolate(self, x, xp, yp, slopes):
"""Perform cubic Hermite interpolation."""
x = np.clip(x, xp[0], xp[-1])
idx = np.clip(np.searchsorted(xp, x) - 1, 0, len(slopes) - 2)
x0, x1 = xp[idx], xp[idx+1]
y0, y1 = yp[idx], yp[idx+1]
m0, m1 = slopes[idx], slopes[idx+1]
t = (x - x0) / (x1 - x0)
h = [2*t**3 - 3*t**2 + 1, t**3 - 2*t**2 + t, -2*t**3 + 3*t**2, t**3 - t**2]
return h[0]*y0 + h[1]*(x1 - x0)*m0 + h[2]*y1 + h[3]*(x1 - x0)*m1