mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-10 02:03:57 +08:00
what's that mean, dumby
This commit is contained in:
@@ -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)
|
||||
|
||||
@@ -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],
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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) /
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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]
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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"
|
||||
},
|
||||
{
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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(), {}))
|
||||
|
||||
@@ -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"):
|
||||
|
||||
@@ -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,
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user