what's that mean, dumby

This commit is contained in:
firestar5683
2026-09-03 15:01:53 -05:00
parent f18cf22104
commit d2fb362876
29 changed files with 1097 additions and 78 deletions
+26
View File
@@ -259,6 +259,32 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"CustomAccelProfile45MPH", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},
{"CustomAccelProfile56MPH", {PERSISTENT, FLOAT, "0.8", "0.8", 3}},
{"CustomAccelProfile89MPH", {PERSISTENT, FLOAT, "0.6", "0.6", 3}},
{"CustomAccelProfileBreakpointsInitialized", {PERSISTENT, BOOL, "0", "0", 3}},
{"CustomAccelProfilePointCount", {PERSISTENT, INT, "7", "7", 3}},
{"CustomAccelProfileBreakpoint1MPH", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
{"CustomAccelProfileBreakpoint2MPH", {PERSISTENT, FLOAT, "11.184681", "11.184681", 3}},
{"CustomAccelProfileBreakpoint3MPH", {PERSISTENT, FLOAT, "22.369363", "22.369363", 3}},
{"CustomAccelProfileBreakpoint4MPH", {PERSISTENT, FLOAT, "33.554044", "33.554044", 3}},
{"CustomAccelProfileBreakpoint5MPH", {PERSISTENT, FLOAT, "44.738726", "44.738726", 3}},
{"CustomAccelProfileBreakpoint6MPH", {PERSISTENT, FLOAT, "55.923407", "55.923407", 3}},
{"CustomAccelProfileBreakpoint7MPH", {PERSISTENT, FLOAT, "89.477452", "89.477452", 3}},
{"CustomAccelProfileBreakpoint8MPH", {PERSISTENT, FLOAT, "100.662133", "100.662133", 3}},
{"CustomAccelProfileBreakpoint9MPH", {PERSISTENT, FLOAT, "111.846815", "111.846815", 3}},
{"CustomAccelProfileBreakpoint10MPH", {PERSISTENT, FLOAT, "123.031496", "123.031496", 3}},
{"CustomAccelProfileBreakpoint11MPH", {PERSISTENT, FLOAT, "134.216178", "134.216178", 3}},
{"CustomAccelProfileBreakpoint12MPH", {PERSISTENT, FLOAT, "145.400859", "145.400859", 3}},
{"CustomAccelProfilePoint1Accel", {PERSISTENT, FLOAT, "3.0", "3.0", 3}},
{"CustomAccelProfilePoint2Accel", {PERSISTENT, FLOAT, "2.5", "2.5", 3}},
{"CustomAccelProfilePoint3Accel", {PERSISTENT, FLOAT, "2.0", "2.0", 3}},
{"CustomAccelProfilePoint4Accel", {PERSISTENT, FLOAT, "1.5", "1.5", 3}},
{"CustomAccelProfilePoint5Accel", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},
{"CustomAccelProfilePoint6Accel", {PERSISTENT, FLOAT, "0.8", "0.8", 3}},
{"CustomAccelProfilePoint7Accel", {PERSISTENT, FLOAT, "0.6", "0.6", 3}},
{"CustomAccelProfilePoint8Accel", {PERSISTENT, FLOAT, "0.55", "0.55", 3}},
{"CustomAccelProfilePoint9Accel", {PERSISTENT, FLOAT, "0.5", "0.5", 3}},
{"CustomAccelProfilePoint10Accel", {PERSISTENT, FLOAT, "0.45", "0.45", 3}},
{"CustomAccelProfilePoint11Accel", {PERSISTENT, FLOAT, "0.4", "0.4", 3}},
{"CustomAccelProfilePoint12Accel", {PERSISTENT, FLOAT, "0.35", "0.35", 3}},
{"CustomCruise", {PERSISTENT, FLOAT, "1.0", "1.0", 2, SETTINGS_SIMPLE}},
{"CustomCruiseLong", {PERSISTENT, FLOAT, "5.0", "5.0", 2, SETTINGS_SIMPLE}},
{"CustomPersonalities", {PERSISTENT, BOOL, "0", "0", 2}},
@@ -18,6 +18,26 @@ AVERAGE_ROAD_ROLL = 0.06 # ~3.4 degrees, 6% superelevation. higher actual roll
MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL) # ~2.4 m/s^2
class FordStockCruiseButton:
"""Resolve Ford's context-sensitive cancel/resume switch for stock ACC."""
def __init__(self):
self.pressed = False
self.cancel = False
self.resume = False
def update(self, pressed: bool, cruise_available: bool, cruise_enabled: bool) -> tuple[bool, bool]:
if pressed and not self.pressed:
self.cancel = cruise_available and cruise_enabled
self.resume = cruise_available and not cruise_enabled
elif not pressed:
self.cancel = False
self.resume = False
self.pressed = pressed
return self.cancel, self.resume
def apply_ford_angle(desired_angle_deg: float, current_angle_deg: float) -> float:
relative_angle = desired_angle_deg - current_angle_deg
return float(np.clip(relative_angle, -5.8, 5.8))
@@ -85,6 +105,7 @@ class CarController(CarControllerBase):
self.ford_lateral = None if CP.flags & FordFlags.LKA_STEERING else FordLateralController(CP)
self.ford_shadow_curvature = 0.0
self.ford_lateral_announced_mode = FordLateralMode.native
self.stock_cruise_button = FordStockCruiseButton()
def update(self, CC, CS, now_nanos, starpilot_toggles):
can_sends = []
@@ -100,9 +121,23 @@ class CarController(CarControllerBase):
self.ford_lateral.update_inputs()
### acc buttons ###
stock_cancel = False
stock_resume = False
if not self.CP.openpilotLongitudinalControl:
stock_cancel, stock_resume = self.stock_cruise_button.update(
bool(CS.buttons_stock_values["CcAslButtnCnclResPress"]),
CS.out.cruiseState.available,
CS.out.cruiseState.enabled,
)
if CC.cruiseControl.cancel:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True))
elif (stock_cancel or stock_resume) and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
can_sends.append(fordcan.create_button_msg(
self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=stock_cancel, resume=stock_resume))
can_sends.append(fordcan.create_button_msg(
self.packer, self.CAN.main, CS.buttons_stock_values, cancel=stock_cancel, resume=stock_resume))
elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True))
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True))
@@ -9,6 +9,7 @@ import pytest
from opendbc.car import Bus, gen_empty_fingerprint
from opendbc.can import CANPacker
from opendbc.car.ford import fordcan
from opendbc.car.ford.carcontroller import FordStockCruiseButton
from opendbc.car.gps import FORD_MACH_E_GPS_MESSAGES, get_car_gps_config, parse_ford_can_gps
from opendbc.car.structs import CarParams
from opendbc.car.fw_versions import build_fw_dict
@@ -19,6 +20,24 @@ from opendbc.car.ford.fingerprints import FW_VERSIONS
Ecu = CarParams.Ecu
def test_stock_cruise_button_latches_context_until_release():
button = FordStockCruiseButton()
assert button.update(True, cruise_available=True, cruise_enabled=True) == (True, False)
assert button.update(True, cruise_available=True, cruise_enabled=False) == (True, False)
assert button.update(False, cruise_available=True, cruise_enabled=False) == (False, False)
assert button.update(True, cruise_available=True, cruise_enabled=False) == (False, True)
assert button.update(True, cruise_available=True, cruise_enabled=True) == (False, True)
assert button.update(False, cruise_available=True, cruise_enabled=True) == (False, False)
def test_stock_cruise_button_ignores_press_with_cruise_master_off():
button = FordStockCruiseButton()
assert button.update(True, cruise_available=False, cruise_enabled=False) == (False, False)
ECU_ADDRESSES = {
Ecu.eps: 0x730, # Power Steering Control Module (PSCM)
Ecu.abs: 0x760, # Anti-Lock Brake System (ABS)
@@ -39,6 +39,7 @@ _STOP_START_PULSE_FRAMES = 30
_STOP_START_PULSE_PERIOD_FRAMES = 5
_AVH_STARTUP_DELAY_FRAMES = _STOP_START_STARTUP_DELAY_FRAMES
_AVH_STARTUP_DEADLINE_FRAMES = _STOP_START_STARTUP_DEADLINE_FRAMES
_AVH_PULSE_MESSAGES = 15 # Match the native 10 Hz AVH frame for roughly 1.5 seconds
def get_safety_CP():
@@ -92,6 +93,7 @@ class CarController(CarControllerBase):
self.avh_attempted = False
self.avh_request_started = False
self.avh_last_counter = None
self.avh_messages_sent = 0
def _stop_start_off_request(self, CC, CS, starpilot_toggles):
"""Send one bounded Subaru Stop/Start OFF request after ignition.
@@ -149,7 +151,7 @@ class CarController(CarControllerBase):
return msg
def _avh_on_request(self, CC, CS, starpilot_toggles):
"""Send one bounded Subaru AVH ON request after ignition.
"""Send a bounded Subaru AVH ON pulse after ignition.
The AVH button frame was identified on the 2025 Legacy only. Keep this
independent from Stop/Start so the existing Outback request is unchanged.
@@ -181,15 +183,20 @@ class CarController(CarControllerBase):
self.avh_request_started = True
self.avh_last_counter = int(avh_msg.get("COUNTER", 0)) % 0x10
if self.avh_messages_sent >= _AVH_PULSE_MESSAGES:
self.avh_attempted = True
return None
counter = int(avh_msg.get("COUNTER", 0)) % 0x10
if counter == self.avh_last_counter:
return None
self.avh_attempted = True
msg = subarucan.create_avh_control(
self.packer, avh_msg, raw_dat=avh_dat,
counter=counter, bus=CanBus.alt_for_cp(self.CP),
)
self.avh_last_counter = counter
self.avh_messages_sent += 1
return msg
def _reset_legacy_2025_handoff(self):
@@ -292,7 +292,7 @@ def test_stop_start_request_is_bounded_and_uses_live_dashlights(platform, expect
assert controller.stop_start_acknowledged
def test_avh_request_sets_observed_bit_and_is_bounded():
def test_avh_request_sets_observed_bit_and_pulses_at_native_rate():
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
controller = CarController({}, CP)
controller.frame = 101
@@ -318,6 +318,8 @@ def test_avh_request_sets_observed_bit_and_is_bounded():
out=SimpleNamespace(
standstill=True,
gearShifter=structs.CarState.GearShifter.park,
vEgoRaw=0.0,
steeringAngleDeg=0.0,
),
)
toggles = SimpleNamespace(subaru_stop_start_off=False, subaru_avh_on=True, subaru_sng=False)
@@ -341,6 +343,37 @@ def test_avh_request_sets_observed_bit_and_is_bounded():
assert parser.vl["AVH"]["AVH"] == 1
assert parser.vl["AVH"]["COUNTER"] == 0
controller.frame = 104
_, can_sends = controller.update(CC, CS, 0, toggles)
assert not any(msg[0] == 0x32b for msg in can_sends)
avh_msgs = []
for counter in range(1, 15):
CS.avh_msg["COUNTER"] = counter
raw_dat = bytearray.fromhex("14001c4208800000")
raw_dat[1] = counter
raw_dat[0] = ((0x32B & 0xFF) + ((0x32B >> 8) & 0xFF) + sum(raw_dat[1:])) & 0xFF
CS.avh_dat = bytes(raw_dat)
controller.frame = 103 + (counter * 10)
_, can_sends = controller.update(CC, CS, 0, toggles)
sent = [msg for msg in can_sends if msg[0] == 0x32b]
assert len(sent) == 1
avh_msgs.extend(sent)
assert len(avh_msgs) == 14
assert [msg[1][1] & 0x0F for msg in avh_msgs] == list(range(1, 15))
assert all(msg[1][5] & 0x20 for msg in avh_msgs)
assert not controller.avh_attempted
CS.avh_msg["COUNTER"] = 15
CS.avh_dat = bytes.fromhex("230f1c4208800000")
controller.frame = 253
_, can_sends = controller.update(CC, CS, 0, toggles)
assert not any(msg[0] == 0x32b for msg in can_sends)
assert controller.avh_attempted
CS.avh_msg["COUNTER"] = 0
CS.avh_dat = bytes.fromhex("14001c4208800000")
controller.frame = 131
_, can_sends = controller.update(CC, CS, 0, toggles)
assert not any(msg[0] == 0x32b for msg in can_sends)
+11 -2
View File
@@ -89,6 +89,8 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) {
static bool ford_lka_steering = false;
static bool ford_extended_lateral = false;
static bool ford_angle_mode = false;
static bool ford_longitudinal = false;
static bool ford_cancel_resume_button = false;
static int16_t ford_shadow_curvature = 0;
// Curvature rate limits
@@ -219,6 +221,10 @@ static void ford_rx_hook(const CANPacket_t *msg) {
acc_main_on = (cruise_state == 3U) || cruise_engaged;
}
if (msg->addr == FORD_Steering_Data_FD1) {
ford_cancel_resume_button = ((msg->data[2] >> 5) & 1U) != 0U;
}
}
}
@@ -276,7 +282,8 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
// if cancel button is pressed when cruise isn't engaged.
bool violation = false;
violation |= ((msg->data[1] >> 0) & 1U) && !cruise_engaged_prev; // Signal: CcAslButtnCnclPress (cancel)
violation |= ((msg->data[3] >> 1) & 1U) && !controls_allowed; // Signal: CcAsllButtnResPress (resume)
bool stock_resume_from_driver = !ford_longitudinal && acc_main_on && ford_cancel_resume_button;
violation |= ((msg->data[3] >> 1) & 1U) && !(controls_allowed || stock_resume_from_driver); // Signal: CcAsllButtnResPress (resume)
if (violation) {
tx = false;
@@ -415,6 +422,7 @@ static safety_config ford_init(uint16_t param) {
{.msg = {{FORD_Yaw_Data_FD1, 0, 8, 100U, .max_counter = 255U}, { 0 }, { 0 }}},
// These messages have no counter or checksum
{.msg = {{FORD_EngBrakeData, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{FORD_Steering_Data_FD1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{FORD_EngVehicleSpThrottle, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{FORD_DesiredTorqBrk, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
@@ -454,10 +462,11 @@ static safety_config ford_init(uint16_t param) {
ford_lka_steering = GET_FLAG(param, FORD_PARAM_LKA_STEERING);
ford_extended_lateral = false;
ford_angle_mode = false;
ford_cancel_resume_button = false;
ford_shadow_curvature = 0;
ford_desired_path_angle_last = 0;
bool ford_longitudinal = false;
ford_longitudinal = false;
#ifdef ALLOW_DEBUG
const uint16_t FORD_PARAM_LONGITUDINAL = 1;
@@ -75,6 +75,7 @@ class TestFordSafetyBase(common.CarSafetyTest):
MSG_LateralMotionControl2, MSG_IPMA_Data]}
STEER_MESSAGE = 0
STOCK_LONGITUDINAL = False
# Curvature control limits
LKA_STEERING = False
@@ -199,6 +200,17 @@ class TestFordSafetyBase(common.CarSafetyTest):
}
return self.packer.make_can_msg_safety("Steering_Data_FD1", bus, values)
def _combined_cancel_resume_msg(self, pressed: bool):
values = {"CcAslButtnCnclResPress": int(pressed)}
return self.packer.make_can_msg_safety("Steering_Data_FD1", 0, values)
def _pcm_main_on_msg(self, main_on: bool):
values = {
"BpedDrvAppl_D_Actl": 1,
"CcStat_D_Actl": 3 if main_on else 0,
}
return self.packer.make_can_msg_safety("EngBrakeData", 0, values)
def test_rx_hook(self):
# checksum, counter, and quality flag checks
for quality_flag in [True, False]:
@@ -381,6 +393,25 @@ class TestFordSafetyBase(common.CarSafetyTest):
for bus in (0, 2):
self.assertEqual(enabled, self._tx(self._acc_button_msg(Buttons.CANCEL, bus)))
def test_stock_resume_relay_requires_physical_button_and_cruise_main(self):
self.safety.set_controls_allowed(False)
self._rx(self._pcm_main_on_msg(True))
for bus in (0, 2):
self.assertFalse(self._tx(self._acc_button_msg(Buttons.RESUME, bus)))
self._rx(self._combined_cancel_resume_msg(True))
for bus in (0, 2):
self.assertEqual(self.STOCK_LONGITUDINAL, self._tx(self._acc_button_msg(Buttons.RESUME, bus)))
self._rx(self._combined_cancel_resume_msg(False))
for bus in (0, 2):
self.assertFalse(self._tx(self._acc_button_msg(Buttons.RESUME, bus)))
self._rx(self._pcm_main_on_msg(False))
self._rx(self._combined_cancel_resume_msg(True))
for bus in (0, 2):
self.assertFalse(self._tx(self._acc_button_msg(Buttons.RESUME, bus)))
def _toggle_aol(self, toggle_on):
# EngBrakeData, CcStat_D_Actl is the cruise state
# 3 is standby (main on), 5 is active (engaged)
@@ -394,6 +425,7 @@ class TestFordSafetyBase(common.CarSafetyTest):
class TestFordCANFDStockSafety(TestFordSafetyBase):
STEER_MESSAGE = MSG_LateralMotionControl2
STOCK_LONGITUDINAL = True
TX_MSGS = [
[MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0],
@@ -446,6 +478,7 @@ class TestFordCANFDStockSafety(TestFordSafetyBase):
class TestFordStockSafety(TestFordSafetyBase):
STEER_MESSAGE = MSG_LateralMotionControl
STOCK_LONGITUDINAL = True
TX_MSGS = [
[MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0],
+2 -1
View File
@@ -749,7 +749,8 @@ class Controls:
CC.latActive,
bool(self.sm.all_checks(['modelV2'])),
self.starpilot_toggles.lane_centering_pause_on_signal,
bool(CS.leftBlinker or CS.rightBlinker))
bool(CS.leftBlinker or CS.rightBlinker),
bool(CS.steeringPressed))
jerk_factor = 1.0
if self.starpilot_toggles.lane_change_pace < 10:
+5 -1
View File
@@ -33,7 +33,7 @@ class LaneCenteringController:
self._correction = 0.0
def update(self, model_curvature, model_v2, v_ego, enabled, offset, e2e_authority, lat_active, model_valid,
pause_on_signal=False, turn_signal_active=False) -> float:
pause_on_signal=False, turn_signal_active=False, driver_override=False) -> float:
model_curvature = float(model_curvature)
try:
@@ -52,6 +52,10 @@ class LaneCenteringController:
self.reset()
return model_curvature
if driver_override:
self.reset()
return model_curvature
if pause_on_signal and turn_signal_active:
self._correction = float(smooth_value(0.0, self._correction, _SIGNAL_RELEASE_TAU, dt=DT_CTRL))
return model_curvature + self._correction
@@ -288,11 +288,11 @@ GENESIS_G70_CURVE_UNWIND_LAT = 0.25
GENESIS_G70_CURVE_UNWIND_LAT_WIDTH = 0.12
GENESIS_G70_CURVE_UNWIND_JERK = 0.08
GENESIS_G70_CURVE_UNWIND_JERK_WIDTH = 0.08
GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.28
GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.18
GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.20
GENESIS_G70_UNWIND_FF_JERK = 0.10
GENESIS_G70_UNWIND_FF_JERK_WIDTH = 0.13
GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.34
GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.13
GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.17
GENESIS_G70_UNWIND_FF_JERK = 0.08
GENESIS_G70_UNWIND_FF_JERK_WIDTH = 0.11
GENESIS_G70_UNWIND_FF_SPEED = 18.0
GENESIS_G70_UNWIND_FF_SPEED_WIDTH = 3.0
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.15
@@ -1161,8 +1161,8 @@ TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX_WIDTH = 2.0
LEXUS_IS_PHASE_SCALE = 0.10
LEXUS_IS_TURN_IN_FF_BOOST_LEFT = 0.06
LEXUS_IS_TURN_IN_FF_BOOST_RIGHT = 0.06
LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.10
LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT = 0.16
LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.13
LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT = 0.20
LEXUS_IS_UNWIND_LAT_ONSET = 0.18
LEXUS_IS_UNWIND_LAT_WIDTH = 0.07
LEXUS_IS_UNWIND_SPEED_ONSET = 9.0
@@ -3227,9 +3227,10 @@ def get_genesis_g70_unwind_ff_scale(setpoint: float, measured_lateral_accel: flo
def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
tracking_error = abs(measured_lateral_accel - setpoint)
if tracking_error <= 0.0:
if (setpoint == 0.0 or setpoint * measured_lateral_accel <= 0.0 or
abs(measured_lateral_accel) <= abs(setpoint)):
return 1.0
tracking_error = abs(measured_lateral_accel - setpoint)
speed_weight = _sigmoid((v_ego - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED) /
GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH)
error_weight = _sigmoid((tracking_error - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR) /
+13 -3
View File
@@ -25,6 +25,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
get_honda_accord_lead_departure_tune,
get_honda_accord_stop_go_accel_cap,
get_honda_accord_stop_go_accel_rise_rate,
get_vision_low_speed_stop_buffer_lead_speed_limits,
get_toyota_rav4_tss2_lead_departure_tune,
get_toyota_rav4_tss2_lead_creep_tune,
get_force_stop_distance_bias,
@@ -44,6 +45,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
get_honda_crv_5g_early_radar_follow_cap,
get_standstill_gap_settle_max_extra_gap,
get_standstill_stopped_lead_guard_distance_margin,
get_standstill_stopped_lead_guard_max_ego_speed,
get_standstill_stopped_lead_guard_max_lead_speed,
is_ford_f150_lightning_stopped_radar_follow_lead,
get_tracked_lead_catchup_bias_gain,
@@ -1106,15 +1108,20 @@ class LongitudinalPlanner:
lead_speed = max(float(lead.vLead), 0.0)
relative_speed = float(v_ego) - lead_speed
max_lead_speed, hold_max_lead_speed = get_vision_low_speed_stop_buffer_lead_speed_limits(
self.CP,
VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED,
VISION_LOW_SPEED_STOP_BUFFER_HOLD_MAX_LEAD_SPEED,
)
closing_speed = max(0.0, v_ego - lead_speed)
entry_context = (
v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and
lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED and
lead_speed <= max_lead_speed and
closing_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED
)
hold_context = (
v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and
lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_HOLD_MAX_LEAD_SPEED and
lead_speed <= hold_max_lead_speed and
relative_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_HOLD_REL_SPEED
)
@@ -1669,7 +1676,10 @@ class LongitudinalPlanner:
release_ready, confident_depart_ready):
if lead is None or not lead.status or release_ready or confident_depart_ready:
return None
if float(v_ego) > STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED:
max_ego_speed = get_standstill_stopped_lead_guard_max_ego_speed(
self.CP, STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED,
)
if float(v_ego) > max_ego_speed:
return None
lead_radar = bool(getattr(lead, "radar", False))
@@ -19,6 +19,8 @@ HONDA_ACCORD_STOP_GO_MAX_LEAD_BRAKE = 0.25
HONDA_ACCORD_STOP_GO_MAX_LATERAL_OFFSET = 1.25
HONDA_ACCORD_STOP_GO_MIN_MODEL_PROB = 0.95
HONDA_ACCORD_STOP_GO_ACCEL_RISE_RATE = 4.0
HONDA_ACCORD_LOW_SPEED_STOP_MAX_LEAD_SPEED = 1.0
HONDA_ACCORD_STANDSTILL_GUARD_MAX_EGO_SPEED = 0.25
HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25
GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE = 1.75
FORD_LIGHTNING_LEAD_FOLLOW_JERK_SCALE = 1.35
@@ -292,6 +294,12 @@ def get_standstill_stopped_lead_guard_max_lead_speed(CP, default):
return float(default)
def get_standstill_stopped_lead_guard_max_ego_speed(CP, default):
if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD":
return HONDA_ACCORD_STANDSTILL_GUARD_MAX_EGO_SPEED
return float(default)
def get_tracked_lead_catchup_headway_margins(CP):
if is_honda_crv_5g(CP):
return (
@@ -564,6 +572,13 @@ def get_honda_accord_stop_go_accel_rise_rate(CP):
return 0.0
def get_vision_low_speed_stop_buffer_lead_speed_limits(CP, max_lead_speed, hold_max_lead_speed):
"""Keep the Accord's low-speed stop guard from treating a moving lead as stopped."""
if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD":
return HONDA_ACCORD_LOW_SPEED_STOP_MAX_LEAD_SPEED, HONDA_ACCORD_LOW_SPEED_STOP_MAX_LEAD_SPEED
return max_lead_speed, hold_max_lead_speed
def is_gm_silverado_early_follow_lead(CP, lead, v_ego):
"""Admit a credible centered vision lead before it becomes a close lead."""
if (
@@ -29,9 +29,9 @@ def _model(left=-1.8, right=1.8, model_y=0.0, lane_prob=0.9, lane_std=0.1, path_
def _update(controller, model, *, offset=0.0, authority=1.0, enabled=True, active=True, valid=True, speed=_V_EGO,
pause_on_signal=False, turn_signal_active=False):
pause_on_signal=False, turn_signal_active=False, driver_override=False):
return controller.update(0.0, model, speed, enabled, offset, authority, active, valid,
pause_on_signal, turn_signal_active)
pause_on_signal, turn_signal_active, driver_override)
def _converge(model, *, offset=0.0, authority=1.0):
@@ -78,6 +78,18 @@ def test_turn_signal_pause_can_be_disabled():
assert signaled == pytest.approx(output, abs=1e-7)
def test_driver_override_clears_filtered_correction():
model = _model(left=-1.5, right=2.1)
controller, centered = _converge(model, authority=0.0)
assert centered > 0.0
overridden = _update(controller, model, authority=0.0, driver_override=True)
assert overridden == 0.0
reacquired = _update(controller, model, authority=0.0)
assert 0.0 < reacquired < centered
@pytest.mark.parametrize(
"field,value",
[
@@ -1772,6 +1772,50 @@ def test_acc_mode_low_speed_vision_stop_buffer_brakes_harder_for_close_slow_visi
assert planner.output_a_target <= -2.7
def test_accord_low_speed_vision_stop_buffer_ignores_moving_stop_and_go_lead():
CP = CarInterface.get_non_essential_params(CAR.HONDA_ACCORD)
planner = LongitudinalPlanner(CP, init_v=3.43)
moving_lead = make_lead(
status=True, d_rel=7.3, v_lead=3.07, a_lead=0.23, radar=False, model_prob=1.0,
)
cap, active = planner.get_vision_low_speed_stop_buffer_cap(moving_lead, 3.43, -2.0)
assert cap is None
assert not active
def test_accord_low_speed_vision_stop_buffer_keeps_stopped_lead_guard():
CP = CarInterface.get_non_essential_params(CAR.HONDA_ACCORD)
planner = LongitudinalPlanner(CP, init_v=3.43)
stopped_lead = make_lead(
status=True, d_rel=6.0, v_lead=0.0, a_lead=0.0, radar=False, model_prob=1.0,
)
cap, active = planner.get_vision_low_speed_stop_buffer_cap(stopped_lead, 3.43, -2.0)
assert cap is not None
assert active
def test_accord_standstill_guard_waits_for_final_crawl():
accord = CarInterface.get_non_essential_params(CAR.HONDA_ACCORD)
civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
accord_planner = LongitudinalPlanner(accord, init_v=0.49)
civic_planner = LongitudinalPlanner(civic, init_v=0.49)
stopped_lead = make_lead(status=True, d_rel=7.3, v_lead=0.0, radar=False, model_prob=1.0)
assert accord_planner.get_standstill_stopped_lead_guard_cap(
stopped_lead, 0.49, -2.0, 5.5, False, False,
) is None
assert civic_planner.get_standstill_stopped_lead_guard_cap(
stopped_lead, 0.49, -2.0, 5.5, False, False,
) is not None
assert accord_planner.get_standstill_stopped_lead_guard_cap(
stopped_lead, 0.20, -2.0, 5.5, False, False,
) is not None
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
def test_acc_mode_low_speed_vision_stop_buffer_stays_latched_when_closure_softens_near_stop(model_version, monkeypatch):
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
@@ -38,6 +38,7 @@ def make_toggles(**overrides):
"acceleration_profile": ACCELERATION_PROFILES["STANDARD"],
"deceleration_profile": DECELERATION_PROFILES["ECO"],
"custom_accel_profile": False,
"custom_accel_profile_breakpoints": A_CRUISE_MAX_BP_CUSTOM,
"custom_accel_profile_values": [],
"ev_tuning": True,
"truck_tuning": False,
@@ -196,6 +197,20 @@ def test_traffic_mode_overrides_custom_accel_profile():
assert accel.max_accel == pytest.approx(get_max_accel_traffic(5.0))
def test_custom_accel_profile_uses_configured_breakpoints():
accel = StarPilotAcceleration(FakePlanner(v_cruise=25.0))
sm = make_sm()
breakpoints = [0.0, 10.0, 20.0]
accel.update(10.0, sm, make_toggles(
custom_accel_profile=True,
custom_accel_profile_breakpoints=breakpoints,
custom_accel_profile_values=[3.0, 1.25, 0.5],
))
assert accel.max_accel == pytest.approx(1.25)
def test_traffic_mode_sets_soft_cruise_decel_floor():
accel = StarPilotAcceleration(FakePlanner(v_cruise=25.0))
sm = make_sm(traffic_mode=True)
+34 -9
View File
@@ -33,6 +33,7 @@ FORD_CURVATURE_LOOKAHEAD = {
CAR.FORD_EXPLORER_MK6: 0.20,
}
ANGLE_HANDOFF_PRESS_SECONDS = 0.5
ANGLE_HANDOFF_RECOVERY_SECONDS = 0.75
HANDOFF_PAUSE_MIN_FRAMES = 3
HANDOFF_PAUSE_FRAMES = 6
HANDOFF_COOLDOWN_SECONDS = 2.0
@@ -132,6 +133,8 @@ class FordLateralController:
self.handoff_driver_override = False
self.angle_pause_frames = 0
self.angle_pause_cooldown = 0.0
self.angle_handoff_recovery = 0.0
self.angle_handoff_rebase = False
self.angle_stall_timer = 0.0
self.angle_stall_recoveries = 0
self._frame = 0
@@ -226,6 +229,8 @@ class FordLateralController:
self.handoff_driver_override = False
self.angle_pause_frames = 0
self.angle_pause_cooldown = 0.0
self.angle_handoff_recovery = 0.0
self.angle_handoff_rebase = False
self.angle_stall_timer = 0.0
self.angle_stall_recoveries = 0
@@ -236,12 +241,17 @@ class FordLateralController:
self.angle_pause_cooldown = max(0.0, self.angle_pause_cooldown - STEER_DT)
if CS.out.steeringPressed:
self.angle_handoff_recovery = 0.0
self.angle_handoff_rebase = False
self.handoff_press_timer += STEER_DT
self.handoff_driver_override |= self.handoff_press_timer + 1e-9 >= ANGLE_HANDOFF_PRESS_SECONDS
else:
if (self.handoff_driver_override and self.angle_pause_cooldown <= 0.0
and self.angle_pause_frames <= 0 and abs(self.path_angle_last) < HANDOFF_MAX_PATH_ANGLE):
self.angle_pause_frames = HANDOFF_PAUSE_FRAMES
if self.handoff_driver_override:
self.angle_handoff_recovery = ANGLE_HANDOFF_RECOVERY_SECONDS
self.angle_handoff_rebase = True
if (self.angle_pause_cooldown <= 0.0 and self.angle_pause_frames <= 0
and abs(self.path_angle_last) < HANDOFF_MAX_PATH_ANGLE):
self.angle_pause_frames = HANDOFF_PAUSE_FRAMES
self.handoff_driver_override = False
self.handoff_press_timer = 0.0
@@ -262,6 +272,15 @@ class FordLateralController:
return True
return False
def _recover_angle_handoff(self, requested: float, current: float) -> float:
if self.angle_handoff_recovery <= 0.0:
return requested
authority = 1.0 - self.angle_handoff_recovery / ANGLE_HANDOFF_RECOVERY_SECONDS
recovered = current + float(np.clip(authority, 0.0, 1.0)) * (requested - current)
self.angle_handoff_recovery = max(0.0, self.angle_handoff_recovery - STEER_DT)
return recovered
def _inactive_angle_result(self, current_curvature: float) -> FordLateralResult:
self.path_angle_last = 0.0
return FordLateralResult(shadow_curvature=current_curvature)
@@ -328,11 +347,9 @@ class FordLateralController:
self._reset_handoff()
return self._inactive_angle_result(current)
if self._manual_turn(CC, CS):
self._reset_handoff()
return self._inactive_angle_result(current)
if self._angle_handoff_pause_active(CS):
manual_turn = self._manual_turn(CC, CS)
handoff_pause = self._angle_handoff_pause_active(CS)
if manual_turn or handoff_pause:
return self._inactive_angle_result(current)
v_ego = float(CS.out.vEgoRaw)
@@ -349,6 +366,11 @@ class FordLateralController:
current + CarControllerParams.CURVATURE_ERROR))
deviation_limited = abs(requested - requested_before_deviation_limit) > 1e-9
measured_curvature = float(getattr(CC, "currentCurvature", current))
if not np.isfinite(measured_curvature):
measured_curvature = current
requested = self._recover_angle_handoff(requested, measured_curvature)
low_gain_high_speed, high_gain_high_speed = self._platform_angle_gains()
low_gain = float(np.interp(v_ego, [13.5, 26.82],
[1.0, low_gain_high_speed * self.angle_high_speed_damping]))
@@ -359,7 +381,10 @@ class FordLateralController:
path_angle = float(np.clip(requested * v_ego * gain, PATH_ANGLE_MIN, PATH_ANGLE_MAX))
max_delta = float(np.interp(v_ego, [9.0, 10.0, 15.0, 25.0], [0.055, 0.055, 0.0425, 0.009]))
path_angle = float(np.clip(path_angle, self.path_angle_last - max_delta, self.path_angle_last + max_delta))
if self.angle_handoff_rebase:
self.angle_handoff_rebase = False
else:
path_angle = float(np.clip(path_angle, self.path_angle_last - max_delta, self.path_angle_last + max_delta))
self.path_angle_last = path_angle
lane_change = self._lane_change()[0]
+40 -1
View File
@@ -4,7 +4,7 @@ from types import SimpleNamespace
import pytest
from opendbc.car.ford.values import CAR
from ..lateral import HANDOFF_PAUSE_FRAMES, HANDOFF_PAUSE_MIN_FRAMES, FordLateralController, HumanTurnDetector
from ..lateral import ANGLE_HANDOFF_RECOVERY_SECONDS, HANDOFF_PAUSE_FRAMES, HANDOFF_PAUSE_MIN_FRAMES, STEER_DT, FordLateralController, HumanTurnDetector
class FakeSubMaster(dict):
@@ -188,6 +188,45 @@ def test_angle_control_resumes_after_pscm_acknowledges_pause(controller):
CC, car_state(lateral_control_status=1), actuators).active
def test_long_manual_turn_still_resets_angle_control_on_release(controller):
controller.human_turn_enabled = True
CC = SimpleNamespace(latActive=True, currentCurvature=0.0)
actuators = SimpleNamespace(curvature=0.001)
for _ in range(40):
controller.update_angle(
CC, car_state(steering_pressed=True, steering_angle=50.0), actuators)
for _ in range(HANDOFF_PAUSE_FRAMES):
assert not controller.update_angle(CC, car_state(), actuators).active
assert controller.update_angle(CC, car_state(), actuators).active
def test_angle_handoff_reenters_from_measured_curvature(controller):
controller.human_turn_enabled = True
controller.angle_blend = 0.0
measured_curvature = 0.004
CC = SimpleNamespace(latActive=True, currentCurvature=measured_curvature)
actuators = SimpleNamespace(curvature=-0.005)
for _ in range(10):
controller.update_angle(
CC, car_state(speed=8.0, curvature=measured_curvature, steering_pressed=True, steering_angle=10.0), actuators)
for _ in range(HANDOFF_PAUSE_FRAMES):
assert not controller.update_angle(
CC, car_state(speed=8.0, curvature=measured_curvature), actuators).active
resumed = controller.update_angle(CC, car_state(speed=8.0, curvature=measured_curvature), actuators)
assert resumed.active
assert resumed.path_angle == pytest.approx(measured_curvature * 8.0 * 1.3)
recovery_frames = round(ANGLE_HANDOFF_RECOVERY_SECONDS / STEER_DT)
for _ in range(recovery_frames + 2):
recovered = controller.update_angle(CC, car_state(speed=8.0, curvature=measured_curvature), actuators)
assert recovered.path_angle < 0.0
def test_angle_control_recovers_from_bounded_tracking_stall(controller):
controller.human_turn_enabled = True
controller.angle_blend = 0.0
+100 -4
View File
@@ -3,6 +3,8 @@ from __future__ import annotations
import math
from openpilot.common.constants import CV
ACCELERATION_PROFILES = {
"STANDARD": 0,
"ECO": 1,
@@ -33,6 +35,32 @@ CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY = "CustomAccelProfileInitialized"
CUSTOM_ACCEL_PROFILE_VALUE_MIN = 0.0
CUSTOM_ACCEL_PROFILE_VALUE_MAX = 6.0
CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY = "CustomAccelProfileBreakpointsInitialized"
CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY = "CustomAccelProfilePointCount"
CUSTOM_ACCEL_PROFILE_MIN_POINTS = 2
CUSTOM_ACCEL_PROFILE_MAX_POINTS = 12
CUSTOM_ACCEL_PROFILE_BREAKPOINT_MIN_MPH = 0.0
CUSTOM_ACCEL_PROFILE_BREAKPOINT_MAX_MPH = 150.0
CUSTOM_ACCEL_PROFILE_DEFAULT_POINT_COUNT = len(A_CRUISE_MAX_BP_CUSTOM)
CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH = [
speed / CV.MPH_TO_MS
for speed in (*A_CRUISE_MAX_BP_CUSTOM, 45.0, 50.0, 55.0, 60.0, 65.0)
]
CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS = [
f"CustomAccelProfileBreakpoint{index + 1}MPH"
for index in range(CUSTOM_ACCEL_PROFILE_MAX_POINTS)
]
CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS = [
f"CustomAccelProfilePoint{index + 1}Accel"
for index in range(CUSTOM_ACCEL_PROFILE_MAX_POINTS)
]
CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS = [
CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY,
*CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS,
*CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS,
]
CUSTOM_ACCEL_PROFILE_EXTRA_POINT_VALUES = [0.55, 0.50, 0.45, 0.40, 0.35]
A_CRUISE_MAX_VALS_ECO_EV = [1.50, 1.34, 1.18, 1.02, 0.90, 0.74, 0.58]
A_CRUISE_MAX_VALS_STANDARD_EV = [2.00, 1.84, 1.64, 1.44, 1.24, 1.08, 0.84]
A_CRUISE_MAX_VALS_SPORT_EV = [2.50, 2.30, 2.06, 1.78, 1.54, 1.34, 1.10]
@@ -109,8 +137,11 @@ def get_accel_profile_curve_values(acceleration_profile, ev_tuning=True, truck_t
return list(A_CRUISE_MAX_VALS_STANDARD_GAS)
def interpolate_accel_profile(v_ego, curve_values):
return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, curve_values))
def interpolate_accel_profile(v_ego, curve_values, breakpoints=None):
curve_breakpoints = A_CRUISE_MAX_BP_CUSTOM if breakpoints is None else breakpoints
if len(curve_breakpoints) != len(curve_values) or len(curve_breakpoints) < CUSTOM_ACCEL_PROFILE_MIN_POINTS:
raise ValueError("Acceleration profile requires matching breakpoint and value arrays")
return float(akima_interp(v_ego, curve_breakpoints, curve_values))
def get_max_allowed_accel(v_ego, ev_tuning=True, truck_tuning=False):
@@ -154,10 +185,12 @@ def custom_accel_profile_is_initialized(initialized_flag, raw_values_by_key):
return False
def coerce_custom_accel_profile_values(raw_values, acceleration_profile, ev_tuning=True, truck_tuning=False):
def coerce_custom_accel_profile_values(raw_values, acceleration_profile, ev_tuning=True, truck_tuning=False, point_count=None):
defaults = get_accel_profile_curve_values(acceleration_profile, ev_tuning, truck_tuning)
expected_count = len(defaults) if point_count is None else point_count
values = []
for idx, default in enumerate(defaults):
for idx in range(expected_count):
default = defaults[min(idx, len(defaults) - 1)]
try:
value = float(raw_values[idx])
except (IndexError, TypeError, ValueError):
@@ -166,6 +199,63 @@ def coerce_custom_accel_profile_values(raw_values, acceleration_profile, ev_tuni
return values
def parse_custom_accel_profile_curve(raw_count, raw_breakpoints_mph, raw_values):
try:
numeric_count = float(_decode_param_value(raw_count))
except (TypeError, ValueError):
raise ValueError("Breakpoint count must be a whole number") from None
if not math.isfinite(numeric_count) or not numeric_count.is_integer():
raise ValueError("Breakpoint count must be a whole number")
count = int(numeric_count)
if not CUSTOM_ACCEL_PROFILE_MIN_POINTS <= count <= CUSTOM_ACCEL_PROFILE_MAX_POINTS:
raise ValueError(
f"Breakpoint count must be between {CUSTOM_ACCEL_PROFILE_MIN_POINTS} and {CUSTOM_ACCEL_PROFILE_MAX_POINTS}"
)
if len(raw_breakpoints_mph) < count or len(raw_values) < count:
raise ValueError("The configured breakpoint count exceeds the available curve points")
breakpoints_mph = []
values = []
for index in range(count):
try:
breakpoint_mph = float(_decode_param_value(raw_breakpoints_mph[index]))
value = float(_decode_param_value(raw_values[index]))
except (TypeError, ValueError):
raise ValueError(f"Curve point {index + 1} must contain numeric values") from None
if not math.isfinite(breakpoint_mph) or not CUSTOM_ACCEL_PROFILE_BREAKPOINT_MIN_MPH <= breakpoint_mph <= CUSTOM_ACCEL_PROFILE_BREAKPOINT_MAX_MPH:
bounds = f"{CUSTOM_ACCEL_PROFILE_BREAKPOINT_MIN_MPH:g} and {CUSTOM_ACCEL_PROFILE_BREAKPOINT_MAX_MPH:g} mph"
raise ValueError(f"Breakpoint {index + 1} must be between {bounds}")
if breakpoints_mph and breakpoint_mph <= breakpoints_mph[-1]:
raise ValueError("Breakpoint speeds must be strictly increasing")
if not math.isfinite(value) or not CUSTOM_ACCEL_PROFILE_VALUE_MIN <= value <= CUSTOM_ACCEL_PROFILE_VALUE_MAX:
bounds = f"{CUSTOM_ACCEL_PROFILE_VALUE_MIN:g} and {CUSTOM_ACCEL_PROFILE_VALUE_MAX:g} m/s²"
raise ValueError(f"Max acceleration at point {index + 1} must be between {bounds}")
breakpoints_mph.append(breakpoint_mph)
values.append(value)
return [speed * CV.MPH_TO_MS for speed in breakpoints_mph], values
def get_custom_accel_profile_curve_defaults(acceleration_profile, ev_tuning=True, truck_tuning=False):
profile_values = get_accel_profile_curve_values(acceleration_profile, ev_tuning, truck_tuning)
return {
CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY: CUSTOM_ACCEL_PROFILE_DEFAULT_POINT_COUNT,
**{
key: CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH[index]
for index, key in enumerate(CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS)
},
**{
key: (profile_values[index] if index < len(profile_values) else CUSTOM_ACCEL_PROFILE_EXTRA_POINT_VALUES[index - len(profile_values)])
for index, key in enumerate(CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS)
},
}
def _normalize_profile(value, profile_map, fallback):
if isinstance(value, bytes):
value = value.decode("utf-8", errors="replace")
@@ -181,6 +271,12 @@ def _normalize_profile(value, profile_map, fallback):
return fallback
def _decode_param_value(value):
if isinstance(value, bytes):
return value.decode("utf-8", errors="replace")
return value
def _coerce_bool(value):
if isinstance(value, bytes):
value = value.decode("utf-8", errors="replace")
@@ -685,7 +685,7 @@
{
"key": "CustomAccelProfile",
"label": "Custom Accel Profile",
"description": "Replace the built-in acceleration profile with your own per-speed max-acceleration values. Breakpoint speeds stay fixed, and the starting defaults mirror the currently selected acceleration profile plus EV or Truck tuning.",
"description": "Replace the built-in acceleration profile with your own speed breakpoints and maximum-acceleration values. The starting curve mirrors the selected acceleration profile plus EV or Truck tuning.",
"picker_description": "Uses custom maximum-acceleration values by speed.",
"data_type": "bool",
"ui_type": "toggle",
@@ -694,9 +694,34 @@
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfile0MPH",
"label": "0 mph",
"description": "Max acceleration in m/s² at the fixed 0 mph breakpoint.",
"key": "CustomAccelProfilePointCount",
"label": "Breakpoint Count",
"description": "Choose how many points define the custom acceleration curve.",
"data_type": "int",
"ui_type": "numeric",
"min": 2,
"max": 12,
"step": 1,
"parent_key": "CustomAccelProfile",
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfileBreakpoint1MPH",
"label": "Point 1 Speed",
"description": "Vehicle speed in mph for curve point 1. Breakpoint speeds must increase from one point to the next.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfilePoint1Accel",
"label": "Point 1 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 1.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
@@ -707,9 +732,22 @@
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfile11MPH",
"label": "11 mph",
"description": "Max acceleration in m/s² at the fixed 11 mph breakpoint.",
"key": "CustomAccelProfileBreakpoint2MPH",
"label": "Point 2 Speed",
"description": "Vehicle speed in mph for curve point 2.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfilePoint2Accel",
"label": "Point 2 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 2.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
@@ -720,22 +758,24 @@
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfile22MPH",
"label": "22 mph",
"description": "Max acceleration in m/s² at the fixed 22 mph breakpoint.",
"key": "CustomAccelProfileBreakpoint3MPH",
"label": "Point 3 Speed",
"description": "Vehicle speed in mph for curve point 3.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 6.0,
"step": 0.01,
"precision": 2,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [3, 4, 5, 6, 7, 8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfile34MPH",
"label": "34 mph",
"description": "Max acceleration in m/s² at the fixed 34 mph breakpoint.",
"key": "CustomAccelProfilePoint3Accel",
"label": "Point 3 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 3.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
@@ -743,25 +783,29 @@
"step": 0.01,
"precision": 2,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [3, 4, 5, 6, 7, 8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfile45MPH",
"label": "45 mph",
"description": "Max acceleration in m/s² at the fixed 45 mph breakpoint.",
"key": "CustomAccelProfileBreakpoint4MPH",
"label": "Point 4 Speed",
"description": "Vehicle speed in mph for curve point 4.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 6.0,
"step": 0.01,
"precision": 2,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [4, 5, 6, 7, 8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfile56MPH",
"label": "56 mph",
"description": "Max acceleration in m/s² at the fixed 56 mph breakpoint.",
"key": "CustomAccelProfilePoint4Accel",
"label": "Point 4 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 4.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
@@ -769,12 +813,29 @@
"step": 0.01,
"precision": 2,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [4, 5, 6, 7, 8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfile89MPH",
"label": "89 mph",
"description": "Max acceleration in m/s² at the fixed 89 mph breakpoint.",
"key": "CustomAccelProfileBreakpoint5MPH",
"label": "Point 5 Speed",
"description": "Vehicle speed in mph for curve point 5.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [5, 6, 7, 8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfilePoint5Accel",
"label": "Point 5 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 5.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
@@ -782,6 +843,218 @@
"step": 0.01,
"precision": 2,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [5, 6, 7, 8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfileBreakpoint6MPH",
"label": "Point 6 Speed",
"description": "Vehicle speed in mph for curve point 6.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [6, 7, 8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfilePoint6Accel",
"label": "Point 6 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 6.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 6.0,
"step": 0.01,
"precision": 2,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [6, 7, 8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfileBreakpoint7MPH",
"label": "Point 7 Speed",
"description": "Vehicle speed in mph for curve point 7.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [7, 8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfilePoint7Accel",
"label": "Point 7 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 7.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 6.0,
"step": 0.01,
"precision": 2,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [7, 8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfileBreakpoint8MPH",
"label": "Point 8 Speed",
"description": "Vehicle speed in mph for curve point 8.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfilePoint8Accel",
"label": "Point 8 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 8.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 6.0,
"step": 0.01,
"precision": 2,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [8, 9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfileBreakpoint9MPH",
"label": "Point 9 Speed",
"description": "Vehicle speed in mph for curve point 9.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfilePoint9Accel",
"label": "Point 9 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 9.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 6.0,
"step": 0.01,
"precision": 2,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [9, 10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfileBreakpoint10MPH",
"label": "Point 10 Speed",
"description": "Vehicle speed in mph for curve point 10.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfilePoint10Accel",
"label": "Point 10 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 10.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 6.0,
"step": 0.01,
"precision": 2,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [10, 11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfileBreakpoint11MPH",
"label": "Point 11 Speed",
"description": "Vehicle speed in mph for curve point 11.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfilePoint11Accel",
"label": "Point 11 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 11.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 6.0,
"step": 0.01,
"precision": 2,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [11, 12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfileBreakpoint12MPH",
"label": "Point 12 Speed",
"description": "Vehicle speed in mph for curve point 12.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 150.0,
"step": 0.1,
"precision": 1,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [12],
"settings_tier": "advanced"
},
{
"key": "CustomAccelProfilePoint12Accel",
"label": "Point 12 Max Accel",
"description": "Maximum acceleration in m/s² at curve point 12.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 6.0,
"step": 0.01,
"precision": 2,
"parent_key": "CustomAccelProfile",
"visible_when_key": "CustomAccelProfilePointCount",
"visible_when_values": [12],
"settings_tier": "advanced"
},
{
+6
View File
@@ -4,6 +4,10 @@ from __future__ import annotations
from cereal import log
from openpilot.common.params import Params, UnknownKeyName
from openpilot.starpilot.common.accel_profile import (
CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY,
CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS,
)
SAFE_MODE_PARAM = "SafeMode"
SAFE_MODE_BACKUP_PARAM = "SafeModeBackup"
@@ -70,6 +74,8 @@ SAFE_MODE_MANAGED_KEYS = (
"CustomAccelProfile45MPH",
"CustomAccelProfile56MPH",
"CustomAccelProfile89MPH",
CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY,
*CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS,
"LongitudinalActuatorDelay",
"MaxDesiredAcceleration",
"StartAccel",
+18
View File
@@ -33,8 +33,13 @@ from openpilot.starpilot.common.lateral_delay import full_lateral_delay
from openpilot.starpilot.common.lateral_only_experimental import lateral_only_experimental_available
from openpilot.starpilot.common.accel_profile import (
ACCELERATION_PROFILES,
A_CRUISE_MAX_BP_CUSTOM,
CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS,
CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY,
CUSTOM_ACCEL_PROFILE_PARAM_KEYS,
CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY,
CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY,
CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS,
CUSTOM_ACCEL_PROFILE_VALUE_MAX,
CUSTOM_ACCEL_PROFILE_VALUE_MIN,
DECELERATION_PROFILES,
@@ -42,6 +47,7 @@ from openpilot.starpilot.common.accel_profile import (
custom_accel_profile_is_initialized,
normalize_acceleration_profile,
normalize_deceleration_profile,
parse_custom_accel_profile_curve,
)
from openpilot.system.hardware import HARDWARE
from openpilot.system.hardware.hw import Paths
@@ -1277,6 +1283,18 @@ class StarPilotVariables:
]
else:
toggle.custom_accel_profile_values = [custom_accel_defaults[key] for key in CUSTOM_ACCEL_PROFILE_PARAM_KEYS]
toggle.custom_accel_profile_breakpoints = list(A_CRUISE_MAX_BP_CUSTOM)
if self.get_value(CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY):
try:
custom_breakpoints, custom_values = parse_custom_accel_profile_curve(
self.params_raw.get(CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY),
[self.params_raw.get(key) for key in CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS],
[self.params_raw.get(key) for key in CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS],
)
toggle.custom_accel_profile_breakpoints = custom_breakpoints
toggle.custom_accel_profile_values = custom_values
except ValueError:
pass
toggle.human_lane_changes = has_radar and self.get_value("HumanLaneChanges", condition=longitudinal_tuning)
toggle.nav_longitudinal_allowed = toggle.openpilot_longitudinal and self.get_value("NavLongitudinalAllowed", condition=longitudinal_tuning)
# Keep lead detection sensitivity normalized even when longitudinal tuning is disabled.
@@ -1,11 +1,16 @@
import pytest
from openpilot.common.constants import CV
from openpilot.starpilot.common.accel_profile import (
ACCELERATION_PROFILES,
A_CRUISE_MAX_BP_CUSTOM,
A_CRUISE_MAX_VALS_ECO_TRUCK,
A_CRUISE_MAX_VALS_STANDARD_TRUCK,
A_CRUISE_MAX_VALS_SPORT_PLUS_TRUCK,
A_CRUISE_MAX_VALS_SPORT_TRUCK,
get_accel_profile_curve_values,
interpolate_accel_profile,
parse_custom_accel_profile_curve,
)
@@ -33,3 +38,28 @@ def test_truck_profiles_remain_ordered():
for e, s, sp, spp in zip(eco, standard, sport, sport_plus, strict=True):
assert e < s < sp < spp
def test_custom_accel_profile_accepts_variable_breakpoint_count():
breakpoints, values = parse_custom_accel_profile_curve(3, [0.0, 20.0, 50.0], [2.0, 1.0, 0.5])
assert breakpoints == pytest.approx([0.0, 20.0 * CV.MPH_TO_MS, 50.0 * CV.MPH_TO_MS])
assert values == [2.0, 1.0, 0.5]
assert interpolate_accel_profile(breakpoints[1], values, breakpoints) == pytest.approx(1.0)
@pytest.mark.parametrize("breakpoints", ([0.0, 20.0, 20.0], [0.0, 30.0, 20.0]))
def test_custom_accel_profile_rejects_non_increasing_breakpoints(breakpoints):
with pytest.raises(ValueError, match="strictly increasing"):
parse_custom_accel_profile_curve(3, breakpoints, [2.0, 1.0, 0.5])
def test_custom_accel_profile_rejects_invalid_point_count():
with pytest.raises(ValueError, match="between 2 and 12"):
parse_custom_accel_profile_curve(1, [0.0], [2.0])
def test_default_accel_interpolation_still_uses_legacy_breakpoints():
values = [2.0, 1.8, 1.6, 1.4, 1.2, 1.0, 0.8]
assert interpolate_accel_profile(A_CRUISE_MAX_BP_CUSTOM[3], values) == pytest.approx(values[3])
@@ -10,6 +10,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN,
from openpilot.starpilot.common.accel_profile import (
ACCELERATION_PROFILES,
A_CRUISE_MAX_BP_CUSTOM,
A_CRUISE_MAX_VALS_TRAFFIC_ALL,
DECELERATION_PROFILES,
coerce_custom_accel_profile_values,
@@ -104,9 +105,16 @@ def get_max_accel_standard(v_ego, ev_tuning=True, truck_tuning=False):
def get_max_accel_traffic(v_ego):
return interpolate_accel_profile(v_ego, A_CRUISE_MAX_VALS_TRAFFIC_ALL)
def get_max_accel_custom(v_ego, custom_curve, acceleration_profile, ev_tuning=True, truck_tuning=False):
curve_values = coerce_custom_accel_profile_values(custom_curve, acceleration_profile, ev_tuning, truck_tuning)
return interpolate_accel_profile(v_ego, curve_values)
def get_max_accel_custom(v_ego, custom_curve, acceleration_profile, ev_tuning=True, truck_tuning=False, custom_breakpoints=None):
curve_breakpoints = A_CRUISE_MAX_BP_CUSTOM if custom_breakpoints is None else custom_breakpoints
curve_values = coerce_custom_accel_profile_values(
custom_curve,
acceleration_profile,
ev_tuning,
truck_tuning,
point_count=len(curve_breakpoints),
)
return interpolate_accel_profile(v_ego, curve_values, curve_breakpoints)
def get_max_allowed_accel(v_ego, ev_tuning=True, truck_tuning=False):
return float(get_profile_max_allowed_accel(v_ego, ev_tuning, truck_tuning))
@@ -252,6 +260,7 @@ class StarPilotAcceleration:
truck_tuning = getattr(starpilot_toggles, "truck_tuning", False)
custom_accel_profile = getattr(starpilot_toggles, "custom_accel_profile", False)
custom_accel_profile_values = getattr(starpilot_toggles, "custom_accel_profile_values", [])
custom_accel_profile_breakpoints = getattr(starpilot_toggles, "custom_accel_profile_breakpoints", A_CRUISE_MAX_BP_CUSTOM)
deceleration_profile = normalize_deceleration_profile(
getattr(starpilot_toggles, "deceleration_profile", DECELERATION_PROFILES["STANDARD"])
)
@@ -259,7 +268,14 @@ class StarPilotAcceleration:
if sm["starpilotCarState"].trafficModeEnabled:
self.max_accel = get_max_accel_traffic(v_ego)
elif custom_accel_profile:
self.max_accel = get_max_accel_custom(v_ego, custom_accel_profile_values, starpilot_toggles.acceleration_profile, ev_tuning, truck_tuning)
self.max_accel = get_max_accel_custom(
v_ego,
custom_accel_profile_values,
starpilot_toggles.acceleration_profile,
ev_tuning,
truck_tuning,
custom_accel_profile_breakpoints,
)
elif starpilot_toggles.map_acceleration:
# Drive mode is authoritative while mapping is on, normal gear included. Letting
# normal fall through to the profile param instead leaves the car on a stale eco
@@ -151,7 +151,7 @@ function fallbackDashboard(data, unit) {
longestUndistractedDrive: { value: "0.0 hours", detail: "No clean drives" },
cleanDriveStreak: { value: "0 drives", detail: "No clean drives" },
},
device: { status: "Parked", online: true, uptimeSeconds: null, cpuTempC: null },
device: { status: "Parked", online: true, uptimeSeconds: null, cpuTempC: null, gpuTempC: null },
storage: {
freeBytes: 0,
usedBytes: 0,
@@ -426,6 +426,7 @@ function renderStorage(storage) {
function renderVitals(device) {
const uptime = device.uptimeSeconds == null ? "unknown" : formatDuration(device.uptimeSeconds);
const cpu = device.cpuTempC == null ? "unknown" : `${formatInt(device.cpuTempC)} C`;
const gpu = device.gpuTempC == null ? "unknown" : `${formatInt(device.gpuTempC)} C`;
const lanIp = device.lanIp || "unknown";
const networkName = device.networkName || "No wireless connectivity";
return `
@@ -437,6 +438,7 @@ function renderVitals(device) {
<div><span>Network</span><strong>${escapeHtml(networkName)}</strong></div>
<div><span>Uptime</span><strong>${escapeHtml(uptime)}</strong></div>
<div><span>CPU temp</span><strong>${escapeHtml(cpu)}</strong></div>
<div><span>GPU temp</span><strong>${escapeHtml(gpu)}</strong></div>
</div>
</section>
`;
@@ -175,13 +175,33 @@ def _install_server_import_stubs():
model_manager.model_key_aliases = lambda value: [value]
theme_manager.THEME_COMPONENT_PARAMS = {}
def parse_custom_accel_profile_curve(count, breakpoints, values):
point_count = int(count)
active_breakpoints = [float(value) for value in breakpoints[:point_count]]
if any(current <= previous for previous, current in zip(active_breakpoints, active_breakpoints[1:], strict=False)):
raise ValueError("Breakpoint speeds must be strictly increasing")
return active_breakpoints, [float(value) for value in values[:point_count]]
sys.modules["openpilot.starpilot.common.accel_profile"] = _simple_module(
"openpilot.starpilot.common.accel_profile",
CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS=[f"CustomAccelProfileBreakpoint{index}MPH" for index in range(1, 13)],
CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY="CustomAccelProfileBreakpointsInitialized",
CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS=[
"CustomAccelProfilePointCount",
*[f"CustomAccelProfileBreakpoint{index}MPH" for index in range(1, 13)],
*[f"CustomAccelProfilePoint{index}Accel" for index in range(1, 13)],
],
CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH=[0.0, 11.2, 22.4, 33.6, 44.7, 55.9, 89.5, 100.7, 111.8, 123.0, 134.2, 145.4],
CUSTOM_ACCEL_PROFILE_DEFAULT_POINT_COUNT=7,
CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY="CustomAccelProfileInitialized",
CUSTOM_ACCEL_PROFILE_PARAM_KEYS=[],
CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY="CustomAccelProfilePointCount",
CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS=[f"CustomAccelProfilePoint{index}Accel" for index in range(1, 13)],
build_custom_accel_profile_defaults=lambda *args, **kwargs: {},
custom_accel_profile_is_initialized=lambda *args, **kwargs: False,
get_custom_accel_profile_curve_defaults=lambda *args, **kwargs: {},
normalize_acceleration_profile=lambda value: value,
parse_custom_accel_profile_curve=parse_custom_accel_profile_curve,
)
sys.modules["openpilot.starpilot.common.maps_catalog"] = _simple_module(
"openpilot.starpilot.common.maps_catalog",
@@ -984,6 +1004,20 @@ def test_cpu_temp_reader_uses_hardware_cpu_values(monkeypatch):
assert utilities._read_cpu_temp_c() == 57
def test_gpu_temp_reader_uses_hardware_gpu_values(monkeypatch):
hardware_module = _simple_module(
"openpilot.system.hardware",
HARDWARE=SimpleNamespace(
get_thermal_config=lambda: SimpleNamespace(
get_msg=lambda: {"gpuTempC": [41.2, 42.6], "cpuTempC": [56.0]}
)
),
)
monkeypatch.setitem(sys.modules, "openpilot.system.hardware", hardware_module)
assert utilities._read_gpu_temp_c() == 43
def test_cpu_temp_reader_ignores_non_cpu_thermal_zones(tmp_path):
cpu_zone = tmp_path / "thermal_zone0"
cpu_zone.mkdir()
@@ -998,6 +1032,20 @@ def test_cpu_temp_reader_ignores_non_cpu_thermal_zones(tmp_path):
assert utilities._read_cpu_temp_c(tmp_path) == 61
def test_gpu_temp_reader_ignores_non_gpu_thermal_zones(tmp_path):
gpu_zone = tmp_path / "thermal_zone0"
gpu_zone.mkdir()
(gpu_zone / "type").write_text("gpu0-usr", encoding="utf-8")
(gpu_zone / "temp").write_text("42000", encoding="utf-8")
cpu_zone = tmp_path / "thermal_zone1"
cpu_zone.mkdir()
(cpu_zone / "type").write_text("cpu0-silver-usr", encoding="utf-8")
(cpu_zone / "temp").write_text("61000", encoding="utf-8")
assert utilities._read_gpu_temp_c(tmp_path) == 42
def test_network_name_uses_wifi_ssid(monkeypatch):
monkeypatch.setattr(utilities, "HARDWARE", SimpleNamespace(get_network_type=lambda: 1))
monkeypatch.setattr(utilities, "_read_active_wifi_ssid", lambda: "Garage Wi-Fi")
@@ -1040,6 +1088,7 @@ def test_network_name_reports_no_wireless_connectivity(monkeypatch):
def test_device_summary_includes_network_name(monkeypatch):
monkeypatch.setattr(utilities, "_read_uptime_seconds", lambda: 120)
monkeypatch.setattr(utilities, "_read_cpu_temp_c", lambda: 55)
monkeypatch.setattr(utilities, "_read_gpu_temp_c", lambda: 42)
monkeypatch.setattr(utilities, "get_current_lan_ip", lambda: "192.168.1.10")
monkeypatch.setattr(utilities, "get_current_network_name", lambda: "Home Network")
@@ -1047,6 +1096,7 @@ def test_device_summary_includes_network_name(monkeypatch):
assert summary["networkName"] == "Home Network"
assert summary["lanIp"] == "192.168.1.10"
assert summary["gpuTempC"] == 42
def test_persistent_loader_accepts_decoded_param_dict():
@@ -146,6 +146,29 @@ def test_curve_speed_controller_readouts_are_display_only_and_nested():
assert readout["settings_tier"] == "simple"
def test_custom_accel_profile_exposes_variable_breakpoints():
longitudinal = _params_by_section(_layout())["Longitudinal (Speed & Following)"]
point_count = longitudinal["CustomAccelProfilePointCount"]
assert point_count["parent_key"] == "CustomAccelProfile"
assert point_count["min"] == 2
assert point_count["max"] == 12
assert _declared_default("CustomAccelProfilePointCount") == "7"
for point in range(1, 13):
speed = longitudinal[f"CustomAccelProfileBreakpoint{point}MPH"]
accel = longitudinal[f"CustomAccelProfilePoint{point}Accel"]
assert speed["parent_key"] == "CustomAccelProfile"
assert accel["parent_key"] == "CustomAccelProfile"
assert _declared_default(speed["key"]) is not None
assert _declared_default(accel["key"]) is not None
if point > 2:
expected_counts = list(range(point, 13))
assert speed["visible_when_values"] == expected_counts
assert accel["visible_when_values"] == expected_counts
def test_every_galaxy_setting_has_a_shared_settings_tier():
layout = _layout()
tiers = {
@@ -567,6 +567,68 @@ def test_ford_lateral_mode_is_editable_through_galaxy(monkeypatch):
assert ("FordLateralMode", "2") in fake_params.writes
def test_custom_accel_breakpoint_update_validates_the_complete_curve(monkeypatch):
point_count_key = the_galaxy.CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY
breakpoint_keys = the_galaxy.CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS
value_keys = the_galaxy.CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS
values = {
the_galaxy.CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY: True,
point_count_key: 3,
**dict(zip(breakpoint_keys, [0.0, 20.0, 40.0] + [50.0] * 9, strict=True)),
**dict.fromkeys(value_keys, 1.0),
}
client, fake_params = _params_client(monkeypatch, values, "tici")
monkeypatch.setattr(the_galaxy, "_get_param_type_info", lambda: ({breakpoint_keys[1]}, {breakpoint_keys[1]: float}))
monkeypatch.setattr(the_galaxy, "_get_custom_accel_profile_breakpoints_initialized", lambda: True)
monkeypatch.setattr(the_galaxy, "_get_default_param_values", dict)
valid_response = client.put("/api/params", json={"key": breakpoint_keys[1], "value": 25.0})
assert valid_response.status_code == 200
assert fake_params.values[breakpoint_keys[1]] == "25.0"
invalid_response = client.put("/api/params", json={"key": breakpoint_keys[1], "value": 45.0})
assert invalid_response.status_code == 400
assert "strictly increasing" in invalid_response.get_json()["error"]
assert fake_params.values[breakpoint_keys[1]] == "25.0"
def test_uninitialized_custom_accel_curve_returns_zero_mph_breakpoint(monkeypatch):
client, fake_params = _params_client(monkeypatch, {}, "tici")
monkeypatch.setattr(the_galaxy, "_params_live_raw", fake_params)
response = client.get(f"/api/params?key={the_galaxy.CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS[0]}")
assert response.status_code == 200
assert response.get_data(as_text=True) == "0.0"
def test_first_breakpoint_edit_seeds_existing_legacy_accel_values(monkeypatch):
legacy_keys = [f"CustomAccelProfile{speed}MPH" for speed in (0, 11, 22, 34, 45, 56, 89)]
breakpoint_keys = the_galaxy.CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS
value_keys = the_galaxy.CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS
legacy_values = [3.2, 2.7, 2.1, 1.6, 1.1, 0.75, 0.5]
defaults = {
the_galaxy.CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY: 7,
**dict(zip(breakpoint_keys, the_galaxy.CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH, strict=True)),
**dict.fromkeys(value_keys, 0.35),
}
values = dict(zip(legacy_keys, legacy_values, strict=True))
client, fake_params = _params_client(monkeypatch, values, "tici")
monkeypatch.setattr(the_galaxy, "_params_live_raw", fake_params)
monkeypatch.setattr(the_galaxy, "CUSTOM_ACCEL_PROFILE_PARAM_KEYS", legacy_keys)
monkeypatch.setattr(the_galaxy, "_get_param_type_info", lambda: ({breakpoint_keys[1]}, {breakpoint_keys[1]: float}))
monkeypatch.setattr(the_galaxy, "_get_custom_accel_profile_initialized", lambda: True)
monkeypatch.setattr(the_galaxy, "_get_custom_accel_profile_breakpoints_initialized", lambda: False)
monkeypatch.setattr(the_galaxy, "_get_default_param_values", lambda: defaults)
response = client.put("/api/params", json={"key": breakpoint_keys[1], "value": 18.0})
assert response.status_code == 200
assert [float(fake_params.values[key]) for key in value_keys[:7]] == legacy_values
assert fake_params.values[breakpoint_keys[1]] == "18.0"
assert fake_params.values[the_galaxy.CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY] is True
def test_favorite_slot_options_include_virtual_cruise_actions(monkeypatch):
monkeypatch.setattr(the_galaxy, "_favorite_slot_options", None)
monkeypatch.setattr(the_galaxy, "_get_param_type_info", lambda: (set(), {}))
+99 -2
View File
@@ -59,11 +59,20 @@ from openpilot.starpilot.assets.model_manager import (
)
from openpilot.starpilot.assets.theme_manager import HOLIDAY_THEME_PATH, THEME_COMPONENT_PARAMS
from openpilot.starpilot.common.accel_profile import (
CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS,
CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY,
CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS,
CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH,
CUSTOM_ACCEL_PROFILE_DEFAULT_POINT_COUNT,
CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY,
CUSTOM_ACCEL_PROFILE_PARAM_KEYS,
CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY,
CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS,
build_custom_accel_profile_defaults,
custom_accel_profile_is_initialized,
get_custom_accel_profile_curve_defaults,
normalize_acceleration_profile,
parse_custom_accel_profile_curve,
)
from openpilot.starpilot.common.maps_catalog import (
MAPS_CATALOG,
@@ -1859,6 +1868,9 @@ _TROUBLESHOOT_ADVANCED_LONGITUDINAL_KEYS = [
"TrailerLoad",
"CustomAccelProfile",
*CUSTOM_ACCEL_PROFILE_PARAM_KEYS,
CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY,
*CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS,
*CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS,
"LongitudinalActuatorDelay",
"StartAccel",
"VEgoStarting",
@@ -3615,12 +3627,15 @@ def _get_runtime_default_param_overrides():
acceleration_profile_raw if not _is_blank_param_raw(acceleration_profile_raw) else static_defaults.get("AccelerationProfile", "0")
)
overrides.update(build_custom_accel_profile_defaults(acceleration_profile, ev_tuning, truck_tuning))
overrides.update(get_custom_accel_profile_curve_defaults(acceleration_profile, ev_tuning, truck_tuning))
return overrides
def _get_current_param_value(key, value_type, defaults_lookup=None):
if key == CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY:
return _get_custom_accel_profile_initialized()
if key == CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY:
return _get_custom_accel_profile_breakpoints_initialized()
if key == "LeadIndicator":
return _get_lead_indicator_enabled(defaults_lookup)
@@ -3633,6 +3648,11 @@ def _get_current_param_value(key, value_type, defaults_lookup=None):
defaults_lookup = _get_default_param_values()
return _coerce_param_value(defaults_lookup.get(key), value_type)
if key in CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS and not _get_custom_accel_profile_breakpoints_initialized():
if defaults_lookup is None:
defaults_lookup = _get_default_param_values()
return _coerce_param_value(_get_legacy_compatible_curve_value(key, defaults_lookup), value_type)
raw_value = _safe_params_get_live_raw(key)
if _is_blank_param_raw(raw_value):
if defaults_lookup is None:
@@ -3665,12 +3685,43 @@ def _get_custom_accel_profile_initialized():
raw_values,
)
def _get_custom_accel_profile_breakpoints_initialized():
return _coerce_param_value(_safe_params_get_live_raw(CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY), bool)
def _get_legacy_compatible_curve_value(key, defaults_lookup):
if key == CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY:
return CUSTOM_ACCEL_PROFILE_DEFAULT_POINT_COUNT
if key in CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS:
index = CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS.index(key)
return CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH[index]
if key in CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS:
index = CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS.index(key)
if index < len(CUSTOM_ACCEL_PROFILE_PARAM_KEYS):
legacy_key = CUSTOM_ACCEL_PROFILE_PARAM_KEYS[index]
return _get_current_param_value(legacy_key, float, defaults_lookup)
return defaults_lookup.get(key)
def _seed_custom_accel_profile_curve(defaults_lookup):
seeded = {}
for key in CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS:
value = _get_legacy_compatible_curve_value(key, defaults_lookup)
params.put(key, _serialize_param_write_value(value))
seeded[key] = value
params.put_bool(CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY, True)
return seeded
def _serialize_param_write_value(raw_value):
if isinstance(raw_value, bool):
return "1" if raw_value else "0"
if isinstance(raw_value, bytes):
return raw_value.decode("utf-8", errors="replace")
return str(raw_value or "")
return "" if raw_value is None else str(raw_value)
def _offroad_excessive_actuation_type():
alert = _safe_params_get_live_raw("Offroad_ExcessiveActuation")
@@ -5616,6 +5667,44 @@ def setup(app):
return jsonify({"error": f"{key} must be between {minimum} and {maximum}."}), 400
str_val = str(numeric)
if key in CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS:
try:
numeric = float(data["value"])
if key == CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY:
if not math.isfinite(numeric) or not numeric.is_integer():
raise ValueError("Breakpoint count must be a whole number")
numeric = int(numeric)
elif not math.isfinite(numeric):
raise ValueError(f"{key} must be numeric")
defaults_lookup = _get_default_param_values()
initialized = _get_custom_accel_profile_breakpoints_initialized()
candidate = {}
for curve_key in CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS:
if initialized:
candidate[curve_key] = _safe_params_get(curve_key, encoding="utf-8")
else:
candidate[curve_key] = _get_legacy_compatible_curve_value(curve_key, defaults_lookup)
candidate[key] = numeric
parse_custom_accel_profile_curve(
candidate[CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY],
[candidate[curve_key] for curve_key in CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS],
[candidate[curve_key] for curve_key in CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS],
)
except (TypeError, ValueError) as exc:
return jsonify({"error": str(exc)}), 400
updated = _seed_custom_accel_profile_curve(defaults_lookup) if not initialized else {}
params.put(key, _serialize_param_write_value(numeric))
params.put_bool(CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY, True)
updated.update({key: numeric, CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY: True})
update_starpilot_toggles()
return jsonify({
"message": "Custom acceleration curve updated.",
"updated": updated,
}), 200
if key == "AlphaLongitudinalEnabled":
if not _get_alpha_longitudinal_available():
return jsonify({"error": "Alpha Longitudinal is not available for the detected vehicle."}), 403
@@ -5756,13 +5845,16 @@ def setup(app):
params.put_bool(key, enabled)
updated = {key: enabled}
defaults_lookup = _get_default_param_values()
if enabled and not _get_custom_accel_profile_initialized():
defaults_lookup = _get_default_param_values()
for custom_key in CUSTOM_ACCEL_PROFILE_PARAM_KEYS:
custom_value = defaults_lookup[custom_key]
params.put(custom_key, _serialize_param_write_value(custom_value))
updated[custom_key] = float(custom_value)
params.put_bool(CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY, True)
if enabled and not _get_custom_accel_profile_breakpoints_initialized():
updated.update(_seed_custom_accel_profile_curve(defaults_lookup))
updated[CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY] = True
update_starpilot_toggles()
return jsonify({
@@ -5970,6 +6062,11 @@ def setup(app):
return _serialize_param_write_value(defaults_lookup.get(request_key)), 200
if request_key == CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY:
return _serialize_param_write_value(_get_custom_accel_profile_initialized()), 200
if request_key in CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS and not _get_custom_accel_profile_breakpoints_initialized():
defaults_lookup = _get_default_param_values()
return _serialize_param_write_value(_get_legacy_compatible_curve_value(request_key, defaults_lookup)), 200
if request_key == CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY:
return _serialize_param_write_value(_get_custom_accel_profile_breakpoints_initialized()), 200
if request_key == "LeadIndicator":
return _serialize_param_write_value(_get_lead_indicator_enabled()), 200
if request_key == "IsRHD" and not params.get_bool("IsRHDOverride"):
+26 -8
View File
@@ -2843,7 +2843,7 @@ def _normalize_temp_c(value):
return raw if 0 < raw < 150 else None
def _read_hardware_cpu_temps():
def _read_hardware_component_temps(component):
try:
from openpilot.system.hardware import HARDWARE
thermal_config = HARDWARE.get_thermal_config()
@@ -2851,18 +2851,26 @@ def _read_hardware_cpu_temps():
except Exception:
return []
cpu_temps = thermal_msg.get("cpuTempC", [])
if not isinstance(cpu_temps, (list, tuple)):
cpu_temps = [cpu_temps]
temps = thermal_msg.get(f"{component}TempC", [])
if not isinstance(temps, (list, tuple)):
temps = [temps]
return [
temp for temp in (_normalize_temp_c(value) for value in cpu_temps)
temp for temp in (_normalize_temp_c(value) for value in temps)
if temp is not None
]
def _read_cpu_temp_c(thermal_root=None):
def _read_hardware_cpu_temps():
return _read_hardware_component_temps("cpu")
def _read_hardware_gpu_temps():
return _read_hardware_component_temps("gpu")
def _read_component_temp_c(component, thermal_root=None):
if thermal_root is None:
hardware_temps = _read_hardware_cpu_temps()
hardware_temps = _read_hardware_component_temps(component)
if hardware_temps:
return round(max(hardware_temps))
thermal_root = Path("/sys/class/thermal")
@@ -2878,7 +2886,7 @@ def _read_cpu_temp_c(thermal_root=None):
zone_type = temp_path.with_name("type").read_text(encoding="utf-8").strip().lower()
except Exception:
zone_type = ""
if "cpu" not in zone_type:
if component not in zone_type:
continue
try:
raw = temp_path.read_text().strip()
@@ -2891,10 +2899,19 @@ def _read_cpu_temp_c(thermal_root=None):
return round(max(values)) if values else None
def _read_cpu_temp_c(thermal_root=None):
return _read_component_temp_c("cpu", thermal_root)
def _read_gpu_temp_c(thermal_root=None):
return _read_component_temp_c("gpu", thermal_root)
def _build_device_summary(params_obj):
is_onroad = _params_get_bool(params_obj, "IsOnroad")
uptime_seconds = _read_uptime_seconds()
cpu_temp_c = _read_cpu_temp_c()
gpu_temp_c = _read_gpu_temp_c()
lan_ip = get_current_lan_ip()
network_name = get_current_network_name()
return {
@@ -2902,6 +2919,7 @@ def _build_device_summary(params_obj):
"online": True,
"uptimeSeconds": uptime_seconds,
"cpuTempC": cpu_temp_c,
"gpuTempC": gpu_temp_c,
"lanIp": lan_ip,
"networkName": network_name,
}