mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-20 15:54:13 +08:00
This Ain't The Armory
This commit is contained in:
@@ -22,8 +22,12 @@ _LEGACY_2025_REENGAGE_MAX_STEER_RATE = 2.0
|
||||
_LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA = 1.0
|
||||
_LEGACY_2025_RECLAIM_FRAMES = 36
|
||||
_LEGACY_2025_RECLAIM_EXPONENT = 2.5
|
||||
_ANGLE_REENGAGE_MAX_STEER_RATE = 3.0
|
||||
_ANGLE_REENGAGE_SETTLE_FRAMES = 2
|
||||
_ASCENT_OVERRIDE_HOLD_FRAMES = 10
|
||||
_ASCENT_REENGAGE_SETTLE_FRAMES = 8
|
||||
_ASCENT_REENGAGE_MAX_STEER_RATE = 2.0
|
||||
_ASCENT_REENGAGE_MAX_ANGLE_DELTA = 1.0
|
||||
_ASCENT_RECLAIM_FRAMES = 36
|
||||
_ASCENT_RECLAIM_EXPONENT = 2.5
|
||||
|
||||
|
||||
def get_safety_CP():
|
||||
@@ -37,7 +41,6 @@ class CarController(CarControllerBase):
|
||||
self.apply_torque_last = 0
|
||||
self.apply_steer_last = 0
|
||||
self.driver_override = False
|
||||
self.angle_reengage_settle_frames = 0
|
||||
self.legacy_2025_lkas_active = False
|
||||
self.legacy_2025_handoff_active = False
|
||||
self.legacy_2025_override_hold_frames = 0
|
||||
@@ -45,6 +48,13 @@ class CarController(CarControllerBase):
|
||||
self.legacy_2025_reengage_reference_angle = 0.0
|
||||
self.legacy_2025_reclaim_frames = 0
|
||||
self.legacy_2025_reclaim_start_angle = 0.0
|
||||
self.ascent_lkas_active = False
|
||||
self.ascent_handoff_active = False
|
||||
self.ascent_override_hold_frames = 0
|
||||
self.ascent_reengage_settle_frames = 0
|
||||
self.ascent_reengage_reference_angle = 0.0
|
||||
self.ascent_reclaim_frames = 0
|
||||
self.ascent_reclaim_start_angle = 0.0
|
||||
|
||||
self.cruise_button_prev = 0
|
||||
self.steer_rate_counter = 0
|
||||
@@ -125,6 +135,69 @@ class CarController(CarControllerBase):
|
||||
self.legacy_2025_reclaim_frames -= 1
|
||||
return target_angle
|
||||
|
||||
def _reset_ascent_handoff(self):
|
||||
self.ascent_handoff_active = False
|
||||
self.ascent_override_hold_frames = 0
|
||||
self.ascent_reengage_settle_frames = 0
|
||||
self.ascent_reengage_reference_angle = 0.0
|
||||
self.ascent_reclaim_frames = 0
|
||||
self.ascent_reclaim_start_angle = 0.0
|
||||
|
||||
def _ascent_manual_handoff(self, CS, lat_active):
|
||||
if not lat_active:
|
||||
self._reset_ascent_handoff()
|
||||
return False
|
||||
|
||||
if CS.out.steeringPressed:
|
||||
self.ascent_handoff_active = True
|
||||
self.ascent_override_hold_frames = _ASCENT_OVERRIDE_HOLD_FRAMES
|
||||
self.ascent_reengage_settle_frames = 0
|
||||
self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
self.ascent_reclaim_frames = 0
|
||||
return True
|
||||
|
||||
if not self.ascent_handoff_active and not self.ascent_lkas_active and \
|
||||
abs(CS.out.steeringRateDeg) > _ASCENT_REENGAGE_MAX_STEER_RATE:
|
||||
self.ascent_handoff_active = True
|
||||
self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
|
||||
if not self.ascent_handoff_active:
|
||||
return False
|
||||
|
||||
if self.ascent_override_hold_frames > 0:
|
||||
self.ascent_override_hold_frames -= 1
|
||||
if self.ascent_override_hold_frames == 0:
|
||||
self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
return True
|
||||
|
||||
wheel_stable = abs(CS.out.steeringRateDeg) <= _ASCENT_REENGAGE_MAX_STEER_RATE and \
|
||||
abs(CS.out.steeringAngleDeg - self.ascent_reengage_reference_angle) <= _ASCENT_REENGAGE_MAX_ANGLE_DELTA
|
||||
if wheel_stable:
|
||||
self.ascent_reengage_settle_frames += 1
|
||||
else:
|
||||
self.ascent_reengage_settle_frames = 0
|
||||
self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg
|
||||
|
||||
if self.ascent_reengage_settle_frames < _ASCENT_REENGAGE_SETTLE_FRAMES:
|
||||
return True
|
||||
|
||||
self.ascent_handoff_active = False
|
||||
self.ascent_reengage_settle_frames = 0
|
||||
self.ascent_reclaim_frames = _ASCENT_RECLAIM_FRAMES
|
||||
self.ascent_reclaim_start_angle = CS.out.steeringAngleDeg
|
||||
return True
|
||||
|
||||
def _ascent_reclaim_target(self, target_angle):
|
||||
if self.ascent_reclaim_frames <= 0:
|
||||
return target_angle
|
||||
|
||||
progress = (_ASCENT_RECLAIM_FRAMES - self.ascent_reclaim_frames + 1) / _ASCENT_RECLAIM_FRAMES
|
||||
eased_progress = progress ** _ASCENT_RECLAIM_EXPONENT
|
||||
target_angle = self.ascent_reclaim_start_angle + eased_progress * \
|
||||
(target_angle - self.ascent_reclaim_start_angle)
|
||||
self.ascent_reclaim_frames -= 1
|
||||
return target_angle
|
||||
|
||||
def lateral_angle(self, CC, CS):
|
||||
if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025:
|
||||
mads_only = CC.latActive and not CC.enabled
|
||||
@@ -136,9 +209,6 @@ class CarController(CarControllerBase):
|
||||
manual_handoff = self._legacy_2025_manual_handoff(CS, lkas_available)
|
||||
lkas_active = lkas_available and not manual_handoff
|
||||
|
||||
if lkas_active and not self.legacy_2025_lkas_active:
|
||||
self.apply_steer_last = CS.out.steeringAngleDeg
|
||||
|
||||
steer_target = self._legacy_2025_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
|
||||
apply_steer = apply_std_steer_angle_limits(
|
||||
steer_target,
|
||||
@@ -152,20 +222,29 @@ class CarController(CarControllerBase):
|
||||
self.legacy_2025_lkas_active = lkas_active
|
||||
return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
|
||||
|
||||
if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
|
||||
manual_handoff = self._ascent_manual_handoff(CS, CC.latActive)
|
||||
lkas_active = CC.latActive and not manual_handoff
|
||||
|
||||
if lkas_active and not self.ascent_lkas_active:
|
||||
self.apply_steer_last = CS.out.steeringAngleDeg
|
||||
|
||||
steer_target = self._ascent_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg
|
||||
apply_steer = apply_std_steer_angle_limits(
|
||||
steer_target,
|
||||
self.apply_steer_last,
|
||||
CS.out.vEgoRaw,
|
||||
CS.out.steeringAngleDeg,
|
||||
lkas_active,
|
||||
self.p.FIXED_ANGLE_LIMITS,
|
||||
)
|
||||
self.apply_steer_last = apply_steer
|
||||
self.ascent_lkas_active = lkas_active
|
||||
return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
|
||||
|
||||
abs_torque = abs(CS.out.steeringTorque)
|
||||
if abs_torque > self.p.STEER_OVERRIDE_TORQUE_HIGH:
|
||||
self.driver_override = True
|
||||
self.angle_reengage_settle_frames = 0
|
||||
elif self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023 and self.driver_override:
|
||||
wheel_settled = abs(CS.out.steeringRateDeg) <= _ANGLE_REENGAGE_MAX_STEER_RATE
|
||||
if abs_torque < self.p.STEER_OVERRIDE_TORQUE_LOW and wheel_settled:
|
||||
self.angle_reengage_settle_frames += 1
|
||||
else:
|
||||
self.angle_reengage_settle_frames = 0
|
||||
|
||||
if self.angle_reengage_settle_frames >= _ANGLE_REENGAGE_SETTLE_FRAMES:
|
||||
self.driver_override = False
|
||||
self.angle_reengage_settle_frames = 0
|
||||
elif abs_torque < self.p.STEER_OVERRIDE_TORQUE_LOW:
|
||||
self.driver_override = False
|
||||
|
||||
|
||||
@@ -40,8 +40,8 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM.value
|
||||
if ret.flags & SubaruFlags.D_PLATFORM_CAMERA:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value
|
||||
if candidate == CAR.SUBARU_LEGACY_2025:
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS.value
|
||||
if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023):
|
||||
ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.steerActuatorDelay = 0.1
|
||||
|
||||
@@ -185,7 +185,7 @@ def test_legacy_2025_uses_gen2_angle_bus_layout():
|
||||
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM)
|
||||
assert not (CP.flags & SubaruFlags.D_PLATFORM_CAMERA)
|
||||
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM_CAMERA)
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.FIXED_ANGLE_LIMITS
|
||||
assert CanBus.main_for_cp(CP) == CanBus.main
|
||||
assert CanBus.angle_for_cp(CP) == CanBus.main
|
||||
assert parsers[Bus.pt].bus == CanBus.main
|
||||
@@ -201,7 +201,7 @@ def test_legacy_2025_uses_validated_angle_request_limits():
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(
|
||||
enabled=False,
|
||||
latActive=True,
|
||||
latActive=False,
|
||||
actuators=SimpleNamespace(steeringAngleDeg=-73.05),
|
||||
)
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
@@ -213,6 +213,8 @@ def test_legacy_2025_uses_validated_angle_request_limits():
|
||||
standstill=False,
|
||||
))
|
||||
|
||||
controller.lateral_angle(CC, CS)
|
||||
CC.latActive = True
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
|
||||
parser.update([(1, [msg])])
|
||||
@@ -228,6 +230,39 @@ def test_legacy_2025_uses_validated_angle_request_limits():
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||
|
||||
|
||||
def test_legacy_2025_engagement_continues_from_last_sent_angle():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(
|
||||
enabled=False,
|
||||
latActive=False,
|
||||
actuators=SimpleNamespace(steeringAngleDeg=3.17),
|
||||
)
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
vEgoRaw=13.9,
|
||||
steeringAngleDeg=-0.14,
|
||||
steeringRateDeg=0.0,
|
||||
steeringPressed=False,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
standstill=False,
|
||||
))
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
|
||||
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(1, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(-0.14)
|
||||
|
||||
# CarState can advance between the last inactive command and the first active one.
|
||||
# Continue from the command panda accepted rather than skipping ahead to the newer sample.
|
||||
CS.out.steeringAngleDeg = -0.09
|
||||
CC.latActive = True
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(2, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(0.47)
|
||||
|
||||
|
||||
def test_legacy_2025_waits_for_manual_steering_to_settle_before_reengaging():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025)
|
||||
controller = CarController({}, CP)
|
||||
@@ -339,6 +374,7 @@ def test_ascent_2023_uses_gen2_angle_bus_layout():
|
||||
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM)
|
||||
assert not (CP.flags & SubaruFlags.D_PLATFORM_CAMERA)
|
||||
assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM_CAMERA)
|
||||
assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.FIXED_ANGLE_LIMITS
|
||||
assert CanBus.main_for_cp(CP) == CanBus.main
|
||||
assert CanBus.angle_for_cp(CP) == CanBus.main
|
||||
assert parsers[Bus.pt].bus == CanBus.main
|
||||
@@ -373,38 +409,55 @@ def test_angle_controller_tracks_driver_override():
|
||||
assert msg[0] == 0x124
|
||||
|
||||
|
||||
def test_ascent_angle_controller_waits_for_manual_steering_to_settle():
|
||||
def test_ascent_angle_controller_uses_fixed_angle_rate_limits():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-100.0))
|
||||
CC = SimpleNamespace(latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-14.88))
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
vEgoRaw=2.2,
|
||||
steeringAngleDeg=-114.04,
|
||||
steeringRateDeg=-90.0,
|
||||
steeringTorque=-201.0,
|
||||
vEgoRaw=21.66,
|
||||
steeringAngleDeg=-25.77,
|
||||
steeringRateDeg=0.0,
|
||||
steeringTorque=-149.0,
|
||||
steeringPressed=False,
|
||||
))
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
|
||||
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(1, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
assert CS.out.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < -25.0
|
||||
|
||||
|
||||
def test_ascent_angle_controller_yields_until_manual_steering_settles():
|
||||
CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
|
||||
controller = CarController({}, CP)
|
||||
CC = SimpleNamespace(latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-10.0))
|
||||
CS = SimpleNamespace(out=SimpleNamespace(
|
||||
vEgoRaw=21.66,
|
||||
steeringAngleDeg=-25.06,
|
||||
steeringRateDeg=35.0,
|
||||
steeringTorque=-149.0,
|
||||
steeringPressed=True,
|
||||
))
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
|
||||
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(1, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
|
||||
CS.out.steeringAngleDeg = -216.05
|
||||
CS.out.steeringRateDeg = -133.0
|
||||
CS.out.steeringTorque = -148.0
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(2, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
|
||||
|
||||
CS.out.steeringRateDeg = 2.0
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(3, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
CS.out.steeringPressed = False
|
||||
CS.out.steeringAngleDeg = -17.91
|
||||
CS.out.steeringRateDeg = 0.0
|
||||
for i in range(18):
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(2 + i, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
|
||||
|
||||
msg = controller.lateral_angle(CC, CS)
|
||||
parser.update([(4, [msg])])
|
||||
parser.update([(20, [msg])])
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
|
||||
assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg, abs=0.1)
|
||||
|
||||
|
||||
def test_lkas_hud_state_uses_lateral_active():
|
||||
|
||||
@@ -19,11 +19,12 @@ class CarControllerParams:
|
||||
MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * 0.06),
|
||||
MAX_ANGLE_RATE=1,
|
||||
)
|
||||
LEGACY_2025_ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
|
||||
FIXED_ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
|
||||
545,
|
||||
([0., 5., 35.], [5., .8, .15]),
|
||||
([0., 5., 35.], [5., .8, .15]),
|
||||
)
|
||||
LEGACY_2025_ANGLE_LIMITS = FIXED_ANGLE_LIMITS
|
||||
|
||||
def __init__(self, CP):
|
||||
self.STEER_STEP = 2 # how often we update the steer cmd
|
||||
@@ -81,7 +82,8 @@ class SubaruSafetyFlags(IntFlag):
|
||||
LKAS_ANGLE = 16
|
||||
D_PLATFORM = 32
|
||||
D_PLATFORM_CAMERA = 64
|
||||
LEGACY_2025_ANGLE_LIMITS = 128
|
||||
FIXED_ANGLE_LIMITS = 128
|
||||
LEGACY_2025_ANGLE_LIMITS = FIXED_ANGLE_LIMITS
|
||||
|
||||
|
||||
class SubaruFlags(IntFlag):
|
||||
|
||||
@@ -107,7 +107,7 @@ static bool subaru_longitudinal = false;
|
||||
static bool subaru_stop_and_go = false;
|
||||
static bool subaru_lkas_angle = false;
|
||||
static bool subaru_d_platform = false;
|
||||
static bool subaru_legacy_2025_angle_limits = false;
|
||||
static bool subaru_fixed_angle_limits = false;
|
||||
|
||||
static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
|
||||
return (uint8_t)msg->data[0];
|
||||
@@ -192,7 +192,7 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
|
||||
.frequency = 50U,
|
||||
};
|
||||
|
||||
const AngleSteeringLimits SUBARU_LEGACY_2025_ANGLE_STEERING_LIMITS = {
|
||||
const AngleSteeringLimits SUBARU_FIXED_ANGLE_STEERING_LIMITS = {
|
||||
.max_angle = 545 * 100,
|
||||
.angle_deg_to_can = 100.,
|
||||
.angle_rate_up_lookup = {
|
||||
@@ -241,8 +241,8 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
|
||||
desired_angle = -1 * to_signed(desired_angle, 17);
|
||||
bool lkas_request = GET_BIT(msg, 12U);
|
||||
|
||||
if (subaru_legacy_2025_angle_limits) {
|
||||
violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_LEGACY_2025_ANGLE_STEERING_LIMITS);
|
||||
if (subaru_fixed_angle_limits) {
|
||||
violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_FIXED_ANGLE_STEERING_LIMITS);
|
||||
} else {
|
||||
violation |= steer_angle_cmd_checks_vm(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS, SUBARU_ANGLE_STEERING_PARAMS);
|
||||
}
|
||||
@@ -375,8 +375,8 @@ static safety_config subaru_init(uint16_t param) {
|
||||
const uint16_t SUBARU_PARAM_D_PLATFORM_CAMERA = 64;
|
||||
const bool subaru_d_platform_camera = GET_FLAG(param, SUBARU_PARAM_D_PLATFORM_CAMERA);
|
||||
|
||||
const uint16_t SUBARU_PARAM_LEGACY_2025_ANGLE_LIMITS = 128;
|
||||
subaru_legacy_2025_angle_limits = GET_FLAG(param, SUBARU_PARAM_LEGACY_2025_ANGLE_LIMITS);
|
||||
const uint16_t SUBARU_PARAM_FIXED_ANGLE_LIMITS = 128;
|
||||
subaru_fixed_angle_limits = GET_FLAG(param, SUBARU_PARAM_FIXED_ANGLE_LIMITS);
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
|
||||
|
||||
@@ -358,8 +358,8 @@ class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafe
|
||||
TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS, SubaruMsg.ES_LKAS_ANGLE)
|
||||
|
||||
|
||||
class TestSubaruGen2Legacy2025AngleSafety(TestSubaruGen2AngleStockLongitudinalSafety):
|
||||
FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS
|
||||
class TestSubaruGen2FixedAngleSafety(TestSubaruGen2AngleStockLongitudinalSafety):
|
||||
FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.FIXED_ANGLE_LIMITS
|
||||
STEER_ANGLE_MAX = 545
|
||||
ANGLE_RATE_BP = [0., 5., 35.]
|
||||
ANGLE_RATE_UP = [5., .8, .15]
|
||||
|
||||
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-cbf7f35c-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-8a5cef0f-DEBUG";
|
||||
|
||||
@@ -1 +1 @@
|
||||
DEV-cbf7f35c-DEBUG
|
||||
DEV-8a5cef0f-DEBUG
|
||||
@@ -20,8 +20,8 @@ BOLT_ACC_PEDAL_START_HANDOFF_FLOOR_V = [0.22, 0.18, 0.10]
|
||||
NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35
|
||||
NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40
|
||||
GM_TRUCK_TARGET_FILTER_MIN_SPEED = 12.0
|
||||
GM_TRUCK_TARGET_FILTER_UP_TAU = 0.10
|
||||
GM_TRUCK_TARGET_FILTER_DOWN_TAU = 0.06
|
||||
GM_TRUCK_TARGET_FILTER_UP_TAU = 0.20
|
||||
GM_TRUCK_TARGET_FILTER_DOWN_TAU = 0.14
|
||||
GM_TRUCK_TARGET_FILTER_BRAKE_BYPASS = -0.65
|
||||
GM_TRUCK_TARGET_FILTER_DROP_BYPASS = 0.45
|
||||
TOYOTA_SIENNA_TARGET_FILTER_MIN_SPEED = 12.0
|
||||
|
||||
@@ -1160,6 +1160,22 @@ def test_gm_stock_truck_target_filter_smooths_mild_follow_reversals():
|
||||
assert filtered_brake < filtered_accel < 0.25
|
||||
|
||||
|
||||
def test_gm_stock_truck_target_filter_uses_comfort_slew_for_mild_braking():
|
||||
CP = make_longcontrol_cp(
|
||||
brand="gm",
|
||||
carFingerprint=CAR.CHEVROLET_SILVERADO,
|
||||
enableGasInterceptorDEPRECATED=False,
|
||||
)
|
||||
tuning = LongControl(CP).vehicle_tuning
|
||||
|
||||
tuning.shape_gm_truck_accel_target(0.30, 25.0, False)
|
||||
filtered = tuning.shape_gm_truck_accel_target(-0.10, 25.0, False)
|
||||
expected = 0.30 + vehicle_tunes.DT_CTRL / (vehicle_tunes.GM_TRUCK_TARGET_FILTER_DOWN_TAU + vehicle_tunes.DT_CTRL) * (-0.40)
|
||||
|
||||
assert filtered == pytest.approx(expected)
|
||||
assert filtered > -0.10
|
||||
|
||||
|
||||
def test_gm_stock_truck_target_filter_bypasses_urgent_braking():
|
||||
CP = make_longcontrol_cp(
|
||||
brand="gm",
|
||||
|
||||
@@ -14,6 +14,17 @@ EXPERIMENTAL_COLOR = rl.Color(218, 111, 37, 255)
|
||||
CEM_OVERRIDE_COLOR = rl.Color(255, 214, 0, 255)
|
||||
SWITCHBACK_COLOR = rl.Color(139, 108, 197, 255)
|
||||
TRAFFIC_COLOR = rl.Color(201, 34, 49, 255)
|
||||
LONGITUDINAL_ONLY_COLOR = rl.Color(255, 105, 180, 255)
|
||||
|
||||
|
||||
def is_longitudinal_only_active(state: UIState) -> bool:
|
||||
"""Return true when control is enabled but lateral control is inactive.
|
||||
|
||||
Do not use carControl.longActive here: stock-cruise cars intentionally leave
|
||||
that field false while the vehicle's own longitudinal controller is active.
|
||||
"""
|
||||
car_control = state.sm["carControl"]
|
||||
return bool(state.sm["selfdriveState"].enabled and not car_control.latActive)
|
||||
|
||||
|
||||
def get_border_color(state: UIState):
|
||||
@@ -21,6 +32,8 @@ def get_border_color(state: UIState):
|
||||
lateral_active = enabled or state.always_on_lateral_active
|
||||
if state.status == UIStatus.OVERRIDE:
|
||||
return OVERRIDE_COLOR
|
||||
if is_longitudinal_only_active(state):
|
||||
return LONGITUDINAL_ONLY_COLOR
|
||||
if state.switchback_mode_enabled and lateral_active:
|
||||
return SWITCHBACK_COLOR
|
||||
if state.traffic_mode_enabled and enabled:
|
||||
@@ -48,6 +61,8 @@ def get_screen_edge_color(state: UIState):
|
||||
lateral_active = enabled or state.always_on_lateral_active
|
||||
if state.status == UIStatus.OVERRIDE:
|
||||
return OVERRIDE_COLOR
|
||||
if is_longitudinal_only_active(state):
|
||||
return LONGITUDINAL_ONLY_COLOR
|
||||
if state.switchback_mode_enabled and lateral_active:
|
||||
return SWITCHBACK_COLOR
|
||||
if state.always_on_lateral_active:
|
||||
|
||||
@@ -7,6 +7,7 @@ from openpilot.selfdrive.ui.lib.starpilot_status import (
|
||||
DISENGAGED_COLOR,
|
||||
ENGAGED_COLOR,
|
||||
EXPERIMENTAL_COLOR,
|
||||
LONGITUDINAL_ONLY_COLOR,
|
||||
OVERRIDE_COLOR,
|
||||
SWITCHBACK_COLOR,
|
||||
TRAFFIC_COLOR,
|
||||
@@ -25,6 +26,7 @@ __all__ = [
|
||||
"DISENGAGED_COLOR",
|
||||
"ENGAGED_COLOR",
|
||||
"EXPERIMENTAL_COLOR",
|
||||
"LONGITUDINAL_ONLY_COLOR",
|
||||
"OVERRIDE_COLOR",
|
||||
"SWITCHBACK_COLOR",
|
||||
"TRAFFIC_COLOR",
|
||||
|
||||
@@ -0,0 +1,42 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
from openpilot.selfdrive.ui.lib.starpilot_status import (
|
||||
DISENGAGED_COLOR,
|
||||
ENGAGED_COLOR,
|
||||
LONGITUDINAL_ONLY_COLOR,
|
||||
AOL_COLOR,
|
||||
get_border_color,
|
||||
get_screen_edge_color,
|
||||
)
|
||||
from openpilot.selfdrive.ui.ui_state import UIStatus
|
||||
|
||||
|
||||
def _state(*, enabled=False, lat_active=False, aol=False):
|
||||
return SimpleNamespace(
|
||||
sm={
|
||||
"selfdriveState": SimpleNamespace(enabled=enabled, experimentalMode=False),
|
||||
"carControl": SimpleNamespace(latActive=lat_active),
|
||||
},
|
||||
status=UIStatus.ENGAGED if enabled else UIStatus.DISENGAGED,
|
||||
always_on_lateral_active=aol,
|
||||
switchback_mode_enabled=False,
|
||||
traffic_mode_enabled=False,
|
||||
conditional_status=0,
|
||||
)
|
||||
|
||||
|
||||
def _rgb(color):
|
||||
return color.r, color.g, color.b
|
||||
|
||||
|
||||
def test_cruise_only_uses_pink_for_border_and_screen_edge():
|
||||
state = _state(enabled=True, lat_active=False)
|
||||
|
||||
assert _rgb(get_border_color(state)) == _rgb(LONGITUDINAL_ONLY_COLOR)
|
||||
assert _rgb(get_screen_edge_color(state)) == _rgb(LONGITUDINAL_ONLY_COLOR)
|
||||
|
||||
|
||||
def test_lateral_active_colors_remain_unchanged():
|
||||
assert _rgb(get_border_color(_state(enabled=True, lat_active=True))) == _rgb(ENGAGED_COLOR)
|
||||
assert _rgb(get_border_color(_state(aol=True))) == _rgb(AOL_COLOR)
|
||||
assert _rgb(get_border_color(_state())) == _rgb(DISENGAGED_COLOR)
|
||||
@@ -7,7 +7,9 @@ const FACTORY_RESET_STATUS_POLL_INTERVAL_MS = 1000
|
||||
const state = reactive({
|
||||
showResetDefaultModal: false,
|
||||
showSaveMeModal: false,
|
||||
showDeleteRoutesModal: false,
|
||||
factoryResetBusy: false,
|
||||
routeDeleteBusy: false,
|
||||
factoryResetStatus: null,
|
||||
})
|
||||
|
||||
@@ -199,6 +201,29 @@ export function ToggleControl() {
|
||||
state.showSaveMeModal = true;
|
||||
}
|
||||
|
||||
function confirmDeleteRoutes() {
|
||||
state.showDeleteRoutesModal = true;
|
||||
}
|
||||
|
||||
async function deleteAllRoutes() {
|
||||
if (state.routeDeleteBusy || state.factoryResetBusy) return
|
||||
|
||||
state.showDeleteRoutesModal = false
|
||||
state.routeDeleteBusy = true
|
||||
try {
|
||||
const response = await fetch("/api/routes/delete_all", { method: "POST" })
|
||||
const payload = await response.json().catch(() => ({}))
|
||||
if (!response.ok) {
|
||||
throw new Error(payload.error || response.statusText || "Failed to delete driving routes")
|
||||
}
|
||||
showSnackbar(payload.message || "All local driving routes deleted.")
|
||||
} catch (error) {
|
||||
showSnackbar(error?.message || "Failed to delete driving routes.", "error")
|
||||
} finally {
|
||||
state.routeDeleteBusy = false
|
||||
}
|
||||
}
|
||||
|
||||
async function runFactoryReset() {
|
||||
if (state.factoryResetBusy) return
|
||||
|
||||
@@ -262,6 +287,15 @@ export function ToggleControl() {
|
||||
disabled="${() => state.factoryResetBusy}">
|
||||
${() => state.factoryResetBusy ? "Starting..." : "SAVE ME"}
|
||||
</button>
|
||||
<p class="toggle-control-danger-text">
|
||||
Delete local route recordings without changing settings or rebooting the device.
|
||||
</p>
|
||||
<button
|
||||
class="toggle-control-button toggle-control-button-danger"
|
||||
@click="${confirmDeleteRoutes}"
|
||||
disabled="${() => state.factoryResetBusy || state.routeDeleteBusy}">
|
||||
${() => state.routeDeleteBusy ? "Deleting Routes..." : "Delete All Driving Routes"}
|
||||
</button>
|
||||
${() => state.factoryResetStatus ? html`
|
||||
<div class="toggle-control-status ${state.factoryResetStatus.stage === "error" ? "error" : ""}">
|
||||
<div class="toggle-control-status-title">Factory Reset Status</div>
|
||||
@@ -296,6 +330,13 @@ export function ToggleControl() {
|
||||
onConfirm: runFactoryReset,
|
||||
onCancel: () => { state.showSaveMeModal = false; },
|
||||
confirmText: "Factory Reset"
|
||||
}) : ""}
|
||||
${() => state.showDeleteRoutesModal ? Modal({
|
||||
title: "Delete All Driving Routes",
|
||||
message: "This permanently deletes all local routes from standard, high-resolution, and alternate footage storage. It does not reset settings or reboot the device. Saved personal records and model history will be kept.",
|
||||
onConfirm: deleteAllRoutes,
|
||||
onCancel: () => { state.showDeleteRoutesModal = false; },
|
||||
confirmText: "Delete Routes"
|
||||
}) : ""}
|
||||
`
|
||||
}
|
||||
|
||||
@@ -945,6 +945,31 @@ def test_dashboard_persistent_stats_fallback_to_file_when_param_put_fails(tmp_pa
|
||||
assert stats["routes"]["route-1"]["analysisComplete"] is True
|
||||
|
||||
|
||||
def test_clear_dashboard_route_history_keeps_durable_records(tmp_path, monkeypatch):
|
||||
monkeypatch.setattr(utilities, "DASHBOARD_PARAMS_DIR", tmp_path)
|
||||
params = FakeParams({
|
||||
utilities.DASHBOARD_PERSISTENT_STATS_PARAM: {
|
||||
"routes": {
|
||||
"route-1": {"date": "2026-06-15T08:00:00"},
|
||||
"route-2": {"date": "2026-06-16T08:00:00"},
|
||||
},
|
||||
"ignoredRoutes": ["route-2"],
|
||||
"personalRecords": {"cleanDriveStreak": {"drives": 4}},
|
||||
"attentionRecords": {"cleanDriveStreak": {"drives": 4}},
|
||||
"modelUsage": {"orion": {"drives": 3}},
|
||||
},
|
||||
})
|
||||
|
||||
assert utilities.clear_dashboard_route_history(params) == 2
|
||||
|
||||
stats = utilities._load_dashboard_persistent_stats(params)
|
||||
assert stats["routes"] == {}
|
||||
assert stats["ignoredRoutes"] == []
|
||||
assert stats["personalRecords"]["cleanDriveStreak"]["drives"] == 4
|
||||
assert stats["attentionRecords"]["cleanDriveStreak"]["drives"] == 4
|
||||
assert stats["modelUsage"]["orion"]["drives"] == 3
|
||||
|
||||
|
||||
def test_lightweight_routes_surface_recent_drives_without_log_analysis(monkeypatch):
|
||||
utilities._invalidate_dashboard_cache()
|
||||
now = utilities.datetime.now().replace(hour=12, minute=0, second=0, microsecond=0)
|
||||
|
||||
@@ -939,6 +939,7 @@ _fast_update_state = {
|
||||
"progressLabel": "Idle",
|
||||
"progressDetail": "",
|
||||
}
|
||||
_ROUTE_DELETE_LOCK = threading.Lock()
|
||||
|
||||
_FACTORY_RESET_WIPE_PATHS = [
|
||||
"/data/params",
|
||||
@@ -5646,22 +5647,43 @@ def setup(app):
|
||||
delete_file(os.path.join(footage_path, segment))
|
||||
return {"message": "Route deleted!"}, 200
|
||||
|
||||
@app.route("/api/routes/delete_all", methods=["DELETE"])
|
||||
@app.route("/api/routes/delete_all", methods=["DELETE", "POST"])
|
||||
def delete_all_routes():
|
||||
route_names = set()
|
||||
for footage_path in FOOTAGE_PATHS:
|
||||
if os.path.exists(footage_path):
|
||||
for segment in os.listdir(footage_path):
|
||||
route_names.add(segment.split("--")[0])
|
||||
if _safe_params_get_bool("IsOnroad"):
|
||||
return jsonify({"error": "Cannot delete driving routes while driving."}), 409
|
||||
|
||||
for route_name in sorted(list(route_names)):
|
||||
if not _ROUTE_DELETE_LOCK.acquire(blocking=False):
|
||||
return jsonify({"error": "Route deletion is already in progress."}), 409
|
||||
|
||||
try:
|
||||
utilities.stop_dashboard_background_analysis()
|
||||
|
||||
route_paths = []
|
||||
seen_paths = set()
|
||||
for footage_path in FOOTAGE_PATHS:
|
||||
if os.path.exists(footage_path):
|
||||
for segment in os.listdir(footage_path):
|
||||
if segment.startswith(route_name):
|
||||
delete_file(os.path.join(footage_path, segment))
|
||||
path = str(footage_path).rstrip("/")
|
||||
if path and path not in seen_paths:
|
||||
seen_paths.add(path)
|
||||
route_paths.append(path)
|
||||
|
||||
return {"message": "All routes deleted!"}, 200
|
||||
for route_path in route_paths:
|
||||
_run_factory_reset_delete(route_path)
|
||||
|
||||
persisted_route_count = utilities.clear_dashboard_route_history(params)
|
||||
_STATS_RESPONSE_CACHE.update({
|
||||
"updated_at": 0.0,
|
||||
"payload": None,
|
||||
})
|
||||
return jsonify({
|
||||
"success": True,
|
||||
"message": "All local driving routes deleted. Saved personal records were kept.",
|
||||
"deletedPaths": len(route_paths),
|
||||
"clearedDashboardRoutes": persisted_route_count,
|
||||
}), 200
|
||||
except Exception as exception:
|
||||
return jsonify({"error": f"Failed to delete driving routes: {exception}"}), 500
|
||||
finally:
|
||||
_ROUTE_DELETE_LOCK.release()
|
||||
|
||||
@app.route("/api/routes/<name>/preserve", methods=["POST"])
|
||||
def preserve_route(name):
|
||||
|
||||
@@ -1671,6 +1671,35 @@ def _invalidate_dashboard_cache():
|
||||
})
|
||||
|
||||
|
||||
def clear_dashboard_route_history(params_obj):
|
||||
"""Remove route-backed dashboard history while keeping durable records."""
|
||||
stats = _load_dashboard_persistent_stats(params_obj)
|
||||
route_count = len(stats.get("routes", {}))
|
||||
stats["routes"] = {}
|
||||
stats["ignoredRoutes"] = []
|
||||
serialized = json.dumps(stats, separators=(",", ":"))
|
||||
|
||||
persisted_to_params = False
|
||||
if params_obj is not None:
|
||||
try:
|
||||
params_obj.put(DASHBOARD_PERSISTENT_STATS_PARAM, serialized)
|
||||
persisted_to_params = True
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
fallback_path = _dashboard_param_file_path(DASHBOARD_PERSISTENT_STATS_PARAM)
|
||||
if persisted_to_params:
|
||||
try:
|
||||
fallback_path.unlink()
|
||||
except (FileNotFoundError, OSError):
|
||||
pass
|
||||
elif not _write_dashboard_param_file(DASHBOARD_PERSISTENT_STATS_PARAM, serialized):
|
||||
raise RuntimeError("Unable to clear persisted dashboard route history.")
|
||||
|
||||
_invalidate_dashboard_cache()
|
||||
return route_count
|
||||
|
||||
|
||||
def warm_dashboard_stats(footage_paths=None):
|
||||
params_obj = params
|
||||
if params_obj.get_bool("IsOnroad"):
|
||||
|
||||
Reference in New Issue
Block a user