mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-06-08 17:14:36 +08:00
Compare commits
4 Commits
feat/relc
...
accel-cont
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
41ce29af86 | ||
|
|
dfc3c98b22 | ||
|
|
107a6f4c00 | ||
|
|
059d0b6c4c |
@@ -1,3 +1,6 @@
|
||||
sunnypilot Version 2026.002.000 (2026-xx-xx)
|
||||
========================
|
||||
|
||||
sunnypilot Version 2026.001.000 (2026-05-06)
|
||||
========================
|
||||
* What's Changed (sunnypilot/sunnypilot)
|
||||
|
||||
@@ -194,6 +194,13 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
aTarget @5 :Float32;
|
||||
events @6 :List(OnroadEventSP.Event);
|
||||
e2eAlerts @7 :E2eAlerts;
|
||||
accelPersonality @8 :AccelerationPersonality;
|
||||
|
||||
enum AccelerationPersonality {
|
||||
sport @0;
|
||||
normal @1;
|
||||
eco @2;
|
||||
}
|
||||
|
||||
struct DynamicExperimentalControl {
|
||||
state @0 :DynamicExperimentalControlState;
|
||||
|
||||
@@ -2054,16 +2054,14 @@ struct DriverStateV2 {
|
||||
facePosition @2 :List(Float32);
|
||||
facePositionStd @3 :List(Float32);
|
||||
faceProb @4 :Float32;
|
||||
eyesVisibleProb @14 :Float32;
|
||||
eyesClosedProb @15 :Float32;
|
||||
leftEyeProb @5 :Float32;
|
||||
rightEyeProb @6 :Float32;
|
||||
leftBlinkProb @7 :Float32;
|
||||
rightBlinkProb @8 :Float32;
|
||||
sunglassesProb @9 :Float32;
|
||||
phoneProb @13 :Float32;
|
||||
|
||||
deprecated :group {
|
||||
leftEyeProb @5 :Float32;
|
||||
rightEyeProb @6 :Float32;
|
||||
leftBlinkProb @7 :Float32;
|
||||
rightBlinkProb @8 :Float32;
|
||||
sunglassesProb @9 :Float32;
|
||||
notReadyProb @12 :List(Float32);
|
||||
occludedProb @10 :Float32;
|
||||
readyProb @11 :List(Float32);
|
||||
|
||||
@@ -135,6 +135,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"Version", {PERSISTENT, STRING}},
|
||||
|
||||
// --- sunnypilot params --- //
|
||||
{"AccelPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast<int>(cereal::LongitudinalPlanSP::AccelerationPersonality::NORMAL))}},
|
||||
{"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ApiCache_DriveStats", {PERSISTENT, JSON}},
|
||||
{"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
|
||||
BIN
selfdrive/assets/icons_mici/onroad/glasses.png
LFS
Normal file
BIN
selfdrive/assets/icons_mici/onroad/glasses.png
LFS
Normal file
Binary file not shown.
@@ -313,11 +313,14 @@ class LongitudinalMpc:
|
||||
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
|
||||
return lead_xv
|
||||
|
||||
def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
|
||||
def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard, a_cruise_min=None):
|
||||
t_follow = get_T_FOLLOW(personality)
|
||||
v_ego = self.x0[1]
|
||||
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
|
||||
|
||||
if a_cruise_min is None:
|
||||
a_cruise_min = CRUISE_MIN_ACCEL
|
||||
|
||||
lead_xv_0 = self.process_lead(radarstate.leadOne)
|
||||
lead_xv_1 = self.process_lead(radarstate.leadTwo)
|
||||
|
||||
@@ -329,7 +332,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), v_lower, v_upper)
|
||||
|
||||
@@ -110,7 +110,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
# No change cost when user is controlling the speed, or when standstill
|
||||
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
|
||||
|
||||
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
|
||||
accel_clip = self.get_accel_clip(v_ego) or [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)
|
||||
|
||||
@@ -138,7 +138,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
|
||||
self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality,
|
||||
a_cruise_min=self.get_cruise_min_accel(v_ego))
|
||||
|
||||
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
|
||||
self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
|
||||
|
||||
@@ -83,7 +83,7 @@ def parse_model_output(model_output):
|
||||
face_descs = model_output[f'face_descs_{ds_suffix}']
|
||||
parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
|
||||
parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
|
||||
for key in ['face_prob', 'eyes_visible_prob', 'eyes_closed_prob', 'using_phone_prob']:
|
||||
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
|
||||
parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
|
||||
return parsed
|
||||
|
||||
@@ -93,8 +93,11 @@ def fill_driver_data(msg, model_output, ds_suffix):
|
||||
msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist()
|
||||
msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist()
|
||||
msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.eyesVisibleProb = model_output[f'eyes_visible_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.eyesClosedProb = model_output[f'eyes_closed_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.leftBlinkProb = model_output[f'left_blink_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
|
||||
|
||||
def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
|
||||
|
||||
Binary file not shown.
@@ -32,8 +32,9 @@ class DRIVER_MONITOR_SETTINGS:
|
||||
self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
|
||||
|
||||
self._FACE_THRESHOLD = 0.7
|
||||
self._EYE_THRESHOLD = 0.5
|
||||
self._BLINK_THRESHOLD = 0.5
|
||||
self._EYE_THRESHOLD = 0.65
|
||||
self._SG_THRESHOLD = 0.9
|
||||
self._BLINK_THRESHOLD = 0.865
|
||||
self._PHONE_THRESH = 0.5
|
||||
|
||||
self._POSE_PITCH_THRESHOLD = 0.3133
|
||||
@@ -110,6 +111,11 @@ class DriverProb:
|
||||
self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable)
|
||||
self.prob_calibrated = False
|
||||
|
||||
class DriverBlink:
|
||||
def __init__(self):
|
||||
self.left = 0.
|
||||
self.right = 0.
|
||||
|
||||
|
||||
# model output refers to center of undistorted+leveled image
|
||||
EFL = 598.0 # focal length in K
|
||||
@@ -144,7 +150,7 @@ class DriverMonitoring:
|
||||
wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2)
|
||||
self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT)
|
||||
self.pose = DriverPose(settings=self.settings)
|
||||
self.blink_prob = 0.
|
||||
self.blink = DriverBlink()
|
||||
self.phone_prob = 0.
|
||||
|
||||
self.always_on = always_on
|
||||
@@ -247,7 +253,7 @@ class DriverMonitoring:
|
||||
if pitch_error > pitch_threshold or yaw_error > yaw_threshold:
|
||||
distracted_types.append(DistractedType.DISTRACTED_POSE)
|
||||
|
||||
if self.blink_prob > self.settings._BLINK_THRESHOLD:
|
||||
if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD:
|
||||
distracted_types.append(DistractedType.DISTRACTED_BLINK)
|
||||
|
||||
if self.phone_prob > self.settings._PHONE_THRESH:
|
||||
@@ -288,7 +294,10 @@ class DriverMonitoring:
|
||||
self.pose.yaw_std = driver_data.faceOrientationStd[1]
|
||||
model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
|
||||
self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD
|
||||
self.blink_prob = driver_data.eyesClosedProb * (driver_data.eyesVisibleProb > self.settings._EYE_THRESHOLD)
|
||||
self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \
|
||||
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \
|
||||
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.phone_prob = driver_data.phoneProb
|
||||
|
||||
self.distracted_types = self._get_distracted_types()
|
||||
|
||||
@@ -20,8 +20,10 @@ def make_msg(face_detected, distracted=False, model_uncertain=False):
|
||||
ds.leftDriverData.faceOrientation = [0., 0., 0.]
|
||||
ds.leftDriverData.facePosition = [0., 0.]
|
||||
ds.leftDriverData.faceProb = 1. * face_detected
|
||||
ds.leftDriverData.eyesVisibleProb = 1.
|
||||
ds.leftDriverData.eyesClosedProb = 1. * distracted
|
||||
ds.leftDriverData.leftEyeProb = 1.
|
||||
ds.leftDriverData.rightEyeProb = 1.
|
||||
ds.leftDriverData.leftBlinkProb = 1. * distracted
|
||||
ds.leftDriverData.rightBlinkProb = 1. * distracted
|
||||
ds.leftDriverData.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain]
|
||||
ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
|
||||
# TODO: test both separately when e2e is used
|
||||
|
||||
@@ -76,7 +76,7 @@ def generate_report(proposed, master, tmp, commit):
|
||||
(lambda x: get_idx_if_non_empty(x.wheelOnRightProb), "wheelOnRightProb"),
|
||||
(lambda x: get_idx_if_non_empty(x.leftDriverData.faceProb), "leftDriverData.faceProb"),
|
||||
(lambda x: get_idx_if_non_empty(x.leftDriverData.faceOrientation, 0), "leftDriverData.faceOrientation0"),
|
||||
(lambda x: get_idx_if_non_empty(x.leftDriverData.eyesClosedProb), "leftDriverData.eyesClosedProb"),
|
||||
(lambda x: get_idx_if_non_empty(x.leftDriverData.leftBlinkProb), "leftDriverData.leftBlinkProb"),
|
||||
(lambda x: get_idx_if_non_empty(x.leftDriverData.phoneProb), "leftDriverData.phoneProb"),
|
||||
(lambda x: get_idx_if_non_empty(x.rightDriverData.faceProb), "rightDriverData.faceProb"),
|
||||
], "driverStateV2")
|
||||
|
||||
@@ -39,6 +39,8 @@ class BaseDriverCameraDialog(Widget):
|
||||
self._eye_fill_texture = None
|
||||
self._eye_orange_texture = None
|
||||
self._eye_size = 74
|
||||
self._glasses_texture = None
|
||||
self._glasses_size = 171
|
||||
|
||||
self._load_eye_textures()
|
||||
|
||||
@@ -152,6 +154,8 @@ class BaseDriverCameraDialog(Widget):
|
||||
self._eye_fill_texture = gui_app.texture("icons_mici/onroad/eye_fill.png", self._eye_size, self._eye_size)
|
||||
if self._eye_orange_texture is None:
|
||||
self._eye_orange_texture = gui_app.texture("icons_mici/onroad/eye_orange.png", self._eye_size, self._eye_size)
|
||||
if self._glasses_texture is None:
|
||||
self._glasses_texture = gui_app.texture("icons_mici/onroad/glasses.png", self._glasses_size, self._glasses_size)
|
||||
|
||||
def _draw_face_detection(self, rect: rl.Rectangle):
|
||||
dm_state = ui_state.sm["driverMonitoringState"]
|
||||
@@ -198,21 +202,31 @@ class BaseDriverCameraDialog(Widget):
|
||||
eye_offset_x = 10
|
||||
eye_offset_y = 10
|
||||
eye_spacing = self._eye_size + 15
|
||||
eyes_prob = driver_data.eyesVisibleProb
|
||||
|
||||
left_eye_x = rect.x + eye_offset_x
|
||||
left_eye_y = rect.y + eye_offset_y
|
||||
left_eye_prob = driver_data.leftEyeProb
|
||||
|
||||
right_eye_x = rect.x + eye_offset_x + eye_spacing
|
||||
right_eye_y = rect.y + eye_offset_y
|
||||
right_eye_prob = driver_data.rightEyeProb
|
||||
|
||||
# Draw eyes with opacity based on probability
|
||||
fill_opacity = eyes_prob
|
||||
orange_opacity = 1.0 - eyes_prob
|
||||
for eye_x, eye_y in [(left_eye_x, left_eye_y), (right_eye_x, right_eye_y)]:
|
||||
for eye_x, eye_y, eye_prob in [(left_eye_x, left_eye_y, left_eye_prob), (right_eye_x, right_eye_y, right_eye_prob)]:
|
||||
fill_opacity = eye_prob
|
||||
orange_opacity = 1.0 - eye_prob
|
||||
|
||||
rl.draw_texture_v(self._eye_orange_texture, (eye_x, eye_y), rl.Color(255, 255, 255, int(255 * orange_opacity)))
|
||||
rl.draw_texture_v(self._eye_fill_texture, (eye_x, eye_y), rl.Color(255, 255, 255, int(255 * fill_opacity)))
|
||||
|
||||
# Draw sunglasses indicator based on sunglasses probability
|
||||
# Position glasses centered between the two eyes at top left
|
||||
glasses_x = rect.x + eye_offset_x - 4
|
||||
glasses_y = rect.y
|
||||
glasses_pos = rl.Vector2(glasses_x, glasses_y)
|
||||
glasses_prob = driver_data.sunglassesProb
|
||||
rl.draw_texture_v(self._glasses_texture, glasses_pos, rl.Color(70, 80, 161, int(255 * glasses_prob)))
|
||||
|
||||
|
||||
class DriverCameraDialog(NavWidget, BaseDriverCameraDialog):
|
||||
def __init__(self):
|
||||
|
||||
@@ -1 +1 @@
|
||||
#define SUNNYPILOT_VERSION "2026.001.000"
|
||||
#define SUNNYPILOT_VERSION "2026.002.000"
|
||||
|
||||
@@ -0,0 +1,156 @@
|
||||
"""
|
||||
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 custom
|
||||
import numpy as np
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
|
||||
AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
|
||||
|
||||
|
||||
A_MAX_BP = [0.0, 4.0, 8.0, 16.0, 40.0]
|
||||
A_MAX_V = {
|
||||
AccelPersonality.eco: [1.20, 1.40, 1.20, 0.40, 0.08],
|
||||
AccelPersonality.normal: [1.80, 1.80, 1.35, 0.50, 0.15],
|
||||
AccelPersonality.sport: [2.20, 2.20, 1.60, 0.70, 0.25],
|
||||
}
|
||||
|
||||
COAST_DRAG_BP = [0.0, 10.0, 25.0, 40.0]
|
||||
COAST_DRAG_V = {
|
||||
AccelPersonality.eco: [-0.03, -0.05, -0.08, -0.12],
|
||||
AccelPersonality.normal: [-0.04, -0.07, -0.12, -0.18],
|
||||
AccelPersonality.sport: [-0.06, -0.10, -0.18, -0.28],
|
||||
}
|
||||
|
||||
A_MIN_FLOOR_BP = [0.0, 5.0, 15.0, 40.0]
|
||||
A_MIN_FLOOR_V = {
|
||||
AccelPersonality.eco: [-0.20, -0.35, -0.55, -0.50],
|
||||
AccelPersonality.normal: [-0.25, -0.45, -0.75, -0.65],
|
||||
AccelPersonality.sport: [-0.35, -0.65, -1.00, -0.95],
|
||||
}
|
||||
|
||||
DEFICIT_TO_FLOOR = 8.5
|
||||
COAST_DEADBAND = 1.0
|
||||
RAMP_OFF_RANGE = 5.0
|
||||
|
||||
A_MIN_TIGHTEN_RATE = 0.6
|
||||
A_MIN_RELAX_RATE = 0.9
|
||||
A_MAX_RATE_UP = 1.5
|
||||
A_MAX_RATE_DOWN = 0.6
|
||||
|
||||
MIN_MAX_GAP = 0.05
|
||||
|
||||
PARAM_REFRESH_FRAMES = max(1, int(1.0 / DT_MDL))
|
||||
|
||||
|
||||
class AccelPersonalityController:
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.frame = 0
|
||||
self._first = True
|
||||
|
||||
val = self.params.get('AccelPersonality')
|
||||
self._personality = val if val is not None else AccelPersonality.normal
|
||||
self._enabled = self.params.get_bool('AccelPersonalityEnabled')
|
||||
|
||||
self._v_cruise = 0.0
|
||||
self._a_min = -0.05
|
||||
self._a_max = 1.50
|
||||
|
||||
self._cache_v: float | None = None
|
||||
self._cache_v_cruise: float | None = None
|
||||
self._cache_a_min = self._a_min
|
||||
self._cache_a_max = self._a_max
|
||||
|
||||
def update(self, sm=None):
|
||||
self.frame += 1
|
||||
self._cache_v = None
|
||||
self._cache_v_cruise = None
|
||||
|
||||
if sm is not None:
|
||||
vc = sm['carState'].vCruise
|
||||
self._v_cruise = float(vc) * (1000.0 / 3600.0) if vc != V_CRUISE_UNSET else 0.0
|
||||
|
||||
if self.frame % PARAM_REFRESH_FRAMES == 0:
|
||||
val = self.params.get('AccelPersonality')
|
||||
self._personality = val if val is not None else AccelPersonality.normal
|
||||
new_enabled = self.params.get_bool('AccelPersonalityEnabled')
|
||||
if new_enabled and not self._enabled:
|
||||
self._first = True
|
||||
self._enabled = new_enabled
|
||||
|
||||
def get_accel_personality(self) -> int:
|
||||
return int(self._personality)
|
||||
|
||||
def is_enabled(self) -> bool:
|
||||
return self._enabled
|
||||
|
||||
def get_accel_limits(self, v_ego: float) -> tuple[float, float]:
|
||||
v_ego = max(0.0, v_ego)
|
||||
if (self._cache_v is not None
|
||||
and abs(self._cache_v - v_ego) < 0.01
|
||||
and self._cache_v_cruise == self._v_cruise):
|
||||
return self._cache_a_min, self._cache_a_max
|
||||
self._cache_a_min, self._cache_a_max = self._step(v_ego)
|
||||
self._cache_v = v_ego
|
||||
self._cache_v_cruise = self._v_cruise
|
||||
return self._cache_a_min, self._cache_a_max
|
||||
|
||||
def get_min_accel(self, v_ego: float) -> float:
|
||||
return self.get_accel_limits(v_ego)[0]
|
||||
|
||||
def get_max_accel(self, v_ego: float) -> float:
|
||||
return self.get_accel_limits(v_ego)[1]
|
||||
|
||||
def _ramp_off(self, v_ego: float) -> float:
|
||||
if self._v_cruise <= 0.0:
|
||||
return 1.0
|
||||
return float(np.clip((self._v_cruise - v_ego) / RAMP_OFF_RANGE, 0.0, 1.0))
|
||||
|
||||
def _target_max(self, v_ego: float) -> float:
|
||||
base = float(np.interp(v_ego, A_MAX_BP, A_MAX_V[self._personality]))
|
||||
return base * self._ramp_off(v_ego)
|
||||
|
||||
def _target_min(self, v_ego: float) -> float:
|
||||
coast = float(np.interp(v_ego, COAST_DRAG_BP, COAST_DRAG_V[self._personality]))
|
||||
if self._v_cruise <= 0.0 or v_ego >= self._v_cruise:
|
||||
return coast
|
||||
floor = float(np.interp(v_ego, A_MIN_FLOOR_BP, A_MIN_FLOOR_V[self._personality]))
|
||||
deficit = self._v_cruise - v_ego
|
||||
t = float(np.clip(deficit / DEFICIT_TO_FLOOR, 0.0, 1.0)) ** 1.5
|
||||
return coast + t * (floor - coast)
|
||||
|
||||
def _apply_coast_deadband(self, v_ego: float, t_min: float, t_max: float) -> tuple[float, float]:
|
||||
if self._v_cruise <= 0.0 or abs(v_ego - self._v_cruise) >= COAST_DEADBAND:
|
||||
return t_min, t_max
|
||||
coast = float(np.interp(v_ego, COAST_DRAG_BP, COAST_DRAG_V[self._personality]))
|
||||
return coast, max(0.05, t_max * 0.25)
|
||||
|
||||
def _rate_limit(self, last: float, target: float, rate_down: float, rate_up: float) -> float:
|
||||
rate = rate_up if target > last else rate_down
|
||||
step = rate * DT_MDL
|
||||
return float(np.clip(target, last - step, last + step))
|
||||
|
||||
def _step(self, v_ego: float) -> tuple[float, float]:
|
||||
t_max = self._target_max(v_ego)
|
||||
t_min = self._target_min(v_ego)
|
||||
t_min, t_max = self._apply_coast_deadband(v_ego, t_min, t_max)
|
||||
|
||||
if self._first:
|
||||
self._a_min, self._a_max = t_min, t_max
|
||||
self._first = False
|
||||
return self._a_min, self._a_max
|
||||
|
||||
new_min = self._rate_limit(self._a_min, t_min, rate_down=A_MIN_TIGHTEN_RATE, rate_up=A_MIN_RELAX_RATE)
|
||||
new_max = self._rate_limit(self._a_max, t_max, rate_down=A_MAX_RATE_DOWN, rate_up=A_MAX_RATE_UP)
|
||||
|
||||
new_min = min(new_min, new_max - MIN_MAX_GAP)
|
||||
|
||||
self._a_min, self._a_max = new_min, new_max
|
||||
return self._a_min, self._a_max
|
||||
@@ -17,6 +17,9 @@ 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.accel_personality.accel_controller import AccelPersonalityController
|
||||
from opendbc.car.interfaces import ACCEL_MIN
|
||||
|
||||
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
|
||||
LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
|
||||
|
||||
@@ -26,6 +29,7 @@ class LongitudinalPlannerSP:
|
||||
self.events_sp = EventsSP()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
self.dec = DynamicExperimentalController(CP, mpc)
|
||||
self.accel_controller = AccelPersonalityController()
|
||||
self.scc = SmartCruiseControl()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
self.sla = SpeedLimitAssist(CP, CP_SP)
|
||||
@@ -43,6 +47,17 @@ class LongitudinalPlannerSP:
|
||||
|
||||
return experimental_mode and self.dec.mode() == "blended"
|
||||
|
||||
def get_accel_clip(self, v_ego: float) -> list[float] | None:
|
||||
if not self.accel_controller.is_enabled():
|
||||
return None
|
||||
a_max = self.accel_controller.get_max_accel(v_ego)
|
||||
return [ACCEL_MIN, max(ACCEL_MIN, a_max)]
|
||||
|
||||
def get_cruise_min_accel(self, v_ego: float) -> float | None:
|
||||
if self.accel_controller.is_enabled():
|
||||
return self.accel_controller.get_min_accel(v_ego)
|
||||
return None
|
||||
|
||||
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
|
||||
CS = sm['carState']
|
||||
v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
|
||||
@@ -77,6 +92,7 @@ class LongitudinalPlannerSP:
|
||||
self.events_sp.clear()
|
||||
self.dec.update(sm)
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
self.accel_controller.update(sm)
|
||||
|
||||
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
||||
plan_sp_send = messaging.new_message('longitudinalPlanSP')
|
||||
@@ -95,6 +111,8 @@ class LongitudinalPlannerSP:
|
||||
dec.enabled = self.dec.enabled()
|
||||
dec.active = self.dec.active()
|
||||
|
||||
longitudinalPlanSP.accelPersonality = int(self.accel_controller.get_accel_personality())
|
||||
|
||||
# Smart Cruise Control
|
||||
smartCruiseControl = longitudinalPlanSP.smartCruiseControl
|
||||
# Vision Control
|
||||
|
||||
147
sunnypilot/selfdrive/controls/lib/tests/test_accel_controller.py
Normal file
147
sunnypilot/selfdrive/controls/lib/tests/test_accel_controller.py
Normal file
@@ -0,0 +1,147 @@
|
||||
"""
|
||||
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.
|
||||
|
||||
Coverage for AccelPersonalityController:
|
||||
- live param flip via auto-refresh (no Python set_enabled() call needed)
|
||||
- V_CRUISE_UNSET guard
|
||||
- enable-transition snap to fresh target
|
||||
- per-personality accel limit deltas vs stock get_max_accel
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
from cereal import custom
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from opendbc.car.interfaces import ACCEL_MIN
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel as stock_get_max_accel
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import (
|
||||
AccelPersonalityController,
|
||||
PARAM_REFRESH_FRAMES,
|
||||
)
|
||||
|
||||
|
||||
AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
|
||||
|
||||
|
||||
class FakeCarState:
|
||||
def __init__(self, v_cruise=30.0):
|
||||
self.vCruise = v_cruise
|
||||
|
||||
|
||||
class FakeSM:
|
||||
def __init__(self, v_cruise=30.0):
|
||||
self._data = {'carState': FakeCarState(v_cruise)}
|
||||
|
||||
def __getitem__(self, k):
|
||||
return self._data[k]
|
||||
|
||||
|
||||
def _print_table(title, header, rows):
|
||||
print(f"\n--- {title} ---")
|
||||
print(" | ".join(f"{h:>12}" for h in header))
|
||||
print("-" * (15 * len(header)))
|
||||
for row in rows:
|
||||
print(" | ".join(f"{v:>12.3f}" if isinstance(v, float) else f"{v:>12}" for v in row))
|
||||
|
||||
|
||||
class TestAccelLiveFlip:
|
||||
def test_enable_via_param(self):
|
||||
Params().put_bool('AccelPersonalityEnabled', False)
|
||||
c = AccelPersonalityController()
|
||||
assert not c.is_enabled()
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM())
|
||||
assert c.is_enabled()
|
||||
|
||||
def test_disable_via_param(self):
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
c = AccelPersonalityController()
|
||||
assert c.is_enabled()
|
||||
Params().put_bool('AccelPersonalityEnabled', False)
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM())
|
||||
assert not c.is_enabled()
|
||||
|
||||
def test_personality_change_via_param(self):
|
||||
Params().put('AccelPersonality', AccelPersonality.normal)
|
||||
c = AccelPersonalityController()
|
||||
assert c.get_accel_personality() == AccelPersonality.normal
|
||||
Params().put('AccelPersonality', AccelPersonality.sport)
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM())
|
||||
assert c.get_accel_personality() == AccelPersonality.sport
|
||||
|
||||
def test_refresh_boundary_below_threshold(self):
|
||||
Params().put_bool('AccelPersonalityEnabled', False)
|
||||
c = AccelPersonalityController()
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
for _ in range(PARAM_REFRESH_FRAMES - 1):
|
||||
c.update(FakeSM())
|
||||
assert not c.is_enabled()
|
||||
|
||||
def test_enable_transition_snaps_to_target(self):
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
Params().put('AccelPersonality', AccelPersonality.sport)
|
||||
c = AccelPersonalityController()
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM(v_cruise=35.0))
|
||||
c.get_accel_limits(25.0)
|
||||
|
||||
Params().put_bool('AccelPersonalityEnabled', False)
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM(v_cruise=35.0))
|
||||
assert not c.is_enabled()
|
||||
|
||||
Params().put('AccelPersonality', AccelPersonality.eco)
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
for _ in range(PARAM_REFRESH_FRAMES + 1):
|
||||
c.update(FakeSM(v_cruise=35.0))
|
||||
assert c._first
|
||||
|
||||
def test_vcruise_unset_treated_as_zero(self):
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
c = AccelPersonalityController()
|
||||
c.update(FakeSM(v_cruise=V_CRUISE_UNSET))
|
||||
assert c._v_cruise == 0.0
|
||||
|
||||
|
||||
class TestAccelUsageDiff:
|
||||
def test_accel_clip_per_personality(self, capsys):
|
||||
rows = []
|
||||
speeds = [3.0, 10.0, 20.0, 30.0]
|
||||
personalities = [
|
||||
('eco', AccelPersonality.eco),
|
||||
('normal', AccelPersonality.normal),
|
||||
('sport', AccelPersonality.sport),
|
||||
]
|
||||
|
||||
Params().put_bool('AccelPersonalityEnabled', True)
|
||||
sm = FakeSM(v_cruise=35.0)
|
||||
|
||||
any_delta = False
|
||||
for label, p in personalities:
|
||||
Params().put('AccelPersonality', p)
|
||||
c = AccelPersonalityController()
|
||||
c.update(sm)
|
||||
for v_ego in speeds:
|
||||
stock_hi = float(stock_get_max_accel(v_ego))
|
||||
c_lo, c_hi = c.get_accel_limits(v_ego)
|
||||
delta_hi = c_hi - stock_hi
|
||||
delta_lo = c_lo - ACCEL_MIN
|
||||
if abs(delta_hi) > 0.01 or abs(delta_lo) > 0.01:
|
||||
any_delta = True
|
||||
rows.append((label, v_ego, stock_hi, c_hi, delta_hi, c_lo, delta_lo))
|
||||
|
||||
with capsys.disabled():
|
||||
_print_table(
|
||||
"AccelPersonalityController: a_max stock vs controller",
|
||||
["personality", "v_ego", "stock_hi", "ctrl_hi", "delta_hi", "ctrl_lo", "delta_lo"],
|
||||
rows,
|
||||
)
|
||||
assert any_delta
|
||||
@@ -1,4 +1,26 @@
|
||||
{
|
||||
"AccelPersonality": {
|
||||
"title": "Acceleration Personality",
|
||||
"description": "Select the acceleration personality profile. Sport provides more aggressive acceleration, Eco provides gentler acceleration.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
"label": "Sport"
|
||||
},
|
||||
{
|
||||
"value": 1,
|
||||
"label": "Normal"
|
||||
},
|
||||
{
|
||||
"value": 2,
|
||||
"label": "Eco"
|
||||
}
|
||||
]
|
||||
},
|
||||
"AccelPersonalityEnabled": {
|
||||
"title": "Custom Acceleration Personality",
|
||||
"description": "Enable custom acceleration and braking profiles that adjust max acceleration and min deceleration based on speed and selected personality."
|
||||
},
|
||||
"AccessToken": {
|
||||
"title": "AccessTokenIsNice",
|
||||
"description": ""
|
||||
|
||||
@@ -620,6 +620,65 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"widget": "toggle",
|
||||
"title": "Acceleration Personality",
|
||||
"description": "Enable per-personality acceleration profiles. Sport allows stronger acceleration; Eco is gentler.",
|
||||
"visibility": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelPersonality",
|
||||
"widget": "multiple_button",
|
||||
"title": "Acceleration Profile",
|
||||
"description": "Sport allows the most aggressive acceleration; Eco the gentlest. Normal sits between.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
"label": "Sport"
|
||||
},
|
||||
{
|
||||
"value": 1,
|
||||
"label": "Normal"
|
||||
},
|
||||
{
|
||||
"value": 2,
|
||||
"label": "Eco"
|
||||
}
|
||||
],
|
||||
"visibility": [
|
||||
{
|
||||
"type": "param",
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"equals": true
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
},
|
||||
{
|
||||
"type": "param",
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "IntelligentCruiseButtonManagement",
|
||||
"widget": "toggle",
|
||||
@@ -1664,13 +1723,13 @@
|
||||
{
|
||||
"id": "updates",
|
||||
"title": "Updates",
|
||||
"description": "Control automatic software updates",
|
||||
"description": "Control software updates",
|
||||
"items": [
|
||||
{
|
||||
"key": "DisableUpdates",
|
||||
"widget": "toggle",
|
||||
"title": "Disable Updates",
|
||||
"description": "When enabled, automatic software updates will be off. This requires a reboot to take effect.",
|
||||
"description": "When enabled, software updates will be off. This requires a reboot to take effect.",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "offroad_only"
|
||||
|
||||
@@ -43,6 +43,34 @@ sections:
|
||||
label: Relaxed
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonalityEnabled
|
||||
widget: toggle
|
||||
title: Acceleration Personality
|
||||
description: Enable per-personality acceleration profiles. Sport allows stronger acceleration; Eco is gentler.
|
||||
visibility:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonality
|
||||
widget: multiple_button
|
||||
title: Acceleration Profile
|
||||
description: Sport allows the most aggressive acceleration; Eco the gentlest. Normal sits between.
|
||||
options:
|
||||
- value: 0
|
||||
label: Sport
|
||||
- value: 1
|
||||
label: Normal
|
||||
- value: 2
|
||||
label: Eco
|
||||
visibility:
|
||||
- type: param
|
||||
key: AccelPersonalityEnabled
|
||||
equals: true
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- type: param
|
||||
key: AccelPersonalityEnabled
|
||||
equals: true
|
||||
- key: IntelligentCruiseButtonManagement
|
||||
widget: toggle
|
||||
title: Intelligent Cruise Button Management (ICBM) (Alpha)
|
||||
|
||||
@@ -9,12 +9,12 @@ description: Software update preferences
|
||||
sections:
|
||||
- id: updates
|
||||
title: Updates
|
||||
description: Control automatic software updates
|
||||
description: Control software updates
|
||||
items:
|
||||
- key: DisableUpdates
|
||||
widget: toggle
|
||||
title: Disable Updates
|
||||
description: When enabled, automatic software updates will be off. This requires a reboot to take effect.
|
||||
description: When enabled, software updates will be off. This requires a reboot to take effect.
|
||||
enablement:
|
||||
- $ref: '#/macros/offroad'
|
||||
- $ref: '#/macros/advanced_only'
|
||||
|
||||
Reference in New Issue
Block a user