IQ.Pilot Release Commit @ 4521b0f

This commit is contained in:
IQ.Lvbs CI [bot]
2026-08-22 10:38:43 -05:00
parent e142a0001a
commit 11cefcb266
29 changed files with 822 additions and 111 deletions

View File

@@ -14,15 +14,20 @@
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/iqpilot_private/updater/_iqos_pat_blob.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "94664d1b9abd1c0cb124707bf10f746f23adf0847a890d507df86f1e4a7e8ca9",
"size": 67616
},
"python/iqpilot_private/updater/_pat_blob.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "f8c5eeeb8cfb6fb077ab80e95a93bb8a36c819a2d2147e560ec015cfe43eed20",
"sha256": "481235a5685e14f5b8e9216908be23be9743fe6f487f2b38fb21c33dd84e22cf",
"size": 67616
},
"python/iqpilot_private/updater/git_remote.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "f9806ad70ffe7718e64a3ecb23821b2aee91658562a525767675d944035ce235",
"size": 70056
"sha256": "75ffbf8212f805b054c97ca4723a56071db822e889cda9d6bec233b2d2c8592a",
"size": 136488
},
"runtime": {
"entries": {
@@ -33,7 +38,8 @@
}
},
"signatures": {
"python/iqpilot_private/updater/_pat_blob.cpython-312-aarch64-linux-gnu.so": "7RM6ZvkT7i7s3djWFUPOs3s/kOiscys7+o+3gGTLlPC6PJAyalRGOPkn5SlsnfaLZk8CIaENZEpqfspISa+DBw==",
"python/iqpilot_private/updater/git_remote.cpython-312-aarch64-linux-gnu.so": "pSvniR0EAnpTwpKqGvM8deAR8qe68W40n/pjkAO8QqNuz0/UgKQFQWlms5E+Xvz6yTMhg33W4ICOSu65mT4MCA=="
"python/iqpilot_private/updater/_iqos_pat_blob.cpython-312-aarch64-linux-gnu.so": "B45oDHpHNmslzjTyeuEVbE4fOytUsC/0po+CiHiLI+T01yWJIb6Dt4yFJm+qVTEqCLJfajedd1FnIQHCyUddDw==",
"python/iqpilot_private/updater/_pat_blob.cpython-312-aarch64-linux-gnu.so": "a+YcE2ik7Oo3btunKGoA+FHN+uSDoric2V6nu5SjtKSM/Nwpg1M9BX0BM55sA56sz6FgPRn93u+yPL7eryLYDw==",
"python/iqpilot_private/updater/git_remote.cpython-312-aarch64-linux-gnu.so": "NcdigDJOQ7d9A+HXCewPowezSML1i4cHW6uRyZ1N6vmtkXDorRk9YC4lxlM8uvoS/tB5/jQBkfc/DhkwPXAoAA=="
}
}

View File

@@ -1 +1 @@
8plcIUBv95Bl+JlOV98JKmC9l9njuGkxueNgvoEeQ0HcGVmTDiCYIvq72XhFsmwdFuOkGCDzn0st3TWu7D1vBg==
54xdruMwhqg4ouUav9CwbOhcsytYbn/uy/OL1pT6Sd/JRvgsLckfe8+jsG527nyhZ7sBpxPx5dFLX3h/eOaSDg==

View File

@@ -259,8 +259,8 @@ class CarController(CarControllerBase):
self._pq_patch_checked = not bool(CP.flags & VolkswagenFlags.PQ) or self.eps_timer_workaround
self.hca_frame_timer_resetting = 0
self.hca_frame_low_torque = 0
self.long_override_counter = 0
self.long_disabled_counter = 0
self.acc_hold_type_last = mebcan.ACC_HMS_NO_REQUEST
self.acc_hold_ramp_counter = 0
self.standstill_manager = MQBStandstillManager(CP.mass, self.CCP.ACCEL_MIN) if self.CCS == mqbcan else None
self.radar_handler = PQRadarHandler(self.CAN) if self.CCS is pqcan else None
self.blend_stock_radar = False
@@ -474,11 +474,7 @@ class CarController(CarControllerBase):
accel = float(np.clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.enabled else 0)
long_override = CC.cruiseControl.override or CS.out.gasPressed
self.long_override_counter = min(self.long_override_counter + 1, 5) if long_override else 0
long_override_begin = long_override and self.long_override_counter < 5
self.long_disabled_counter = min(self.long_disabled_counter + 1, 5) if not CC.enabled else 0
long_disabling = not CC.enabled and self.long_disabled_counter < 5
critical_state = hud_control.visualAlert == VisualAlert.fcw
if CC.longComfortMode and self.long_jerk_control is not None and self.long_limit_control is not None:
@@ -486,8 +482,10 @@ class CarController(CarControllerBase):
self.long_limit_control.update(CC.enabled, CS.out.vEgoRaw, hud_control.setSpeed, hud_control.leadDistance, hud_control.leadVisible, critical_state)
acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled, long_override)
acc_hold_type = self.CCS.acc_hold_type(CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled, starting, stopping,
CS.esp_hold_confirmation, long_override, long_override_begin, long_disabling)
acc_hold_type, self.acc_hold_ramp_counter = self.CCS.acc_hold_type(
CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled, starting, stopping,
CS.esp_hold_confirmation, long_override, self.acc_hold_type_last, self.acc_hold_ramp_counter)
self.acc_hold_type_last = acc_hold_type
can_sends.extend(self.CCS.create_acc_accel_control(
self.packer_pt, self.CAN.pt, self.CP, CS.acc_type, CC.enabled,
self.long_jerk_control.get_jerk_up() if CC.longComfortMode and self.long_jerk_control is not None else 4.0,

View File

@@ -16,6 +16,7 @@ ACC_HMS_RAMP_RELEASE = 5
ACC_HMS_RELEASE = 4
ACC_HMS_HOLD = 1
ACC_HMS_NO_REQUEST = 0
ACC_HMS_RAMP_FRAMES = 5
ACC_HUD_ERROR = 6
ACC_HUD_OVERRIDE = 4
@@ -160,21 +161,16 @@ def acc_control_value(main_switch_on, acc_faulted, long_active, override):
return acc_control
def acc_hold_type(main_switch_on, acc_faulted, long_active, starting, stopping, esp_hold, override, override_begin, long_disabling):
def acc_hold_type(main_switch_on, acc_faulted, long_active, starting, stopping, esp_hold, override,
prev_acc_hold_type, ramp_counter):
# warning: car is reacting to hold mechanic even with long control off
if acc_faulted:
acc_hold_type = ACC_HMS_NO_REQUEST # no hold request
elif not long_active:
if long_disabling:
acc_hold_type = ACC_HMS_RAMP_RELEASE # ramp release of requests right after disabling long control (prevents car error with EPB at low speed)
else:
acc_hold_type = ACC_HMS_NO_REQUEST # no hold request
acc_hold_type = ACC_HMS_NO_REQUEST # no hold request
elif override:
if override_begin:
acc_hold_type = ACC_HMS_RAMP_RELEASE # ramp release of requests at the beginning of override (prevents car error with EPB at low speed)
else:
acc_hold_type = ACC_HMS_NO_REQUEST # overriding / no request
acc_hold_type = ACC_HMS_NO_REQUEST # overriding / no request
elif starting:
acc_hold_type = ACC_HMS_RELEASE # release request and startup
elif stopping or esp_hold:
@@ -182,7 +178,18 @@ def acc_hold_type(main_switch_on, acc_faulted, long_active, starting, stopping,
else:
acc_hold_type = ACC_HMS_NO_REQUEST # no hold request
return acc_hold_type
if acc_hold_type == ACC_HMS_HOLD:
ramp_counter = 0
elif prev_acc_hold_type == ACC_HMS_HOLD and acc_hold_type == ACC_HMS_NO_REQUEST:
acc_hold_type = ACC_HMS_RAMP_RELEASE
ramp_counter = ACC_HMS_RAMP_FRAMES
elif ramp_counter > 0:
acc_hold_type = ACC_HMS_RAMP_RELEASE
ramp_counter -= 1
else:
pass
return acc_hold_type, ramp_counter
def create_acc_accel_control(packer, bus, CP, acc_type, acc_enabled, upper_jerk, lower_jerk, upper_control_limit, lower_control_limit,

View File

@@ -139,7 +139,7 @@ def create_blinker_control(packer, bus, leftBlinker, rightBlinker):
def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, leadDistance, distanceBars, fcw_alert, leadVisible, unavailable, decel, d_unresponsive):
priodisp = 0 if fcw_alert else 1 if (acc_hud_status == 4 or decel or leadVisible) else 2 if (acc_hud_status in (3, 2)) else 0
priodisp = 0 if fcw_alert else 0 if (acc_hud_status == 4 or decel) else 2 if (acc_hud_status in (3, 2) or leadVisible) else 0
leadDistanceBars = distanceBars + 1 if distanceBars in (1, 2, 3) else 2
values = {
"ACA_StaACC": acc_hud_status,
@@ -149,7 +149,6 @@ def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, leadDistance,
"ACA_gemZeitl": min(15, max(1, int(round(leadDistance)))) if leadVisible else 0,
"ACA_PrioDisp": priodisp,
"ACA_Akustik1": d_unresponsive,
# "ACA_Fahrerhinw": unavailable,
"ACA_Akustik2": fcw_alert,
"ACA_ACC_Verz": decel,
}

View File

@@ -0,0 +1,205 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
from types import SimpleNamespace
import pytest
from iqdbc.car.tesla.interface import CarInterface
from iqdbc.car.vehicle_model import VehicleModel
from iqdbc.lvbs.car.tesla.torque_blend import (
DT_LAT_CTRL,
STEER_OVERRIDE_MAX_LAT_ACCEL,
STEER_OVERRIDE_MIN_TORQUE,
STEER_OVERRIDE_TORQUE_RANGE,
STEER_OVERRIDE_TORQUE_ZERO_MAX,
SteeringTorqueZero,
TorqueBlendController,
calc_override_angle_limited,
get_steer_from_lat_accel,
)
from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
VM = VehicleModel(CarInterface.get_non_essential_params("TESLA_MODEL_Y"))
COOP_ON = SimpleNamespace(flags=TeslaFlagsIQ.COOP_STEERING.value)
COOP_OFF = SimpleNamespace(flags=0)
HAND_REST_TORQUE = 0.55
ZERO_WEIGHT_SPEED = 20.0
FULL_WEIGHT_SPEED = 8.0
SETTLING_SECONDS = 300.0
def _car_state(torque, v_ego, angle=0.0):
return SimpleNamespace(
out=SimpleNamespace(steeringTorque=torque, vEgo=v_ego, vEgoRaw=v_ego, steeringAngleDeg=angle, steeringRateDeg=0.0),
hands_on_level=0,
)
def _drive(blend, torque, v_ego, seconds, angle=0.0, cp_iq=COOP_ON, lat_active=True):
steps = int(seconds / DT_LAT_CTRL)
out = 0.0
for _ in range(steps):
out = blend.update(angle, lat_active, cp_iq, _car_state(torque, v_ego, angle), VM).steeringAngleDeg
return out
def _override(blend):
return blend.coop_apply_angle_last - blend.debug_angle_desired_limited
MAX_NULLABLE_PRELOAD = STEER_OVERRIDE_MIN_TORQUE + STEER_OVERRIDE_TORQUE_ZERO_MAX
@pytest.mark.parametrize("rest", [0.3, HAND_REST_TORQUE, MAX_NULLABLE_PRELOAD])
@pytest.mark.parametrize("sign", [1.0, -1.0])
def test_torque_zero_pulls_a_sustained_hand_rest_inside_the_deadzone(rest, sign):
zero = SteeringTorqueZero()
for _ in range(int(SETTLING_SECONDS / DT_LAT_CTRL)):
corrected = zero.update(sign * rest, True)
assert abs(corrected) <= STEER_OVERRIDE_MIN_TORQUE
@pytest.mark.parametrize("sign", [1.0, -1.0])
def test_a_settled_hand_rest_at_the_nullable_limit_produces_no_override(sign):
blend = TorqueBlendController()
_drive(blend, sign * MAX_NULLABLE_PRELOAD, ZERO_WEIGHT_SPEED, seconds=SETTLING_SECONDS)
assert abs(_override(blend)) == pytest.approx(0.0, abs=1e-6)
def test_override_unwinds_once_the_driver_releases():
blend = TorqueBlendController()
_drive(blend, 2.0, FULL_WEIGHT_SPEED, seconds=10.0)
assert abs(_override(blend)) > 1.0
_drive(blend, 0.0, FULL_WEIGHT_SPEED, seconds=20.0)
assert _override(blend) == pytest.approx(0.0, abs=1e-6)
assert blend.override_angle_accu == pytest.approx(0.0, abs=1e-6)
def test_torque_zero_passes_a_step_through_untouched():
zero = SteeringTorqueZero()
assert zero.update(2.0, True) == pytest.approx(2.0, abs=1e-3)
def test_torque_zero_is_bounded():
zero = SteeringTorqueZero()
for _ in range(int(SETTLING_SECONDS / DT_LAT_CTRL)):
zero.update(10.0, True)
assert zero.update(0.0, False) == pytest.approx(-STEER_OVERRIDE_TORQUE_ZERO_MAX)
def test_torque_zero_holds_while_not_learning():
zero = SteeringTorqueZero()
zero.update(1.0, False)
assert zero.update(1.0, False) == pytest.approx(1.0)
def test_sustained_hand_rest_stops_steering_the_car():
blend = TorqueBlendController()
_drive(blend, HAND_REST_TORQUE, FULL_WEIGHT_SPEED, seconds=1.0)
early = abs(_override(blend))
_drive(blend, HAND_REST_TORQUE, FULL_WEIGHT_SPEED, seconds=SETTLING_SECONDS)
assert early > 0.1
assert abs(_override(blend)) < 0.1 * early
def test_a_deliberate_push_still_overrides_after_the_zero_has_settled():
blend = TorqueBlendController()
_drive(blend, HAND_REST_TORQUE, FULL_WEIGHT_SPEED, seconds=SETTLING_SECONDS)
_drive(blend, HAND_REST_TORQUE + 1.5, FULL_WEIGHT_SPEED, seconds=1.0)
assert abs(_override(blend)) > 1.0
def test_accumulator_cannot_charge_where_its_weight_is_zero():
blend = TorqueBlendController()
_drive(blend, 2.0, ZERO_WEIGHT_SPEED, seconds=30.0)
assert blend.override_angle_accu == pytest.approx(0.0, abs=1e-6)
def test_accumulator_drains_once_the_car_passes_the_crossover_speed():
blend = TorqueBlendController()
_drive(blend, 2.5, FULL_WEIGHT_SPEED, seconds=10.0)
assert abs(blend.override_angle_accu) > 1.0
_drive(blend, 2.5, ZERO_WEIGHT_SPEED, seconds=5.0)
assert blend.override_angle_accu == pytest.approx(0.0, abs=1e-6)
def test_low_speed_override_can_exceed_the_direct_term_authority():
blend = TorqueBlendController()
_drive(blend, 2.5, FULL_WEIGHT_SPEED, seconds=60.0)
authority = calc_override_angle_limited(STEER_OVERRIDE_TORQUE_RANGE, FULL_WEIGHT_SPEED, VM, STEER_OVERRIDE_MAX_LAT_ACCEL)
assert abs(blend.override_angle_accu) > authority
def test_a_sustained_low_speed_push_reaches_a_large_override():
blend = TorqueBlendController()
_drive(blend, 2.5, FULL_WEIGHT_SPEED, seconds=10.0)
assert abs(_override(blend)) > 20.0
@pytest.mark.parametrize("v_ego", [3.0, 5.0, FULL_WEIGHT_SPEED, 12.0])
def test_override_stays_inside_the_max_lat_accel_envelope_plus_the_direct_term(v_ego):
blend = TorqueBlendController()
_drive(blend, 2.5, v_ego, seconds=60.0)
envelope = get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, v_ego, VM)
direct_authority = calc_override_angle_limited(STEER_OVERRIDE_TORQUE_RANGE, v_ego, VM, STEER_OVERRIDE_MAX_LAT_ACCEL)
assert abs(_override(blend)) <= envelope + direct_authority
@pytest.mark.parametrize("v_ego", [3.0, 5.0, FULL_WEIGHT_SPEED, 12.0])
def test_a_pinned_push_cannot_run_the_contribution_away(v_ego):
blend = TorqueBlendController()
_drive(blend, 2.5, v_ego, seconds=130.0)
envelope = get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, v_ego, VM)
capability = calc_override_angle_limited(STEER_OVERRIDE_TORQUE_RANGE, v_ego, VM, STEER_OVERRIDE_MAX_LAT_ACCEL) / envelope
assert abs(blend.override_angle_accu) * (1.0 - capability) <= envelope + 1e-6
def test_the_envelope_opens_up_as_speed_drops():
low = get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, 3.0, VM)
high = get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, 12.0, VM)
assert low > 10 * high
def test_relative_weight_is_zero_above_the_direct_capability_crossover():
capability = (calc_override_angle_limited(STEER_OVERRIDE_TORQUE_RANGE, ZERO_WEIGHT_SPEED, VM, STEER_OVERRIDE_MAX_LAT_ACCEL) /
get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, ZERO_WEIGHT_SPEED, VM))
assert capability == pytest.approx(1.0)
HEAVY_REST = 0.80
@pytest.mark.parametrize("sign", [1.0, -1.0])
def test_the_zero_nulls_a_hand_rest_heavier_than_the_deadzone(sign):
assert HEAVY_REST > 2 * STEER_OVERRIDE_MIN_TORQUE
blend = TorqueBlendController()
_drive(blend, sign * HEAVY_REST, FULL_WEIGHT_SPEED, seconds=SETTLING_SECONDS)
assert abs(_override(blend)) == pytest.approx(0.0, abs=1e-6)
LIGHT_PUSH = 0.45
@pytest.mark.parametrize("v_ego", [FULL_WEIGHT_SPEED, 17.0])
def test_a_light_push_overrides_without_needing_the_old_half_newton(v_ego):
assert STEER_OVERRIDE_MIN_TORQUE < LIGHT_PUSH < 0.5
blend = TorqueBlendController()
_drive(blend, LIGHT_PUSH, v_ego, seconds=5.0)
assert abs(_override(blend)) > 0.2
def test_torque_below_the_deadzone_never_overrides():
blend = TorqueBlendController()
_drive(blend, STEER_OVERRIDE_MIN_TORQUE * 0.5, FULL_WEIGHT_SPEED, seconds=5.0)
assert abs(_override(blend)) == pytest.approx(0.0, abs=1e-6)
def test_coop_disabled_leaves_the_command_untouched():
blend = TorqueBlendController()
angle = 5.0
out = _drive(blend, 2.0, FULL_WEIGHT_SPEED, seconds=5.0, angle=angle, cp_iq=COOP_OFF)
assert out == pytest.approx(angle, abs=1e-6)
assert blend.override_angle_accu == pytest.approx(0.0)

View File

@@ -21,7 +21,7 @@ class TorqueBlendParams(CarControllerParams):
STEERING_DEG_PHASE_LEAD_COEFF = 8.0
# angle override # todo implement steering torque inertia compensation to increase gains
STEER_OVERRIDE_MIN_TORQUE = 0.5 # Nm - based on typical steering bias + noise
STEER_OVERRIDE_MIN_TORQUE = 0.35 # Nm - only noise now, SteeringTorqueZero removes the bias this used to cover
STEER_OVERRIDE_MAX_TORQUE = 2.5 # Nm max torque before EPS disengages, LKAS takes over at 1.8Nm
STEER_OVERRIDE_MAX_LAT_ACCEL = 1.5 # m/s^2 - determines angle rate - speed dependent - similar to Tesla comfort steering mode
STEER_OVERRIDE_LAT_ACCEL_GAIN_LIMIT = 10 # deg/Nm stability and smoothness for angle control # todo this could be increased after solving feedback stability
@@ -33,6 +33,9 @@ STEER_OVERRIDE_MAX_LAT_JERK_CENTERING = TorqueBlendParams.ANGLE_LIMITS.MAX_LATER
STEER_OVERRIDE_LAT_JERK_GAIN_LIMIT = 100 # deg/s/Nm - should be less than CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE / DT_CTRL / STEER_OVERRIDE_TORQUE_RANGE
STEER_OVERRIDE_TORQUE_RANGE = STEER_OVERRIDE_MAX_TORQUE - STEER_OVERRIDE_MIN_TORQUE
STEER_OVERRIDE_TORQUE_ZERO_TAU = 60.0
STEER_OVERRIDE_TORQUE_ZERO_MAX = 0.5 # Nm - sized to the observed hand rest and sensor bias, not to the deadzone
# model fighting mitigation
STEER_DESIRED_LIMITER_ALLOW_SPEED = 6.0 # m/s - below this speed the desired angle limiter is active
STEER_DESIRED_LIMITER_ACCEL = 100 # deg/s^2 when override angle ramp is active
@@ -96,6 +99,20 @@ def calc_override_angle_delta_limited(torque: float, vEgo: float, VM: VehicleMod
return apply_bounds(override_angle_rate * DT_LAT_CTRL, TorqueBlendParams.ANGLE_LIMITS.MAX_ANGLE_RATE)
class SteeringTorqueZero:
def __init__(self):
self._zero = 0.0
def reset(self) -> None:
self._zero = 0.0
def update(self, torque: float, learning: bool) -> float:
if learning:
self._zero += (torque - self._zero) * (DT_LAT_CTRL / STEER_OVERRIDE_TORQUE_ZERO_TAU)
self._zero = apply_bounds(self._zero, STEER_OVERRIDE_TORQUE_ZERO_MAX)
return torque - self._zero
class SteerRateLimiter:
"""Handles rate limiting of steering angle changes with a configurable rate."""
def __init__(self):
@@ -169,6 +186,7 @@ class TorqueBlendController:
self.resume_rate_limiter_delta = SteerRateLimiter()
self.resume_rate_limiter = SteerRateLimiter()
self.override_accel_rate_limiter = SteerAccelLimiter()
self.torque_zero = SteeringTorqueZero()
self.debug_angle_desired_limited = 0
def apply_override_angle_direct(self, lat_active: bool, driverTorque: float, vEgo: float, VM: VehicleModel) -> float:
@@ -238,16 +256,25 @@ class TorqueBlendController:
if not lat_active:
return 0
override_envelope = get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, vEgo, VM)
# calculate capability of direct angle override (fully active above ~36kph)
direct_override_capability = (calc_override_angle_limited(STEER_OVERRIDE_TORQUE_RANGE, vEgo, VM, STEER_OVERRIDE_MAX_LAT_ACCEL) /
get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, vEgo, VM))
direct_override_capability = calc_override_angle_limited(STEER_OVERRIDE_TORQUE_RANGE, vEgo, VM,
STEER_OVERRIDE_MAX_LAT_ACCEL) / override_envelope
angle_override_direct = self.apply_override_angle_direct(lat_active, driverTorque, vEgo, VM)
relative_weight = 1.0 - direct_override_capability
angle_override_relative = self.apply_override_angle_relative(lat_active, driverTorque, vEgo, VM,
unwind_weight=relative_weight)
if relative_weight > 0.0:
self.apply_override_angle_relative(lat_active, driverTorque, vEgo, VM, unwind_weight=relative_weight)
self.override_angle_accu = apply_bounds(self.override_angle_accu, override_envelope / relative_weight)
else:
self.center_override_angle_accu(vEgo, VM)
return angle_override_direct * direct_override_capability + angle_override_relative * relative_weight
return angle_override_direct * direct_override_capability + self.override_angle_accu * relative_weight
def center_override_angle_accu(self, vEgo: float, VM: VehicleModel) -> None:
centering_delta = calc_override_angle_delta_limited(STEER_OVERRIDE_TORQUE_RANGE, vEgo, VM,
STEER_OVERRIDE_MAX_LAT_JERK_CENTERING)
self.override_angle_accu -= apply_bounds(self.override_angle_accu, centering_delta)
def overriding_steer_desired_accel_limit(self, lat_active: bool, apply_angle: float, vEgo: float, steeringTorque: float) -> float:
"""
@@ -302,7 +329,8 @@ class TorqueBlendController:
# apply_angle = self.overriding_steer_desired_accel_limit(lat_active, apply_angle, CS.out.vEgo, CS.out.steeringTorque)
self.debug_angle_desired_limited = apply_angle #! debug
apply_angle += self.apply_override_angle_combined(lat_active, CS.out.steeringTorque, CS.out.vEgo, VM)
driver_torque = self.torque_zero.update(CS.out.steeringTorque, lat_active)
apply_angle += self.apply_override_angle_combined(lat_active, driver_torque, CS.out.vEgo, VM)
# final rate limit - matching panda safety
self.coop_apply_angle_last = apply_angle

View File

@@ -191,6 +191,10 @@ void ignition_can_hook(CANPacket_t *msg) {
static int toyota_gear = TOYOTA_GEAR_P;
static int toyota_hybrid_gear = TOYOTA_HYBRID_GEAR_P;
static bool toyota_hybrid_gear_seen = false;
static bool vw_meb_seen = false;
static bool vw_meb_getriebe_out_of_p = false;
static bool vw_meb_gateway_out_of_p = false;
static int prev_counter_vw_meb = -1;
// GM exception
if ((msg->addr == 0x1F1U) && (len == 8)) {
@@ -293,6 +297,30 @@ void ignition_can_hook(CANPacket_t *msg) {
}
}
if ((msg->addr == 0xADU) && (len == 8)) {
int fahrstufe = (msg->data[5] >> 2) & 0xFU;
vw_meb_getriebe_out_of_p = (fahrstufe >= 6) && (fahrstufe <= 14);
}
if ((msg->addr == 0x3DCU) && (len == 8)) {
int fahrstufe = msg->data[5] & 0xFU;
vw_meb_gateway_out_of_p = (fahrstufe >= 6) && (fahrstufe <= 14);
}
if ((msg->addr == 0x13DU) && (len == 32)) {
vw_meb_seen = true;
}
if ((msg->addr == 0x3C0U) && (len == 4) && vw_meb_seen) {
int counter = msg->data[1] & 0xFU;
if ((counter == ((prev_counter_vw_meb + 1) % 16)) && (prev_counter_vw_meb != -1)) {
bool vw_meb_out_of_park = vw_meb_getriebe_out_of_p || vw_meb_gateway_out_of_p;
ignition_can = (((msg->data[2] >> 1) & 1U) != 0U) && vw_meb_out_of_park;
ignition_can_cnt = 0U;
ignition_can_gates_line = true;
}
prev_counter_vw_meb = counter;
}
}
}

View File

@@ -6,6 +6,7 @@ live estimator learned, or the driver's fixed software delay — gated by the
"IQLiveSteerDelay" param. The pick is mirrored into "IQSteerDelayCache" so consumers that do
not subscribe to lateralDelay can still read the current value.
"""
from iqpilot.cereal import car
from iqpilot.common.params import Params
_ENABLE_KEY = "IQLiveSteerDelay"
@@ -13,13 +14,32 @@ _FIXED_KEY = "IQSoftwareSteerDelay"
_CACHE_KEY = "IQSteerDelayCache"
def fixed_steer_delay(params, stock_delay):
"""The rack's own delay plus the driver's IQSoftwareSteerDelay offset, as the UI reports it."""
return stock_delay + float(params.get(_FIXED_KEY, return_default=True))
def resolve_steer_delay(params, stock_delay):
"""Learned lateral delay while live-learning is enabled, otherwise the stock delay."""
"""Learned lateral delay while live-learning is enabled, otherwise the driver's fixed delay."""
if not params.get_bool(_ENABLE_KEY):
return stock_delay
return fixed_steer_delay(params, stock_delay)
return float(params.get(_CACHE_KEY, return_default=True))
def lateral_action_delay(params, car_params, live_delay):
"""Delay the lateral path should be planned against.
Angle cars honour the IQLiveSteerDelay toggle so that with live learning off the
estimate never reaches the path: lagd cross-correlates against localizer lateral
accel, so it reports whole-vehicle response (~0.36 s measured on VW MQB, 0.44 s on
Tesla) where the lookahead wants actuator delay (~0.10 s). Torque cars keep the
live estimate.
"""
if car_params.steerControlType == car.CarParams.SteerControlType.angle:
return resolve_steer_delay(params, car_params.steerActuatorDelay)
return live_delay
def cached_steer_delay():
"""Last value SteerDelayPublisher mirrored into the param — usable without a
lateralDelay subscription (e.g. at process startup)."""
@@ -34,10 +54,7 @@ class SteerDelayPublisher:
self._params = Params()
self._actuator_delay = car_params.steerActuatorDelay
def _fixed_delay(self):
return self._actuator_delay + self._params.get(_FIXED_KEY, return_default=True)
def update(self, lag_msg):
live = self._params.get_bool(_ENABLE_KEY)
value = lag_msg.lateralDelay.lateralDelay if live else self._fixed_delay()
value = lag_msg.lateralDelay.lateralDelay if live else fixed_steer_delay(self._params, self._actuator_delay)
self._params.put_nonblocking(_CACHE_KEY, value)

View File

@@ -0,0 +1,91 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import time
import pytest
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal import car
from iqpilot.common.params import Params
from iqpilot.common.steer_delay import (
SteerDelayPublisher,
cached_steer_delay,
fixed_steer_delay,
lateral_action_delay,
resolve_steer_delay,
)
ANGLE = car.CarParams.SteerControlType.angle
TORQUE = car.CarParams.SteerControlType.torque
LIVE_DELAY = 0.4387
RACK_DELAY = 0.10
OFFSET = 0.05
@pytest.fixture
def params(tmp_path, monkeypatch):
monkeypatch.setenv("PARAMS_ROOT", str(tmp_path))
p = Params()
p.put("IQSteerDelayCache", LIVE_DELAY)
p.put("IQSoftwareSteerDelay", OFFSET)
return p
def _car_params(steer_control_type):
cp = car.CarParams.new_message()
cp.steerControlType = steer_control_type
cp.steerActuatorDelay = RACK_DELAY
return cp
def _lateral_delay_msg(value):
msg = messaging.new_message("lateralDelay")
msg.lateralDelay.lateralDelay = value
return msg.as_reader()
def test_params_fixture_is_isolated_from_the_real_device(params, tmp_path):
assert str(tmp_path) in params.get_param_path("")
@pytest.mark.parametrize("live_enabled", [True, False])
def test_torque_cars_always_use_live_delay(params, live_enabled):
params.put_bool("IQLiveSteerDelay", live_enabled)
assert lateral_action_delay(params, _car_params(TORQUE), LIVE_DELAY) == pytest.approx(LIVE_DELAY)
def test_angle_cars_ignore_live_delay_when_self_tuning_is_off(params):
params.put_bool("IQLiveSteerDelay", False)
delay = lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY)
assert delay == pytest.approx(RACK_DELAY + OFFSET)
assert delay != pytest.approx(LIVE_DELAY)
def test_angle_cars_use_cached_delay_when_self_tuning_is_on(params):
params.put_bool("IQLiveSteerDelay", True)
assert lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY) == pytest.approx(LIVE_DELAY)
@pytest.mark.parametrize("offset", [0.05, 0.20, 0.50])
def test_manual_offset_reaches_the_path_and_matches_what_the_ui_reports(params, offset):
params.put_bool("IQLiveSteerDelay", False)
params.put("IQSoftwareSteerDelay", offset)
ui_total = RACK_DELAY + offset
assert fixed_steer_delay(params, RACK_DELAY) == pytest.approx(ui_total)
assert lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY) == pytest.approx(ui_total)
@pytest.mark.parametrize("live_enabled", [False, True])
def test_publisher_writes_the_value_the_resolver_reads(params, live_enabled):
params.put_bool("IQLiveSteerDelay", live_enabled)
params.put("IQSteerDelayCache", -1.0)
SteerDelayPublisher(_car_params(ANGLE)).update(_lateral_delay_msg(LIVE_DELAY))
expected = LIVE_DELAY if live_enabled else RACK_DELAY + OFFSET
deadline = time.monotonic() + 5.0
while cached_steer_delay() != pytest.approx(expected) and time.monotonic() < deadline:
time.sleep(0.01)
assert cached_steer_delay() == pytest.approx(expected)
assert resolve_steer_delay(params, RACK_DELAY) == pytest.approx(expected)

View File

@@ -13,7 +13,7 @@ from iqpilot.common.swaglog import cloudlog
from iqdbc.car.car_helpers import interfaces
from iqdbc.car.vehicle_model import VehicleModel
from iqpilot.common.steer_delay import resolve_steer_delay
from iqpilot.common.steer_delay import lateral_action_delay
from iqpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
from iqpilot.selfdrive.controls.lib.curvature_lookahead import get_lookahead_curvature
from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
@@ -227,15 +227,7 @@ class Controls(IQControlsLayer):
lat_accel_override = bool(CS.gasPressed) or bool(self.sm['iqState'].aol.active)
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll, lat_accel_override)
# ALC (angle control) only: honour IQLiveSteerDelay so that with live learning off, lagd's
# estimate never reaches the controls loop and CP.steerActuatorDelay is used instead. lagd
# cross-correlates against localizer lateral accel, so it reports whole-vehicle response
# (~0.36 s measured on VW MQB) where the lookahead wants actuator delay (~0.10 s).
# Torque cars keep their existing path.
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
lat_delay = resolve_steer_delay(self.params, self.CP.steerActuatorDelay) + LAT_SMOOTH_SECONDS
else:
lat_delay = self.sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS
lat_delay = lateral_action_delay(self.params, self.CP, self.sm["lateralDelay"].lateralDelay) + LAT_SMOOTH_SECONDS
lookahead_curvature = None
if not self.sm.valid['lateralManeuverPlan']:
lookahead_curvature = get_lookahead_curvature(model_v2, CS.vEgo, lat_delay)

View File

@@ -14,7 +14,6 @@ from iqdbc.car import structs
from iqpilot.common.constants import CV
from iqpilot.common.params import Params
from iqpilot.common.swaglog import cloudlog
from iqpilot.common.steer_delay import resolve_steer_delay
from iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import build_iq_control_params_from_plan
from iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
from iqpilot.selfdrive.controls.lib.helpers.blinker_pause import IQSignalPauseController
@@ -60,7 +59,7 @@ class IQControlsLayer(InferenceStateBase):
return
self.blinker_pause_lateral.get_params()
if self.CP.lateralTuning.which() == 'torque':
self.lat_delay = resolve_steer_delay(self.params, sm["lateralDelay"].lateralDelay)
self.lat_delay = sm["lateralDelay"].lateralDelay
self._sync_set_speed = self._want_set_speed_to_limit()
self.radar_manager.read_params()
self._next_param_refresh = now

View File

@@ -32,7 +32,7 @@ from iqpilot.selfdrive.controls.lib.drive_helpers import (
from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
from iqpilot.system import sentry
from iqpilot.common.steer_delay import resolve_steer_delay
from iqpilot.common.steer_delay import lateral_action_delay
from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
from iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import get_model_runner
@@ -510,7 +510,7 @@ class InferenceDaemon:
def _refresh_tunables(self, tick: int) -> None:
if tick % 60 != 0:
return
self._runtime.lat_delay = resolve_steer_delay(self._params, self._sub["lateralDelay"].lateralDelay)
self._runtime.lat_delay = lateral_action_delay(self._params, self._car_params, self._sub["lateralDelay"].lateralDelay)
self._runtime.PLANPLUS_CONTROL = self._params.get("PlanplusControl", return_default=True)
self._runtime.model_smoothing_max_extra_sec = _model_lat_smooth_max_sec(self._params)
self._warps.set_offset(self._params.get("CameraOffset", return_default=True))

View File

@@ -112,13 +112,26 @@ class TinygradFusedRunner(ModelRunner):
}
# shapes must match the captured run_policy JIT inputs
on_shapes = self._on_meta['input_shapes']
captured = self._run_policy.captured
jit_shapes = {
name: tuple(int(s) for s in view.shape)
for name, (view, _vars, _dtype, _device) in zip(captured.expected_names, captured.expected_input_info)
}
def policy_input_shape(name):
shape = on_shapes.get(name, jit_shapes.get(name))
if shape is None:
raise ValueError(f"fused pkl declares no shape for policy input {name}")
return shape
self._npy_buffers = {
'desire': np.zeros(dp[2], dtype=np.float32),
'traffic_convention': np.zeros(on_shapes['traffic_convention'], dtype=np.float32),
'action_t': np.zeros(on_shapes['action_t'], dtype=np.float32),
'traffic_convention': np.zeros(policy_input_shape('traffic_convention'), dtype=np.float32),
'tfm': np.zeros((3, 3), dtype=np.float32),
'big_tfm': np.zeros((3, 3), dtype=np.float32),
}
if 'action_t' in jit_shapes:
self._npy_buffers['action_t'] = np.zeros(policy_input_shape('action_t'), dtype=np.float32)
self._cam_resolution = (cam_w, cam_h)
def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict:
@@ -134,7 +147,7 @@ class TinygradFusedRunner(ModelRunner):
self._npy_buffers['desire'][:] = numpy_inputs[desire_key]
if 'traffic_convention' in numpy_inputs:
self._npy_buffers['traffic_convention'][:] = numpy_inputs['traffic_convention']
if 'action_t' in numpy_inputs:
if 'action_t' in numpy_inputs and 'action_t' in self._npy_buffers:
self._npy_buffers['action_t'][:] = numpy_inputs['action_t']
self._npy_buffers['tfm'][:] = transforms['img']
self._npy_buffers['big_tfm'][:] = transforms['big_img']
@@ -149,9 +162,12 @@ class TinygradFusedRunner(ModelRunner):
img, big_img = warp_jit(img_q=self._queues['img_q'], big_img_q=self._queues['big_img_q'],
tfm=npy('tfm'), big_tfm=npy('big_tfm'), frame=frame, big_frame=big_frame)
vision_out_t, on_out_t, off_out_t = self._run_policy(
policy_inputs = dict(
img=img, big_img=big_img, feat_q=self._queues['feat_q'], desire_q=self._queues['desire_q'],
desire=npy('desire'), traffic_convention=npy('traffic_convention'), action_t=npy('action_t'))
desire=npy('desire'), traffic_convention=npy('traffic_convention'))
if 'action_t' in self._npy_buffers:
policy_inputs['action_t'] = npy('action_t')
vision_out_t, on_out_t, off_out_t = self._run_policy(**policy_inputs)
# parse each model's output on its own sliced dict; parsing a merged dict
# would run parse_dynamic_outputs twice and double-parse plan/lead

View File

@@ -0,0 +1,128 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import pickle
from dataclasses import dataclass
import numpy as np
import pytest
from iqpilot.selfdrive.iqmodeld.models.runners import model_runner as model_runner_mod
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelType
from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad import fused_runner as fused_mod
class _View:
def __init__(self, shape):
self.shape = shape
class _Captured:
def __init__(self, expected_names, expected_input_info):
self.expected_names = expected_names
self.expected_input_info = expected_input_info
class _FakeJit:
def __init__(self, expected_names, expected_input_info):
self.captured = _Captured(expected_names, expected_input_info)
def __call__(self, **kwargs):
raise AssertionError("policy jit should not run in this test")
class _FakeTensor:
def __init__(self, arr, device=None):
self.shape = tuple(np.asarray(arr).shape)
def contiguous(self):
return self
def realize(self):
return self
class _FakeDevice:
DEFAULT = "FAKE"
@dataclass
class _Type:
raw: int
@dataclass
class _Artifact:
fileName: str
class _Model:
def __init__(self, file_name):
self.type = _Type(ModelType.vision)
self.artifact = _Artifact(file_name)
self.metadata = None
class _Bundle:
def __init__(self, file_name):
self.models = [_Model(file_name)]
self.is20hz = True
POLICY_INPUTS = ["action_t", "big_img", "desire", "desire_q", "feat_q", "img", "traffic_convention"]
POLICY_SHAPES = {
"action_t": (1, 2), "big_img": (1, 12, 128, 256), "desire": (1, 8), "desire_q": (1, 100, 8),
"feat_q": (1, 99, 512), "img": (1, 12, 128, 256), "traffic_convention": (1, 2),
}
def _write_fused_pkl(path, policy_inputs):
info = [(_View(POLICY_SHAPES[n]), (), None, "NPY") for n in policy_inputs]
role_meta = {
"input_shapes": {"desire_pulse": (1, 100, 8), "traffic_convention": (1, 2), "features_buffer": (1, 99, 512)},
"output_slices": {},
}
blob = {
"metadata": {
"vision": {"input_shapes": {"img": (1, 12, 128, 256), "big_img": (1, 12, 128, 256)}, "output_slices": {}},
"on_policy": role_meta,
"off_policy": role_meta,
},
"run_policy": _FakeJit(policy_inputs, info),
"frame_skip": 4,
(1928, 1208): _FakeJit(["frame"], [(_View((1,)), (), None, "NPY")]),
}
with open(path, "wb") as f:
pickle.dump(blob, f)
@pytest.fixture
def fused_runner(tmp_path, monkeypatch):
def _build(policy_inputs):
name = "driving_fused_test.pkl"
_write_fused_pkl(tmp_path / name, policy_inputs)
monkeypatch.setattr(model_runner_mod, "_fetch_bundle", lambda params=None: _Bundle(name))
monkeypatch.setattr(fused_mod, "CUSTOM_MODEL_PATH", str(tmp_path))
monkeypatch.setattr(fused_mod, "_tinygrad_imports", lambda: (_FakeTensor, _FakeDevice))
return fused_mod.TinygradFusedRunner()
return _build
def test_action_t_allocated_when_only_the_jit_declares_it(fused_runner):
runner = fused_runner(POLICY_INPUTS)
assert "action_t" not in runner._on_meta["input_shapes"]
runner._ensure_queues(1928, 1208)
assert runner._npy_buffers["action_t"].shape == POLICY_SHAPES["action_t"]
assert runner._npy_buffers["traffic_convention"].shape == POLICY_SHAPES["traffic_convention"]
def test_action_t_absent_when_the_jit_does_not_take_it(fused_runner):
runner = fused_runner([n for n in POLICY_INPUTS if n != "action_t"])
runner._ensure_queues(1928, 1208)
assert "action_t" not in runner._npy_buffers

View File

@@ -0,0 +1,72 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
from types import SimpleNamespace
import pytest
from iqpilot.cereal import car
from iqpilot.common.params import Params
from iqpilot.selfdrive.iqmodeld.daemon import InferenceDaemon
LIVE_DELAY = 0.4387
RACK_DELAY = 0.10
OFFSET = 0.05
@pytest.fixture
def params(tmp_path, monkeypatch):
monkeypatch.setenv("PARAMS_ROOT", str(tmp_path))
p = Params()
p.put("IQSteerDelayCache", LIVE_DELAY)
p.put("IQSoftwareSteerDelay", OFFSET)
p.put_bool("ModelSmoothingEnabled", False)
p.put("ModelLatSmoothSec", 0)
p.put("PlanplusControl", 1.0)
p.put("CameraOffset", 0.0)
return p
def _daemon(params, steer_control_type):
car_params = car.CarParams.new_message()
car_params.steerControlType = steer_control_type
car_params.steerActuatorDelay = RACK_DELAY
return SimpleNamespace(
_params=params,
_car_params=car_params,
_sub={"lateralDelay": SimpleNamespace(lateralDelay=LIVE_DELAY)},
_runtime=SimpleNamespace(lat_delay=None, PLANPLUS_CONTROL=None, model_smoothing_max_extra_sec=None),
_warps=SimpleNamespace(set_offset=lambda _: None),
)
@pytest.mark.parametrize("live_enabled, expected", [(False, RACK_DELAY + OFFSET), (True, LIVE_DELAY)])
def test_angle_cars_honour_the_self_tuning_toggle(params, live_enabled, expected):
params.put_bool("IQLiveSteerDelay", live_enabled)
daemon = _daemon(params, car.CarParams.SteerControlType.angle)
InferenceDaemon._refresh_tunables(daemon, 0)
assert daemon._runtime.lat_delay == pytest.approx(expected)
def test_angle_cars_never_plan_against_the_live_estimate_when_disabled(params):
params.put_bool("IQLiveSteerDelay", False)
daemon = _daemon(params, car.CarParams.SteerControlType.angle)
InferenceDaemon._refresh_tunables(daemon, 0)
assert daemon._runtime.lat_delay != pytest.approx(LIVE_DELAY)
@pytest.mark.parametrize("live_enabled", [True, False])
def test_torque_cars_keep_the_live_estimate(params, live_enabled):
params.put_bool("IQLiveSteerDelay", live_enabled)
daemon = _daemon(params, car.CarParams.SteerControlType.torque)
InferenceDaemon._refresh_tunables(daemon, 0)
assert daemon._runtime.lat_delay == pytest.approx(LIVE_DELAY)
def test_refresh_is_throttled_to_every_sixtieth_tick(params):
params.put_bool("IQLiveSteerDelay", False)
daemon = _daemon(params, car.CarParams.SteerControlType.angle)
InferenceDaemon._refresh_tunables(daemon, 1)
assert daemon._runtime.lat_delay is None
InferenceDaemon._refresh_tunables(daemon, 60)
assert daemon._runtime.lat_delay == pytest.approx(RACK_DELAY + OFFSET)

View File

@@ -10,7 +10,6 @@ from iqpilot.common.realtime import DT_MDL
from iqpilot.common.filter_simple import FirstOrderFilter
from iqpilot.common.swaglog import cloudlog
from iqpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
from iqpilot.common.steer_delay import resolve_steer_delay
HISTORY = 5 # secs
POINTS_PER_BUCKET = 1500
@@ -97,7 +96,6 @@ class TorqueEstimator(ParameterEstimator):
# try to restore cached params
params = Params()
self.params = params
params_cache = params.get("CarParamsPrevRoute")
torque_cache = params.get("LiveTorqueParameters")
if params_cache is not None and torque_cache is not None:
@@ -179,7 +177,7 @@ class TorqueEstimator(ParameterEstimator):
elif which == "extrinsicsCalibration":
self.calibrator.feed_live_calib(msg)
elif which == "lateralDelay":
self.lag = resolve_steer_delay(self.params, msg.lateralDelay)
self.lag = msg.lateralDelay
# calculate lateral accel from past steering torque
elif which == "deviceMotion":
if len(self.raw_points['steer_torque']) == self.hist_len:

View File

@@ -1,7 +1,7 @@
[
{
"name": "xbl",
"url": "https://commadist.azureedge.net/agnosupdate/xbl-dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/xbl-dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6.img.xz",
"hash": "dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6",
"hash_raw": "dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6",
"size": 3282256,
@@ -12,7 +12,7 @@
},
{
"name": "xbl_config",
"url": "https://commadist.azureedge.net/agnosupdate/xbl_config-1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/xbl_config-1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9.img.xz",
"hash": "1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9",
"hash_raw": "1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9",
"size": 98124,
@@ -23,7 +23,7 @@
},
{
"name": "abl",
"url": "https://commadist.azureedge.net/agnosupdate/abl-556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/abl-556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee.img.xz",
"hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee",
"hash_raw": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee",
"size": 274432,
@@ -34,7 +34,7 @@
},
{
"name": "aop",
"url": "https://commadist.azureedge.net/agnosupdate/aop-4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/aop-4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788.img.xz",
"hash": "4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788",
"hash_raw": "4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788",
"size": 184364,
@@ -45,7 +45,7 @@
},
{
"name": "devcfg",
"url": "https://commadist.azureedge.net/agnosupdate/devcfg-2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/devcfg-2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585.img.xz",
"hash": "2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585",
"hash_raw": "2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585",
"size": 40336,
@@ -56,7 +56,7 @@
},
{
"name": "splash",
"url": "https://sdn.konn3kt.com/agnos/16-iqlvbs/splash-993d7fb8ddfa552bd7f60e8a78b8735efbc716a0978682ed3c92fa0d694528d2.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/splash-993d7fb8ddfa552bd7f60e8a78b8735efbc716a0978682ed3c92fa0d694528d2.img.xz",
"hash": "993d7fb8ddfa552bd7f60e8a78b8735efbc716a0978682ed3c92fa0d694528d2",
"hash_raw": "993d7fb8ddfa552bd7f60e8a78b8735efbc716a0978682ed3c92fa0d694528d2",
"size": 34226176,
@@ -67,7 +67,7 @@
},
{
"name": "boot",
"url": "https://sdn.konn3kt.com/agnos/16-iqlvbs/boot-aea4aecefd188d9c95726b699902378676c6266a7ae37008b5c2aa0e1e1190fb.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/boot-aea4aecefd188d9c95726b699902378676c6266a7ae37008b5c2aa0e1e1190fb.img.xz",
"hash": "aea4aecefd188d9c95726b699902378676c6266a7ae37008b5c2aa0e1e1190fb",
"hash_raw": "aea4aecefd188d9c95726b699902378676c6266a7ae37008b5c2aa0e1e1190fb",
"size": 18216960,
@@ -78,18 +78,14 @@
},
{
"name": "system",
"url": "https://sdn.konn3kt.com/agnos/16-iqlvbs/system-37572981c7592d5dd20136e13bed9a9fae84ffc75b9b24da55704bbce7da239b.img.xz",
"hash": "a7bec67c4fef85c66736e74ee8828cbef320f3729f72fee6bea4c97f23dc8b70",
"hash_raw": "37572981c7592d5dd20136e13bed9a9fae84ffc75b9b24da55704bbce7da239b",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/system-44b251e1b3d8cd9243d5c79287d2a0e74b5063d3e21b24192b1293430a3471aa.img.xz",
"hash": "44b251e1b3d8cd9243d5c79287d2a0e74b5063d3e21b24192b1293430a3471aa",
"hash_raw": "44b251e1b3d8cd9243d5c79287d2a0e74b5063d3e21b24192b1293430a3471aa",
"size": 6291456000,
"sparse": true,
"sparse": false,
"full_check": false,
"has_ab": true,
"ondevice_hash": "f678dbc0ffb12e49e6b561ce58d84d3831dc7b7a595909f653b330ecec9c2c65",
"alt": {
"hash": "37572981c7592d5dd20136e13bed9a9fae84ffc75b9b24da55704bbce7da239b",
"url": "https://sdn.konn3kt.com/agnos/16-iqlvbs/system-37572981c7592d5dd20136e13bed9a9fae84ffc75b9b24da55704bbce7da239b.img",
"size": 6291456000
}
"ondevice_hash": "44b251e1b3d8cd9243d5c79287d2a0e74b5063d3e21b24192b1293430a3471aa",
"url_parts": 10
}
]

View File

@@ -1 +1 @@
yxhDP2ieuolHlsCLh4gOQFHFGBIZ5TcewAlO8qo+F3L13hp4K0N5vniN5ZsC7K7Lg6z4TtehTycFb5o4IIIRBA==
IE+GKvyryxGDx98VImAM+UaqOPcs+mzWtlVs7r0h4Va2Tb9glgFBFlpaB3Btll+QI5zoo/tMlDrj463kaYQQBg==

View File

@@ -39,6 +39,81 @@ IQPILOT_MANIFEST_PUBLIC_KEY = bytes.fromhex("40ae3f81b77506ecc4982a1ca37ba1d6f87
AGNOS_MANIFEST_FILE = "system/hardware/tici/agnos.json"
LFS_POINTER_MAGIC = b"version https://git-lfs"
def _image_auth_module():
"""Return the git_remote auth module, or None. On IQ.OS this comes through the
verified loader; on stock AGNOS (an AGNOS->IQ.OS upgrade) that loader is not
present, but the compiled bundle IS in every checkout -- import it directly."""
try:
from iqpilot.system.proprietary_runtime._verified_import import import_verified_module
return import_verified_module("iqpilot_updater_private", "iqpilot_private.updater.git_remote")
except Exception:
pass
try:
root = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "..", ".."))
bundle_python = os.path.join(root, "artifacts", "iqpilot_updater_private", "python")
if os.path.isdir(bundle_python):
if bundle_python not in sys.path:
sys.path.insert(0, bundle_python)
import importlib
return importlib.import_module("iqpilot_private.updater.git_remote")
except Exception:
pass
return None
def _download_headers(url: str) -> dict:
mod = _image_auth_module()
if mod is not None:
try:
headers = mod.os_image_headers(url)
if headers:
return headers
except Exception:
pass
try:
from iqpilot.common.git_creds import get_credentials
creds = get_credentials()
if creds and all(creds) and "/iq.lvbs/iqos" in url.lower():
return {"Authorization": "Basic " + base64.b64encode(f"{creds[0]}:{creds[1]}".encode()).decode()}
except Exception:
pass
return {}
def _open_image_response(url: str) -> requests.Response:
"""GET an image URL; when the server answers with a Git-LFS pointer (the image
repo stores partitions as LFS objects and its raw endpoint does not resolve
them), follow it through the LFS batch API using the same credentials."""
auth = _download_headers(url)
req = requests.get(url, stream=True, headers={'Accept-Encoding': None, **auth}, timeout=60)
req.raise_for_status()
if int(req.headers.get('content-length') or 0) >= 1024:
return req
body = req.content
if not body.startswith(LFS_POINTER_MAGIC):
raise requests.exceptions.InvalidURL(f"unexpected tiny response ({len(body)} bytes) for {url}")
meta = dict(line.split(" ", 1) for line in body.decode().strip().splitlines() if " " in line)
oid = meta["oid"].split(":", 1)[1]
size = int(meta["size"])
batch_url = url.split("/raw/", 1)[0] + ".git/info/lfs/objects/batch"
batch = requests.post(batch_url,
data=json.dumps({"operation": "download", "transfers": ["basic"],
"objects": [{"oid": oid, "size": size}]}),
headers={"Content-Type": "application/vnd.git-lfs+json",
"Accept": "application/vnd.git-lfs+json", **auth},
timeout=60)
batch.raise_for_status()
action = batch.json()["objects"][0]["actions"]["download"]
req = requests.get(action["href"], stream=True,
headers={'Accept-Encoding': None, **action.get("header", {})}, timeout=60)
req.raise_for_status()
return req
def verify_manifest_signature(manifest_path: str) -> None:
sig_path = f"{manifest_path}.sig"
@@ -54,11 +129,34 @@ def verify_manifest_signature(manifest_path: str) -> None:
public_key.verify(signature, digest)
class _ChainedParts:
"""Response-like wrapper streaming N sequential part files as one body.
The image host caps single uploads well below the system image size, so big
images are stored as `<name>.pNN` LFS objects; devices re-join them here."""
def __init__(self, urls: list[str]) -> None:
self.urls = urls
self.req: requests.Response | None = None
def raise_for_status(self) -> None:
if self.req is not None:
self.req.raise_for_status()
def iter_content(self, chunk_size: int) -> Generator[bytes, None, None]:
for u in self.urls:
self.req = _open_image_response(u)
yield from self.req.iter_content(chunk_size=chunk_size)
class StreamingDecompressor:
def __init__(self, url: str) -> None:
def __init__(self, url: str, parts: int = 0) -> None:
self.buf = b""
self.req = requests.get(url, stream=True, headers={'Accept-Encoding': None}, timeout=60)
if parts > 1:
self.req = _ChainedParts([f"{url}.p{i:02d}" for i in range(parts)])
else:
self.req = _open_image_response(url)
self.it = self.req.iter_content(chunk_size=1024 * 1024)
self.decompressor = lzma.LZMADecompressor(format=lzma.FORMAT_AUTO)
self.eof = False
@@ -196,7 +294,7 @@ def clear_partition_hash(target_slot_number: int, partition: dict) -> None:
def extract_compressed_image(target_slot_number: int, partition: dict, cloudlog):
path = get_partition_path(target_slot_number, partition)
downloader = StreamingDecompressor(partition['url'])
downloader = StreamingDecompressor(partition['url'], parts=int(partition.get('url_parts', 0)))
with open(path, 'wb+') as out:
# Flash partition

View File

@@ -1,7 +1,7 @@
[
{
"name": "xbl",
"url": "https://commadist.azureedge.net/agnosupdate/xbl-dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/xbl-dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6.img.xz",
"hash": "dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6",
"hash_raw": "dd45c0febdf0e022dab82ed0219370a86e8e6c0dfabfe29f3dab7eb1174d6bc6",
"size": 3282256,
@@ -12,7 +12,7 @@
},
{
"name": "xbl_config",
"url": "https://commadist.azureedge.net/agnosupdate/xbl_config-1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/xbl_config-1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9.img.xz",
"hash": "1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9",
"hash_raw": "1074ae051df159ba6dba988d8f6ba2cfc304ed1466cce0db531df6f7b1e44aa9",
"size": 98124,
@@ -23,7 +23,7 @@
},
{
"name": "abl",
"url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz",
"hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
"hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
"size": 274432,
@@ -34,7 +34,7 @@
},
{
"name": "aop",
"url": "https://commadist.azureedge.net/agnosupdate/aop-4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/aop-4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788.img.xz",
"hash": "4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788",
"hash_raw": "4d925c9248672e4a69a236991983375008c44997a854ee7846d1b5fd7c787788",
"size": 184364,
@@ -45,7 +45,7 @@
},
{
"name": "devcfg",
"url": "https://commadist.azureedge.net/agnosupdate/devcfg-2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/devcfg-2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585.img.xz",
"hash": "2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585",
"hash_raw": "2f374581243910db92f62bb13bd66ec8e3d56d434997ba007ded06d2d6cc8585",
"size": 40336,
@@ -56,7 +56,7 @@
},
{
"name": "boot",
"url": "https://sdn.konn3kt.com/agnos/16-iqlvbs/boot-aea4aecefd188d9c95726b699902378676c6266a7ae37008b5c2aa0e1e1190fb.img.xz",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/boot-aea4aecefd188d9c95726b699902378676c6266a7ae37008b5c2aa0e1e1190fb.img.xz",
"hash": "aea4aecefd188d9c95726b699902378676c6266a7ae37008b5c2aa0e1e1190fb",
"hash_raw": "aea4aecefd188d9c95726b699902378676c6266a7ae37008b5c2aa0e1e1190fb",
"size": 18216960,
@@ -67,18 +67,14 @@
},
{
"name": "system",
"url": "https://sdn.konn3kt.com/agnos/16-iqlvbs/system-37572981c7592d5dd20136e13bed9a9fae84ffc75b9b24da55704bbce7da239b.img.xz",
"hash": "a7bec67c4fef85c66736e74ee8828cbef320f3729f72fee6bea4c97f23dc8b70",
"hash_raw": "37572981c7592d5dd20136e13bed9a9fae84ffc75b9b24da55704bbce7da239b",
"url": "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqos/raw/branch/master/system-44b251e1b3d8cd9243d5c79287d2a0e74b5063d3e21b24192b1293430a3471aa.img.xz",
"hash": "44b251e1b3d8cd9243d5c79287d2a0e74b5063d3e21b24192b1293430a3471aa",
"hash_raw": "44b251e1b3d8cd9243d5c79287d2a0e74b5063d3e21b24192b1293430a3471aa",
"size": 6291456000,
"sparse": true,
"sparse": false,
"full_check": false,
"has_ab": true,
"ondevice_hash": "f678dbc0ffb12e49e6b561ce58d84d3831dc7b7a595909f653b330ecec9c2c65",
"alt": {
"hash": "37572981c7592d5dd20136e13bed9a9fae84ffc75b9b24da55704bbce7da239b",
"url": "https://sdn.konn3kt.com/agnos/16-iqlvbs/system-37572981c7592d5dd20136e13bed9a9fae84ffc75b9b24da55704bbce7da239b.img",
"size": 6291456000
}
"ondevice_hash": "44b251e1b3d8cd9243d5c79287d2a0e74b5063d3e21b24192b1293430a3471aa",
"url_parts": 10
}
]

View File

@@ -1 +1 @@
PvU/rNvS8JnrZabBbjgss1lyLIlUGXdEOsguFCMK22aY2p/MlzzzpI2+3Wz7tCNcPT+FEbLR1SrskEjTBvKsCQ==
9LDZugtT9q8jFab1Gs8qTmfJukKU8NzDAuT9VakInuOUHCCnN5SDFI/Ew/6C/+3SAlcROgZON/4J8FQyQGV+Dg==

View File

@@ -3,18 +3,42 @@ import os
import requests
TEST_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)))
MANIFEST = os.path.join(TEST_DIR, "../agnos.json")
MANIFESTS = [
os.path.join(TEST_DIR, "../agnos.json"),
os.path.join(TEST_DIR, "../agnos_tici_15_1.json"),
]
IMAGE_HOST = "gitlvb.teallvbs.xyz"
# image payloads are xz streams; the repo raw endpoint would serve an LFS pointer
XZ_MAGIC = b"\xfd7zXZ\x00"
LFS_POINTER_MAGIC = b"version https://git-lfs"
class TestAgnosUpdater:
def test_manifest(self):
with open(MANIFEST) as f:
m = json.load(f)
for manifest in MANIFESTS:
with open(manifest) as f:
m = json.load(f)
for img in m:
r = requests.head(img['url'], timeout=10)
r.raise_for_status()
assert r.headers['Content-Type'].split(';', 1)[0] in {"application/x-xz", "application/octet-stream"}
if not img['sparse']:
assert img['hash'] == img['hash_raw']
for img in m:
assert img['url'].split('/')[2] == IMAGE_HOST
if not img['sparse']:
assert img['hash'] == img['hash_raw']
# contract: images are distributed from a private repo, so an anonymous
# request must never receive image content. The denial status varies by
# route (404 via the CDN, catch-all HTML page when resolved directly to
# the origin), so assert on the payload, not the status code. trust_env
# off: requests otherwise picks up ~/.netrc (CI runners have gitlvb
# credentials), silently authenticating the "anonymous" probe.
s = requests.Session()
s.trust_env = False
r = s.get(img['url'], timeout=10, stream=True,
headers={"User-Agent": "IQOS-Updater"})
if r.status_code in (401, 403, 404):
continue
head = next(r.iter_content(chunk_size=256), b"") or b""
assert not head.startswith(XZ_MAGIC), f"{img['name']}: anonymous request served image content"
assert not head.startswith(LFS_POINTER_MAGIC), f"{img['name']}: anonymous request served the LFS pointer"

View File

@@ -158,6 +158,10 @@ def manager_thread() -> None:
started = sm['deviceState'].started
if started and not started_prev:
try:
HARDWARE.set_power_save(False)
except Exception:
cloudlog.exception("failed to leave power save on onroad transition")
params.clear_all(ParamKeyFlag.CLEAR_ON_ONROAD_TRANSITION)
elif not started and started_prev:
params.clear_all(ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION)

View File

@@ -195,7 +195,7 @@ procs = [
procs += [
# Models
BundleProcess("models_manager", "iqpilot_model_selector_private", "iqpilot_private.models.manager", and_(only_offroad, not_low_power)),
NativeProcess("iqmodeld", "iqpilot/selfdrive/iqmodeld", ["./iqmodeld"], and_(only_onroad, is_tinygrad_model)),
NativeProcess("iqmodeld", "iqpilot/selfdrive/iqmodeld", ["./iqmodeld"], and_(only_onroad, is_tinygrad_model), restart_if_crash=True),
BundleProcess("backup_manager_k3", "iqpilot_hephaestusd_private", "iqpilot_private.konn3kt.backups.backup_orchestrator",
and_(only_offroad, hephaestus_ready_shim, not_low_power)),

View File

@@ -42,10 +42,19 @@ def required_agnos_version(install_path: str) -> str:
return ""
def _hardware_dir(install_path: str) -> str:
# the nested layout keeps the package at <install>/iqpilot/; older checkouts had
# system/ at the top level. Accept either so callers can pass the install root.
nested = os.path.join(install_path, "iqpilot", "system", "hardware", "tici")
if os.path.isdir(nested):
return nested
return os.path.join(install_path, "system", "hardware", "tici")
def agnos_manifest_path(install_path: str, device_type: str) -> str:
# comma 3 (tici) uses a different AGNOS manifest than comma 3x (tizi) / comma 4 (mici).
fname = "agnos_tici_15_1.json" if device_type == "tici" else "agnos.json"
return os.path.join(install_path, "system", "hardware", "tici", fname)
return os.path.join(_hardware_dir(install_path), fname)
def os_update_needed(install_path: str) -> tuple[bool, str, str]:
@@ -64,7 +73,7 @@ def run_agnos_update(install_path: str, device_type: str, progress_cb: ProgressC
via progress_cb(percent, note). Returns True on success. The device must be
rebooted by the caller afterward for the new slot to take effect."""
manifest = agnos_manifest_path(install_path, device_type)
agnos_py = os.path.join(install_path, "system", "hardware", "tici", "agnos.py")
agnos_py = os.path.join(_hardware_dir(install_path), "agnos.py")
if not os.path.isfile(manifest) or not os.path.isfile(agnos_py):
progress_cb(0, "manifest_missing")
return False