Compare commits

..

1 Commits

Author SHA1 Message Date
Bruce Wayne 5faf14e04e 8112843f-2a77-4470-b4d0-4392f41ddbeb/100 2026-04-10 19:29:58 -07:00
5 changed files with 14 additions and 69 deletions
+1 -7
View File
@@ -24,8 +24,6 @@ MIN_STD_SANITY_CHECK = 1e-5 # m or rad
MAX_FILTER_REWIND_TIME = 0.8 # s
MAX_SENSOR_TIME_DIFF = 0.1 # s
YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
LOW_SPEED_GYRO_CAMODO_VEGO = 5.0 # m/s
LOW_SPEED_CAMODO_YAWRATE_STD_FLOOR = 0.02 # rad/s
INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
POSENET_STD_INITIAL_VALUE = 10.0
@@ -135,11 +133,7 @@ class LocationEstimator:
gyro_bias = self.kf.x[States.GYRO_BIAS]
gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0])
gyro_camodo_yawrate_std = self.camodo_yawrate_distribution[1]
if self.car_speed < LOW_SPEED_GYRO_CAMODO_VEGO:
# Camera odometry can become overconfident in tight low-speed maneuvers.
gyro_camodo_yawrate_std = max(gyro_camodo_yawrate_std, LOW_SPEED_CAMODO_YAWRATE_STD_FLOOR)
gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * gyro_camodo_yawrate_std
gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1]
gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold
if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid:
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:e53f4e0527766082ba7bde38e275def0fe3c14f6c59ae2854439e239884d3ecc
size 13393365
oid sha256:a8a77e5d70724fe332dc4dd5111b5cf9845d2c97cacf87b24b352da1b224fd08
size 13393589
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:ea89c50da3a16e710da292f97c81b083a982cfdee5c28eca0d37ed2fb99af6c5
size 13022642
oid sha256:1e0ee93c4275eb5893656e6e95b43cb580e2844c40cf9b9f57019376d808a205
size 13022800
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:6263aa3fbb44cde6c68a34cdb7cd8c389789dbc02b15c1911afdac4e018281ae
size 23267151
oid sha256:84f12bffd54334890b23a1f6a3293ea5443effd66e4ea387dbceaada4b7c0213
size 23267519
+7 -56
View File
@@ -33,26 +33,13 @@ SEND_APDU_RETRIES = 3
LOCK_FILE = '/dev/shm/modem_lpa.lock'
DEBUG = os.environ.get("DEBUG") == "1"
# TLV Tags
TAG_ICCID = 0x5A
TAG_STATUS = 0x80
TAG_PROFILE_INFO_LIST = 0xBF2D
TAG_SET_NICKNAME = 0xBF29
TAG_ENABLE_PROFILE = 0xBF31
TAG_DELETE_PROFILE = 0xBF33
TAG_OK = 0xA0
PROFILE_OK = 0x00
PROFILE_NOT_IN_DISABLED_STATE = 0x02
PROFILE_CAT_BUSY = 0x05
PROFILE_ERROR_CODES = {
0x01: "iccidOrAidNotFound", PROFILE_NOT_IN_DISABLED_STATE: "profileNotInDisabledState",
0x03: "disallowedByPolicy", 0x04: "wrongProfileReenabling",
PROFILE_CAT_BUSY: "catBusy", 0x06: "undefinedError",
}
STATE_LABELS = {0: "disabled", 1: "enabled", 255: "unknown"}
ICON_LABELS = {0: "jpeg", 1: "png", 255: "unknown"}
CLASS_LABELS = {0: "test", 1: "provisioning", 2: "operational", 255: "unknown"}
@@ -82,12 +69,6 @@ class AtClient:
self._serial: serial.Serial | None = None
self._use_dbus = not os.path.exists(device)
def send_raw(self, data: bytes) -> None:
self._ensure_serial()
self._serial.reset_input_buffer()
self._serial.write(data)
self._serial.flush()
def close(self) -> None:
try:
if self.channel:
@@ -186,15 +167,6 @@ class AtClient:
return
raise RuntimeError("Failed to open ISD-R application")
def _reset_modem(self) -> None:
if self._serial:
try:
self._serial.close()
except Exception:
pass
self._serial = None
subprocess.run(['/usr/comma/lte/lte.sh', 'start'], capture_output=True)
def open_isdr(self) -> None:
for attempt in range(OPEN_ISDR_RETRIES):
try:
@@ -203,7 +175,9 @@ class AtClient:
except (RuntimeError, TimeoutError, termios.error, serial.SerialException):
time.sleep(OPEN_ISDR_RETRY_DELAY_S)
if attempt == OPEN_ISDR_RESET_ATTEMPT:
self._reset_modem()
# reset modem via lte.sh
subprocess.run(['/usr/comma/lte/lte.sh', 'start'], capture_output=True)
self._serial = None # serial port will be re-opened on next attempt
raise RuntimeError("Failed to open ISD-R after retries")
def send_apdu(self, apdu: bytes) -> tuple[bytes, int, int]:
@@ -363,7 +337,8 @@ def set_profile_nickname(client: AtClient, iccid: str, nickname: str) -> None:
raise ValueError("Profile nickname must be 64 bytes or less")
content = encode_tlv(TAG_ICCID, string_to_tbcd(iccid)) + encode_tlv(0x90, nickname_bytes)
response = es10x_command(client, encode_tlv(TAG_SET_NICKNAME, content))
code = require_tag(require_tag(response, TAG_SET_NICKNAME, "SetNicknameResponse"), TAG_STATUS, "SetNickname status")[0]
root = require_tag(response, TAG_SET_NICKNAME, "SetNicknameResponse")
code = require_tag(root, TAG_STATUS, "status in SetNicknameResponse")[0]
if code == 0x01:
raise LPAError(f"profile {iccid} not found")
if code != 0x00:
@@ -410,14 +385,7 @@ class TiciLPA(LPABase):
return None
def delete_profile(self, iccid: str) -> None:
if self.is_comma_profile(iccid):
raise LPAError("refusing to delete a comma profile")
with self._acquire_channel():
request = encode_tlv(TAG_DELETE_PROFILE, encode_tlv(TAG_ICCID, string_to_tbcd(iccid)))
response = es10x_command(self._client, request)
code = require_tag(require_tag(response, TAG_DELETE_PROFILE, "DeleteProfileResponse"), TAG_STATUS, "DeleteProfile status")[0]
if code != PROFILE_OK:
raise LPAError(f"DeleteProfile failed: {PROFILE_ERROR_CODES.get(code, 'unknown')} (0x{code:02X})")
return None
def download_profile(self, qr: str, nickname: str | None = None) -> None:
return None
@@ -426,22 +394,5 @@ class TiciLPA(LPABase):
with self._acquire_channel():
set_profile_nickname(self._client, iccid, nickname)
def _enable_profile(self, iccid: str) -> int:
inner = encode_tlv(TAG_OK, encode_tlv(TAG_ICCID, string_to_tbcd(iccid)))
inner += b'\x01\x01\x01' # refreshFlag=1
response = es10x_command(self._client, encode_tlv(TAG_ENABLE_PROFILE, inner))
return require_tag(require_tag(response, TAG_ENABLE_PROFILE, "EnableProfileResponse"), TAG_STATUS, "EnableProfile status")[0]
def switch_profile(self, iccid: str) -> None:
with self._acquire_channel():
code = self._enable_profile(iccid)
if code == PROFILE_CAT_BUSY: # stale eUICC transaction, reset and retry
self._client._reset_modem()
self._client.open_isdr()
code = self._enable_profile(iccid)
if code not in (PROFILE_OK, PROFILE_NOT_IN_DISABLED_STATE):
raise LPAError(f"EnableProfile failed: {PROFILE_ERROR_CODES.get(code, 'unknown')} (0x{code:02X})")
from openpilot.system.hardware import HARDWARE
if HARDWARE.get_device_type() == "mici":
self._client.send_raw(b'AT+CFUN=0\rAT+CFUN=1\r') # mici has no SIM presence pin; raw because CFUN=0 drops serial
self._client._ensure_serial(reconnect=True)
return None