Compare commits

..

3 Commits

Author SHA1 Message Date
rav4kumar
e1f2f2b43e clean up 2026-05-15 18:54:31 -07:00
rav4kumar
e66840ac5b so picky 2026-05-15 18:24:26 -07:00
rav4kumar
a23cd7dddc relc 2026-05-15 18:24:00 -07:00
30 changed files with 270 additions and 514 deletions

View File

@@ -1,6 +1,3 @@
sunnypilot Version 2026.002.000 (2026-xx-xx)
========================
sunnypilot Version 2026.001.000 (2026-05-06)
========================
* What's Changed (sunnypilot/sunnypilot)

View File

@@ -194,13 +194,6 @@ 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;
@@ -349,6 +342,7 @@ struct OnroadEventSP @0xda96579883444c35 {
speedLimitChanged @21;
speedLimitPending @22;
e2eChime @23;
laneChangeRoadEdge @24;
}
}
@@ -455,6 +449,8 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
struct ModelDataV2SP @0xa1680744031fdb2d {
laneTurnDirection @0 :TurnDirection;
leftLaneChangeEdgeBlock @1 :Bool;
rightLaneChangeEdgeBlock @2 :Bool;
enum TurnDirection {
none @0;

View File

@@ -2054,14 +2054,16 @@ struct DriverStateV2 {
facePosition @2 :List(Float32);
facePositionStd @3 :List(Float32);
faceProb @4 :Float32;
leftEyeProb @5 :Float32;
rightEyeProb @6 :Float32;
leftBlinkProb @7 :Float32;
rightBlinkProb @8 :Float32;
sunglassesProb @9 :Float32;
eyesVisibleProb @14 :Float32;
eyesClosedProb @15 :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);

View File

@@ -135,8 +135,6 @@ 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"}},
@@ -180,6 +178,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
{"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
{"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
{"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
{"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},

View File

@@ -56,7 +56,7 @@ class DesireHelper:
def get_lane_change_direction(CS):
return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
def update(self, carstate, lateral_active, lane_change_prob):
def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected=False, right_edge_detected=False):
self.alc.update_params()
self.lane_turn_controller.update_params()
v_ego = carstate.vEgo
@@ -88,8 +88,8 @@ class DesireHelper:
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)

View File

@@ -313,14 +313,11 @@ 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, a_cruise_min=None):
def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
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)
@@ -332,7 +329,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 * a_cruise_min * 1.05)
v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 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)

View File

@@ -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 = self.get_accel_clip(v_ego) or [ACCEL_MIN, get_max_accel(v_ego)]
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)
@@ -138,8 +138,7 @@ 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,
a_cruise_min=self.get_cruise_min_accel(v_ego))
self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
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)

View File

@@ -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', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
for key in ['face_prob', 'eyes_visible_prob', 'eyes_closed_prob', 'using_phone_prob']:
parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
return parsed
@@ -93,11 +93,8 @@ 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.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.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.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):

View File

@@ -32,9 +32,8 @@ class DRIVER_MONITOR_SETTINGS:
self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
self._FACE_THRESHOLD = 0.7
self._EYE_THRESHOLD = 0.65
self._SG_THRESHOLD = 0.9
self._BLINK_THRESHOLD = 0.865
self._EYE_THRESHOLD = 0.5
self._BLINK_THRESHOLD = 0.5
self._PHONE_THRESH = 0.5
self._POSE_PITCH_THRESHOLD = 0.3133
@@ -111,11 +110,6 @@ 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
@@ -150,7 +144,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 = DriverBlink()
self.blink_prob = 0.
self.phone_prob = 0.
self.always_on = always_on
@@ -253,7 +247,7 @@ class DriverMonitoring:
if pitch_error > pitch_threshold or yaw_error > yaw_threshold:
distracted_types.append(DistractedType.DISTRACTED_POSE)
if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD:
if self.blink_prob > self.settings._BLINK_THRESHOLD:
distracted_types.append(DistractedType.DISTRACTED_BLINK)
if self.phone_prob > self.settings._PHONE_THRESH:
@@ -294,10 +288,7 @@ 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.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.blink_prob = driver_data.eyesClosedProb * (driver_data.eyesVisibleProb > self.settings._EYE_THRESHOLD)
self.phone_prob = driver_data.phoneProb
self.distracted_types = self._get_distracted_types()

View File

@@ -20,10 +20,8 @@ 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.leftEyeProb = 1.
ds.leftDriverData.rightEyeProb = 1.
ds.leftDriverData.leftBlinkProb = 1. * distracted
ds.leftDriverData.rightBlinkProb = 1. * distracted
ds.leftDriverData.eyesVisibleProb = 1.
ds.leftDriverData.eyesClosedProb = 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

View File

@@ -298,9 +298,16 @@ class SelfdriveD(CruiseHelper):
# Handle lane change
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['modelV2'].meta.laneChangeDirection
mdv2sp = self.sm['modelDataV2SP']
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
elif (mdv2sp.leftLaneChangeEdgeBlock and direction == LaneChangeDirection.left) or \
(mdv2sp.rightLaneChangeEdgeBlock and direction == LaneChangeDirection.right):
self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)

View File

@@ -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.leftBlinkProb), "leftDriverData.leftBlinkProb"),
(lambda x: get_idx_if_non_empty(x.leftDriverData.eyesClosedProb), "leftDriverData.eyesClosedProb"),
(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")

View File

@@ -39,8 +39,6 @@ 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()
@@ -154,8 +152,6 @@ 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"]
@@ -202,31 +198,21 @@ 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
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
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)]:
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):

View File

@@ -51,11 +51,17 @@ class LaneChangeSettingsLayout(Widget):
description=lambda: tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring " +
"(BSM) detects a obstructing vehicle, ensuring safe maneuvering."),
)
self._road_edge_block = toggle_item_sp(
param="RoadEdgeLaneChangeEnabled",
title=lambda: tr("Block Lane Change: Road Edge Detection"),
description=lambda: tr("Blocks the lane change if the model sees a road edge on your signaled side."),
)
items = [
self._lane_change_timer,
LineSeparatorSP(40),
self._bsm_delay,
self._road_edge_block,
]
return items

View File

@@ -1 +1 @@
#define SUNNYPILOT_VERSION "2026.002.000"
#define SUNNYPILOT_VERSION "2026.001.000"

View File

@@ -34,6 +34,7 @@ from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
from openpilot.sunnypilot.models.helpers import get_active_bundle
from openpilot.sunnypilot.models.runners.helpers import get_model_runner
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
@@ -246,6 +247,9 @@ def main(demo=False):
prev_action = log.ModelDataV2.Action()
DH = DesireHelper()
RELC = RoadEdgeLaneChangeController(DH)
while True:
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -349,7 +353,10 @@ def main(demo=False):
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
lane_change_prob = l_lane_change_prob + r_lane_change_prob
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego)
mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

View File

@@ -1,156 +0,0 @@
"""
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

View File

@@ -17,9 +17,6 @@ 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
@@ -29,7 +26,6 @@ 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)
@@ -47,17 +43,6 @@ 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)
@@ -92,7 +77,6 @@ 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')
@@ -111,8 +95,6 @@ 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

View File

@@ -0,0 +1,84 @@
"""
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.
"""
import numpy as np
from openpilot.common.constants import CV
from openpilot.common.realtime import DT_MDL
from openpilot.common.params import Params
NEARSIDE_PROB = 0.2
EDGE_PROB = 0.35
EDGE_REACTION_TIME = 1.0
EDGE_CLEAR_TIME = 0.3
MIN_SPEED = 20 * CV.MPH_TO_MS
class RoadEdgeLaneChangeController:
def __init__(self, desire_helper):
self.DH = desire_helper
self.params = Params()
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
self.param_read_counter = 0
self.left_edge_detected = False
self.right_edge_detected = False
self.left_edge_timer = 0.0
self.right_edge_timer = 0.0
self.left_clear_timer = 0.0
self.right_clear_timer = 0.0
def read_params(self) -> None:
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
def update_params(self) -> None:
if self.param_read_counter % 50 == 0:
self.read_params()
self.param_read_counter += 1
def reset(self) -> None:
self.left_edge_detected = False
self.right_edge_detected = False
self.left_edge_timer = 0.0
self.right_edge_timer = 0.0
self.left_clear_timer = 0.0
self.right_clear_timer = 0.0
def update(self, road_edge_stds, lane_line_probs, v_ego: float) -> None:
self.update_params()
if not self.enabled or v_ego < MIN_SPEED:
self.reset()
return
left_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
right_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
left_lane_prob = lane_line_probs[0]
right_lane_prob = lane_line_probs[3]
left_cond = left_edge_prob > EDGE_PROB and left_lane_prob < NEARSIDE_PROB and right_lane_prob >= left_lane_prob
right_cond = right_edge_prob > EDGE_PROB and right_lane_prob < NEARSIDE_PROB and left_lane_prob >= right_lane_prob
if left_cond:
self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
self.left_clear_timer = 0.0
if self.left_edge_timer > EDGE_REACTION_TIME:
self.left_edge_detected = True
else:
self.left_clear_timer += DT_MDL
if self.left_clear_timer > EDGE_CLEAR_TIME:
self.left_edge_timer = 0.0
self.left_edge_detected = False
if right_cond:
self.right_edge_timer = min(self.right_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
self.right_clear_timer = 0.0
if self.right_edge_timer > EDGE_REACTION_TIME:
self.right_edge_detected = True
else:
self.right_clear_timer += DT_MDL
if self.right_clear_timer > EDGE_CLEAR_TIME:
self.right_edge_timer = 0.0
self.right_edge_detected = False

View File

@@ -1,147 +0,0 @@
"""
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

View File

@@ -5,6 +5,8 @@ from openpilot.common.params import Params
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
TurnDirection = custom.ModelDataV2SP.TurnDirection
@@ -107,7 +109,11 @@ def set_lane_turn_params():
])
def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
dh = DesireHelper()
relc = RoadEdgeLaneChangeController(dh)
relc.enabled = True
dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
for _ in range(10):
dh.update(carstate, lateral_active, lane_change_prob)
dh.update(carstate, lateral_active, lane_change_prob,
left_edge_detected=relc.left_edge_detected, right_edge_detected=relc.right_edge_detected)
assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers

View File

@@ -0,0 +1,99 @@
"""
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.
"""
import pytest
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
from openpilot.sunnypilot.selfdrive.controls.lib.relc import (
RoadEdgeLaneChangeController, EDGE_REACTION_TIME, EDGE_CLEAR_TIME, MIN_SPEED,
)
V_HIGH = MIN_SPEED + 2.0
V_LOW = MIN_SPEED - 1.0
@pytest.fixture
def relc(mocker):
mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
mock_params.return_value.get_bool.return_value = True
controller = RoadEdgeLaneChangeController(DesireHelper())
controller.enabled = True
return controller
def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH):
for _ in range(int(seconds / DT_MDL) + 1):
controller.update(road_edge_stds, lane_line_probs, v_ego)
@pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [
([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], "left_edge_detected"),
([0.9, 0.0], [0.8, 0.8, 0.8, 0.0], "right_edge_detected"),
])
def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr):
drive(relc, road_edge_stds, lane_line_probs, EDGE_REACTION_TIME + 0.1)
assert getattr(relc, attr)
def test_edge_detection_requires_time(relc):
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME - 0.05)
assert not relc.left_edge_detected
def test_both_edges_detected(relc):
drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
assert relc.left_edge_detected
assert relc.right_edge_detected
def test_noise_doesnt_clear(relc):
edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
assert relc.left_edge_detected
relc.update(*clear, V_HIGH)
relc.update(*edge, V_HIGH)
assert relc.left_edge_detected
def test_clears_after_window(relc):
edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
assert relc.left_edge_detected
drive(relc, *clear, EDGE_CLEAR_TIME + 0.05)
assert not relc.left_edge_detected
assert relc.left_edge_timer == 0.0
def test_low_speed_skips(relc):
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW)
assert not relc.left_edge_detected
assert relc.left_edge_timer == 0.0
def test_speed_drop_resets(relc):
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
assert relc.left_edge_detected
relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_LOW)
assert not relc.left_edge_detected
def test_param_off_resets(relc):
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
assert relc.left_edge_detected
relc.params.get_bool.return_value = False
relc.read_params()
relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_HIGH)
assert not relc.left_edge_detected
assert not relc.right_edge_detected

View File

@@ -243,4 +243,12 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
AlertStatus.normal, AlertSize.none,
Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
},
EventNameSP.laneChangeRoadEdge: {
ET.WARNING: Alert(
"Lane Change Unavailable: Road Edge",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1),
},
}

View File

@@ -1,26 +1,4 @@
{
"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": ""
@@ -1136,6 +1114,10 @@
"title": "Record Front Lock",
"description": ""
},
"RoadEdgeLaneChangeEnabled": {
"title": "Block Lane Change: Road Edge Detection",
"description": ""
},
"RoadName": {
"title": "Road Name",
"description": ""

View File

@@ -519,6 +519,12 @@
}
]
},
{
"key": "RoadEdgeLaneChangeEnabled",
"widget": "toggle",
"title": "Block Lane Change: Road Edge Detection",
"description": "Blocks lane change when the model sees a road edge on the side you signal."
},
{
"key": "AutoLaneChangeBsmDelay",
"widget": "toggle",
@@ -620,65 +626,6 @@
}
]
},
{
"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",
@@ -1723,13 +1670,13 @@
{
"id": "updates",
"title": "Updates",
"description": "Control software updates",
"description": "Control automatic software updates",
"items": [
{
"key": "DisableUpdates",
"widget": "toggle",
"title": "Disable Updates",
"description": "When enabled, software updates will be off. This requires a reboot to take effect.",
"description": "When enabled, automatic software updates will be off. This requires a reboot to take effect.",
"enablement": [
{
"type": "offroad_only"

View File

@@ -43,34 +43,6 @@ 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)

View File

@@ -9,12 +9,12 @@ description: Software update preferences
sections:
- id: updates
title: Updates
description: Control software updates
description: Control automatic software updates
items:
- key: DisableUpdates
widget: toggle
title: Disable Updates
description: When enabled, software updates will be off. This requires a reboot to take effect.
description: When enabled, automatic software updates will be off. This requires a reboot to take effect.
enablement:
- $ref: '#/macros/offroad'
- $ref: '#/macros/advanced_only'