mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-08-22 16:53:49 +08:00
dragonpilot v0.9.8
This commit is contained in:
Comma Device
Executable
+827
@@ -0,0 +1,827 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import base64
|
||||
import hashlib
|
||||
import io
|
||||
import json
|
||||
import os
|
||||
import queue
|
||||
import random
|
||||
import select
|
||||
import socket
|
||||
import sys
|
||||
import tempfile
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import asdict, dataclass, replace
|
||||
from datetime import datetime
|
||||
from functools import partial
|
||||
from queue import Queue
|
||||
from typing import cast
|
||||
from collections.abc import Callable
|
||||
|
||||
import requests
|
||||
from jsonrpc import JSONRPCResponseManager, dispatcher
|
||||
from websocket import (ABNF, WebSocket, WebSocketException, WebSocketTimeoutException,
|
||||
create_connection)
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log
|
||||
from cereal.services import SERVICE_LIST
|
||||
from openpilot.common.api import Api
|
||||
from openpilot.common.file_helpers import CallbackReader, get_upload_stream
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import set_core_affinity
|
||||
from openpilot.system.hardware import HARDWARE, PC
|
||||
from openpilot.system.loggerd.xattr_cache import getxattr, setxattr
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.version import get_build_metadata
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
|
||||
HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4"))
|
||||
LOCAL_PORT_WHITELIST = {22, } # SSH
|
||||
|
||||
LOG_ATTR_NAME = 'user.upload'
|
||||
LOG_ATTR_VALUE_MAX_UNIX_TIME = int.to_bytes(2147483647, 4, sys.byteorder)
|
||||
RECONNECT_TIMEOUT_S = 70
|
||||
|
||||
RETRY_DELAY = 10 # seconds
|
||||
MAX_RETRY_COUNT = 30 # Try for at most 5 minutes if upload fails immediately
|
||||
MAX_AGE = 31 * 24 * 3600 # seconds
|
||||
WS_FRAME_SIZE = 4096
|
||||
DEVICE_STATE_UPDATE_INTERVAL = 1.0 # in seconds
|
||||
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
|
||||
UploadFileDict = dict[str, str | int | float | bool]
|
||||
UploadItemDict = dict[str, str | bool | int | float | dict[str, str]]
|
||||
|
||||
UploadFilesToUrlResponse = dict[str, int | list[UploadItemDict] | list[str]]
|
||||
|
||||
|
||||
@dataclass
|
||||
class UploadFile:
|
||||
fn: str
|
||||
url: str
|
||||
headers: dict[str, str]
|
||||
allow_cellular: bool
|
||||
|
||||
@classmethod
|
||||
def from_dict(cls, d: dict) -> UploadFile:
|
||||
return cls(d.get("fn", ""), d.get("url", ""), d.get("headers", {}), d.get("allow_cellular", False))
|
||||
|
||||
|
||||
@dataclass
|
||||
class UploadItem:
|
||||
path: str
|
||||
url: str
|
||||
headers: dict[str, str]
|
||||
created_at: int
|
||||
id: str | None
|
||||
retry_count: int = 0
|
||||
current: bool = False
|
||||
progress: float = 0
|
||||
allow_cellular: bool = False
|
||||
|
||||
@classmethod
|
||||
def from_dict(cls, d: dict) -> UploadItem:
|
||||
return cls(d["path"], d["url"], d["headers"], d["created_at"], d["id"], d["retry_count"], d["current"],
|
||||
d["progress"], d["allow_cellular"])
|
||||
|
||||
|
||||
dispatcher["echo"] = lambda s: s
|
||||
recv_queue: Queue[str] = queue.Queue()
|
||||
send_queue: Queue[str] = queue.Queue()
|
||||
upload_queue: Queue[UploadItem] = queue.Queue()
|
||||
low_priority_send_queue: Queue[str] = queue.Queue()
|
||||
log_recv_queue: Queue[str] = queue.Queue()
|
||||
cancelled_uploads: set[str] = set()
|
||||
|
||||
cur_upload_items: dict[int, UploadItem | None] = {}
|
||||
|
||||
|
||||
def strip_zst_extension(fn: str) -> str:
|
||||
if fn.endswith('.zst'):
|
||||
return fn[:-4]
|
||||
return fn
|
||||
|
||||
|
||||
class AbortTransferException(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class UploadQueueCache:
|
||||
|
||||
@staticmethod
|
||||
def initialize(upload_queue: Queue[UploadItem]) -> None:
|
||||
try:
|
||||
upload_queue_json = Params().get("AthenadUploadQueue")
|
||||
if upload_queue_json is not None:
|
||||
for item in json.loads(upload_queue_json):
|
||||
upload_queue.put(UploadItem.from_dict(item))
|
||||
except Exception:
|
||||
cloudlog.exception("athena.UploadQueueCache.initialize.exception")
|
||||
|
||||
@staticmethod
|
||||
def cache(upload_queue: Queue[UploadItem]) -> None:
|
||||
try:
|
||||
queue: list[UploadItem | None] = list(upload_queue.queue)
|
||||
items = [asdict(i) for i in queue if i is not None and (i.id not in cancelled_uploads)]
|
||||
Params().put("AthenadUploadQueue", json.dumps(items))
|
||||
except Exception:
|
||||
cloudlog.exception("athena.UploadQueueCache.cache.exception")
|
||||
|
||||
|
||||
def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None:
|
||||
end_event = threading.Event()
|
||||
|
||||
threads = [
|
||||
threading.Thread(target=ws_manage, args=(ws, end_event), name='ws_manage'),
|
||||
threading.Thread(target=ws_recv, args=(ws, end_event), name='ws_recv'),
|
||||
threading.Thread(target=ws_send, args=(ws, end_event), name='ws_send'),
|
||||
threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'),
|
||||
threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler2'),
|
||||
threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler3'),
|
||||
threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler4'),
|
||||
threading.Thread(target=log_handler, args=(end_event,), name='log_handler'),
|
||||
threading.Thread(target=stat_handler, args=(end_event,), name='stat_handler'),
|
||||
] + [
|
||||
threading.Thread(target=jsonrpc_handler, args=(end_event,), name=f'worker_{x}')
|
||||
for x in range(HANDLER_THREADS)
|
||||
]
|
||||
|
||||
for thread in threads:
|
||||
thread.start()
|
||||
try:
|
||||
while not end_event.wait(0.1):
|
||||
if exit_event is not None and exit_event.is_set():
|
||||
end_event.set()
|
||||
except (KeyboardInterrupt, SystemExit):
|
||||
end_event.set()
|
||||
raise
|
||||
finally:
|
||||
for thread in threads:
|
||||
cloudlog.debug(f"athena.joining {thread.name}")
|
||||
thread.join()
|
||||
|
||||
|
||||
def jsonrpc_handler(end_event: threading.Event) -> None:
|
||||
dispatcher["startLocalProxy"] = partial(startLocalProxy, end_event)
|
||||
while not end_event.is_set():
|
||||
try:
|
||||
data = recv_queue.get(timeout=1)
|
||||
if "method" in data:
|
||||
cloudlog.event("athena.jsonrpc_handler.call_method", data=data)
|
||||
response = JSONRPCResponseManager.handle(data, dispatcher)
|
||||
send_queue.put_nowait(response.json)
|
||||
elif "id" in data and ("result" in data or "error" in data):
|
||||
log_recv_queue.put_nowait(data)
|
||||
else:
|
||||
raise Exception("not a valid request or response")
|
||||
except queue.Empty:
|
||||
pass
|
||||
except Exception as e:
|
||||
cloudlog.exception("athena jsonrpc handler failed")
|
||||
send_queue.put_nowait(json.dumps({"error": str(e)}))
|
||||
|
||||
|
||||
def retry_upload(tid: int, end_event: threading.Event, increase_count: bool = True) -> None:
|
||||
item = cur_upload_items[tid]
|
||||
if item is not None and item.retry_count < MAX_RETRY_COUNT:
|
||||
new_retry_count = item.retry_count + 1 if increase_count else item.retry_count
|
||||
|
||||
item = replace(
|
||||
item,
|
||||
retry_count=new_retry_count,
|
||||
progress=0,
|
||||
current=False
|
||||
)
|
||||
upload_queue.put_nowait(item)
|
||||
UploadQueueCache.cache(upload_queue)
|
||||
|
||||
cur_upload_items[tid] = None
|
||||
|
||||
for _ in range(RETRY_DELAY):
|
||||
time.sleep(1)
|
||||
if end_event.is_set():
|
||||
break
|
||||
|
||||
|
||||
def cb(sm, item, tid, end_event: threading.Event, sz: int, cur: int) -> None:
|
||||
# Abort transfer if connection changed to metered after starting upload
|
||||
# or if athenad is shutting down to re-connect the websocket
|
||||
if not item.allow_cellular:
|
||||
if (time.monotonic() - sm.recv_time['deviceState']) > DEVICE_STATE_UPDATE_INTERVAL:
|
||||
sm.update(0)
|
||||
if sm['deviceState'].networkMetered:
|
||||
raise AbortTransferException
|
||||
|
||||
if end_event.is_set():
|
||||
raise AbortTransferException
|
||||
|
||||
cur_upload_items[tid] = replace(item, progress=cur / sz if sz else 1)
|
||||
|
||||
|
||||
def upload_handler(end_event: threading.Event) -> None:
|
||||
sm = messaging.SubMaster(['deviceState'])
|
||||
tid = threading.get_ident()
|
||||
|
||||
while not end_event.is_set():
|
||||
cur_upload_items[tid] = None
|
||||
|
||||
try:
|
||||
cur_upload_items[tid] = item = replace(upload_queue.get(timeout=1), current=True)
|
||||
|
||||
if item.id in cancelled_uploads:
|
||||
cancelled_uploads.remove(item.id)
|
||||
continue
|
||||
|
||||
# Remove item if too old
|
||||
age = datetime.now() - datetime.fromtimestamp(item.created_at / 1000)
|
||||
if age.total_seconds() > MAX_AGE:
|
||||
cloudlog.event("athena.upload_handler.expired", item=item, error=True)
|
||||
continue
|
||||
|
||||
# Check if uploading over metered connection is allowed
|
||||
sm.update(0)
|
||||
metered = sm['deviceState'].networkMetered
|
||||
network_type = sm['deviceState'].networkType.raw
|
||||
if metered and (not item.allow_cellular):
|
||||
retry_upload(tid, end_event, False)
|
||||
continue
|
||||
|
||||
try:
|
||||
fn = item.path
|
||||
try:
|
||||
sz = os.path.getsize(fn)
|
||||
except OSError:
|
||||
sz = -1
|
||||
|
||||
cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=item.retry_count)
|
||||
|
||||
with _do_upload(item, partial(cb, sm, item, tid, end_event)) as response:
|
||||
if response.status_code not in (200, 201, 401, 403, 412):
|
||||
cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
retry_upload(tid, end_event)
|
||||
else:
|
||||
cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
|
||||
UploadQueueCache.cache(upload_queue)
|
||||
except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError):
|
||||
cloudlog.event("athena.upload_handler.timeout", fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
retry_upload(tid, end_event)
|
||||
except AbortTransferException:
|
||||
cloudlog.event("athena.upload_handler.abort", fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
retry_upload(tid, end_event, False)
|
||||
|
||||
except queue.Empty:
|
||||
pass
|
||||
except Exception:
|
||||
cloudlog.exception("athena.upload_handler.exception")
|
||||
|
||||
|
||||
def _do_upload(upload_item: UploadItem, callback: Callable = None) -> requests.Response:
|
||||
path = upload_item.path
|
||||
compress = False
|
||||
|
||||
# If file does not exist, but does exist without the .zst extension we will compress on the fly
|
||||
if not os.path.exists(path) and os.path.exists(strip_zst_extension(path)):
|
||||
path = strip_zst_extension(path)
|
||||
compress = True
|
||||
|
||||
stream = None
|
||||
try:
|
||||
stream, content_length = get_upload_stream(path, compress)
|
||||
response = requests.put(upload_item.url,
|
||||
data=CallbackReader(stream, callback, content_length) if callback else stream,
|
||||
headers={**upload_item.headers, 'Content-Length': str(content_length)},
|
||||
timeout=30)
|
||||
return response
|
||||
finally:
|
||||
if stream:
|
||||
stream.close()
|
||||
|
||||
|
||||
# security: user should be able to request any message from their car
|
||||
@dispatcher.add_method
|
||||
def getMessage(service: str, timeout: int = 1000) -> dict:
|
||||
if service is None or service not in SERVICE_LIST:
|
||||
raise Exception("invalid service")
|
||||
|
||||
socket = messaging.sub_sock(service, timeout=timeout)
|
||||
try:
|
||||
ret = messaging.recv_one(socket)
|
||||
|
||||
if ret is None:
|
||||
raise TimeoutError
|
||||
|
||||
# this is because capnp._DynamicStructReader doesn't have typing information
|
||||
return cast(dict, ret.to_dict())
|
||||
finally:
|
||||
del socket
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def getVersion() -> dict[str, str]:
|
||||
build_metadata = get_build_metadata()
|
||||
return {
|
||||
"version": build_metadata.openpilot.version,
|
||||
"remote": build_metadata.openpilot.git_normalized_origin,
|
||||
"branch": build_metadata.channel,
|
||||
"commit": build_metadata.openpilot.git_commit,
|
||||
}
|
||||
|
||||
|
||||
def scan_dir(path: str, prefix: str) -> list[str]:
|
||||
files = []
|
||||
# only walk directories that match the prefix
|
||||
# (glob and friends traverse entire dir tree)
|
||||
with os.scandir(path) as i:
|
||||
for e in i:
|
||||
rel_path = os.path.relpath(e.path, Paths.log_root())
|
||||
if e.is_dir(follow_symlinks=False):
|
||||
# add trailing slash
|
||||
rel_path = os.path.join(rel_path, '')
|
||||
# if prefix is a partial dir name, current dir will start with prefix
|
||||
# if prefix is a partial file name, prefix with start with dir name
|
||||
if rel_path.startswith(prefix) or prefix.startswith(rel_path):
|
||||
files.extend(scan_dir(e.path, prefix))
|
||||
else:
|
||||
if rel_path.startswith(prefix):
|
||||
files.append(rel_path)
|
||||
return files
|
||||
|
||||
@dispatcher.add_method
|
||||
def listDataDirectory(prefix='') -> list[str]:
|
||||
return scan_dir(Paths.log_root(), prefix)
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def uploadFileToUrl(fn: str, url: str, headers: dict[str, str]) -> UploadFilesToUrlResponse:
|
||||
# this is because mypy doesn't understand that the decorator doesn't change the return type
|
||||
response: UploadFilesToUrlResponse = uploadFilesToUrls([{
|
||||
"fn": fn,
|
||||
"url": url,
|
||||
"headers": headers,
|
||||
}])
|
||||
return response
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def uploadFilesToUrls(files_data: list[UploadFileDict]) -> UploadFilesToUrlResponse:
|
||||
files = map(UploadFile.from_dict, files_data)
|
||||
|
||||
items: list[UploadItemDict] = []
|
||||
failed: list[str] = []
|
||||
for file in files:
|
||||
if len(file.fn) == 0 or file.fn[0] == '/' or '..' in file.fn or len(file.url) == 0:
|
||||
failed.append(file.fn)
|
||||
continue
|
||||
|
||||
path = os.path.join(Paths.log_root(), file.fn)
|
||||
if not os.path.exists(path) and not os.path.exists(strip_zst_extension(path)):
|
||||
failed.append(file.fn)
|
||||
continue
|
||||
|
||||
# Skip item if already in queue
|
||||
url = file.url.split('?')[0]
|
||||
if any(url == item['url'].split('?')[0] for item in listUploadQueue()):
|
||||
continue
|
||||
|
||||
item = UploadItem(
|
||||
path=path,
|
||||
url=file.url,
|
||||
headers=file.headers,
|
||||
created_at=int(time.time() * 1000),
|
||||
id=None,
|
||||
allow_cellular=file.allow_cellular,
|
||||
)
|
||||
upload_id = hashlib.sha1(str(item).encode()).hexdigest()
|
||||
item = replace(item, id=upload_id)
|
||||
upload_queue.put_nowait(item)
|
||||
items.append(asdict(item))
|
||||
|
||||
UploadQueueCache.cache(upload_queue)
|
||||
|
||||
resp: UploadFilesToUrlResponse = {"enqueued": len(items), "items": items}
|
||||
if failed:
|
||||
cloudlog.event("athena.uploadFilesToUrls.failed", failed=failed, error=True)
|
||||
resp["failed"] = failed
|
||||
|
||||
return resp
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def listUploadQueue() -> list[UploadItemDict]:
|
||||
items = list(upload_queue.queue) + list(cur_upload_items.values())
|
||||
return [asdict(i) for i in items if (i is not None) and (i.id not in cancelled_uploads)]
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def cancelUpload(upload_id: str | list[str]) -> dict[str, int | str]:
|
||||
if not isinstance(upload_id, list):
|
||||
upload_id = [upload_id]
|
||||
|
||||
uploading_ids = {item.id for item in list(upload_queue.queue)}
|
||||
cancelled_ids = uploading_ids.intersection(upload_id)
|
||||
if len(cancelled_ids) == 0:
|
||||
return {"success": 0, "error": "not found"}
|
||||
|
||||
cancelled_uploads.update(cancelled_ids)
|
||||
return {"success": 1}
|
||||
|
||||
@dispatcher.add_method
|
||||
def setRouteViewed(route: str) -> dict[str, int | str]:
|
||||
# maintain a list of the last 10 routes viewed in connect
|
||||
params = Params()
|
||||
|
||||
r = params.get("AthenadRecentlyViewedRoutes", encoding="utf8")
|
||||
routes = [] if r is None else r.split(",")
|
||||
routes.append(route)
|
||||
|
||||
# remove duplicates
|
||||
routes = list(dict.fromkeys(routes))
|
||||
|
||||
params.put("AthenadRecentlyViewedRoutes", ",".join(routes[-10:]))
|
||||
return {"success": 1}
|
||||
|
||||
|
||||
def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local_port: int) -> dict[str, int]:
|
||||
try:
|
||||
# migration, can be removed once 0.9.8 is out for a while
|
||||
if local_port == 8022:
|
||||
local_port = 22
|
||||
|
||||
if local_port not in LOCAL_PORT_WHITELIST:
|
||||
raise Exception("Requested local port not whitelisted")
|
||||
|
||||
cloudlog.debug("athena.startLocalProxy.starting")
|
||||
|
||||
dongle_id = Params().get("DongleId").decode('utf8')
|
||||
identity_token = Api(dongle_id).get_token()
|
||||
ws = create_connection(remote_ws_uri,
|
||||
cookie="jwt=" + identity_token,
|
||||
enable_multithread=True)
|
||||
|
||||
# Set TOS to keep connection responsive while under load.
|
||||
# DSCP of 36/HDD_LINUX_AC_VI with the minimum delay flag
|
||||
ws.sock.setsockopt(socket.IPPROTO_IP, socket.IP_TOS, 0x90)
|
||||
|
||||
ssock, csock = socket.socketpair()
|
||||
local_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
local_sock.connect(('127.0.0.1', local_port))
|
||||
local_sock.setblocking(False)
|
||||
|
||||
proxy_end_event = threading.Event()
|
||||
threads = [
|
||||
threading.Thread(target=ws_proxy_recv, args=(ws, local_sock, ssock, proxy_end_event, global_end_event)),
|
||||
threading.Thread(target=ws_proxy_send, args=(ws, local_sock, csock, proxy_end_event))
|
||||
]
|
||||
for thread in threads:
|
||||
thread.start()
|
||||
|
||||
cloudlog.debug("athena.startLocalProxy.started")
|
||||
return {"success": 1}
|
||||
except Exception as e:
|
||||
cloudlog.exception("athenad.startLocalProxy.exception")
|
||||
raise e
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def getPublicKey() -> str | None:
|
||||
if not os.path.isfile(Paths.persist_root() + '/comma/id_rsa.pub'):
|
||||
return None
|
||||
|
||||
with open(Paths.persist_root() + '/comma/id_rsa.pub') as f:
|
||||
return f.read()
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def getSshAuthorizedKeys() -> str:
|
||||
return Params().get("GithubSshKeys", encoding='utf8') or ''
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def getGithubUsername() -> str:
|
||||
return Params().get("GithubUsername", encoding='utf8') or ''
|
||||
|
||||
@dispatcher.add_method
|
||||
def getSimInfo():
|
||||
return HARDWARE.get_sim_info()
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def getNetworkType():
|
||||
return HARDWARE.get_network_type()
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def getNetworkMetered() -> bool:
|
||||
network_type = HARDWARE.get_network_type()
|
||||
return HARDWARE.get_network_metered(network_type)
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def getNetworks():
|
||||
return HARDWARE.get_networks()
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
def takeSnapshot() -> str | dict[str, str] | None:
|
||||
from openpilot.system.camerad.snapshot.snapshot import jpeg_write, snapshot
|
||||
ret = snapshot()
|
||||
if ret is not None:
|
||||
def b64jpeg(x):
|
||||
if x is not None:
|
||||
f = io.BytesIO()
|
||||
jpeg_write(f, x)
|
||||
return base64.b64encode(f.getvalue()).decode("utf-8")
|
||||
else:
|
||||
return None
|
||||
return {'jpegBack': b64jpeg(ret[0]),
|
||||
'jpegFront': b64jpeg(ret[1])}
|
||||
else:
|
||||
raise Exception("not available while camerad is started")
|
||||
|
||||
|
||||
def get_logs_to_send_sorted() -> list[str]:
|
||||
# TODO: scan once then use inotify to detect file creation/deletion
|
||||
curr_time = int(time.time())
|
||||
logs = []
|
||||
for log_entry in os.listdir(Paths.swaglog_root()):
|
||||
log_path = os.path.join(Paths.swaglog_root(), log_entry)
|
||||
time_sent = 0
|
||||
try:
|
||||
value = getxattr(log_path, LOG_ATTR_NAME)
|
||||
if value is not None:
|
||||
time_sent = int.from_bytes(value, sys.byteorder)
|
||||
except (ValueError, TypeError):
|
||||
pass
|
||||
# assume send failed and we lost the response if sent more than one hour ago
|
||||
if not time_sent or curr_time - time_sent > 3600:
|
||||
logs.append(log_entry)
|
||||
# excluding most recent (active) log file
|
||||
return sorted(logs)[:-1]
|
||||
|
||||
|
||||
def log_handler(end_event: threading.Event) -> None:
|
||||
if PC:
|
||||
return
|
||||
|
||||
log_files = []
|
||||
last_scan = 0.
|
||||
while not end_event.is_set():
|
||||
try:
|
||||
curr_scan = time.monotonic()
|
||||
if curr_scan - last_scan > 10:
|
||||
log_files = get_logs_to_send_sorted()
|
||||
last_scan = curr_scan
|
||||
|
||||
# send one log
|
||||
curr_log = None
|
||||
if len(log_files) > 0:
|
||||
log_entry = log_files.pop() # newest log file
|
||||
cloudlog.debug(f"athena.log_handler.forward_request {log_entry}")
|
||||
try:
|
||||
curr_time = int(time.time())
|
||||
log_path = os.path.join(Paths.swaglog_root(), log_entry)
|
||||
setxattr(log_path, LOG_ATTR_NAME, int.to_bytes(curr_time, 4, sys.byteorder))
|
||||
with open(log_path) as f:
|
||||
jsonrpc = {
|
||||
"method": "forwardLogs",
|
||||
"params": {
|
||||
"logs": f.read()
|
||||
},
|
||||
"jsonrpc": "2.0",
|
||||
"id": log_entry
|
||||
}
|
||||
low_priority_send_queue.put_nowait(json.dumps(jsonrpc))
|
||||
curr_log = log_entry
|
||||
except OSError:
|
||||
pass # file could be deleted by log rotation
|
||||
|
||||
# wait for response up to ~100 seconds
|
||||
# always read queue at least once to process any old responses that arrive
|
||||
for _ in range(100):
|
||||
if end_event.is_set():
|
||||
break
|
||||
try:
|
||||
log_resp = json.loads(log_recv_queue.get(timeout=1))
|
||||
log_entry = log_resp.get("id")
|
||||
log_success = "result" in log_resp and log_resp["result"].get("success")
|
||||
cloudlog.debug(f"athena.log_handler.forward_response {log_entry} {log_success}")
|
||||
if log_entry and log_success:
|
||||
log_path = os.path.join(Paths.swaglog_root(), log_entry)
|
||||
try:
|
||||
setxattr(log_path, LOG_ATTR_NAME, LOG_ATTR_VALUE_MAX_UNIX_TIME)
|
||||
except OSError:
|
||||
pass # file could be deleted by log rotation
|
||||
if curr_log == log_entry:
|
||||
break
|
||||
except queue.Empty:
|
||||
if curr_log is None:
|
||||
break
|
||||
|
||||
except Exception:
|
||||
cloudlog.exception("athena.log_handler.exception")
|
||||
|
||||
|
||||
def stat_handler(end_event: threading.Event) -> None:
|
||||
STATS_DIR = Paths.stats_root()
|
||||
last_scan = 0.0
|
||||
|
||||
while not end_event.is_set():
|
||||
curr_scan = time.monotonic()
|
||||
try:
|
||||
if curr_scan - last_scan > 10:
|
||||
stat_filenames = list(filter(lambda name: not name.startswith(tempfile.gettempprefix()), os.listdir(STATS_DIR)))
|
||||
if len(stat_filenames) > 0:
|
||||
stat_path = os.path.join(STATS_DIR, stat_filenames[0])
|
||||
with open(stat_path) as f:
|
||||
jsonrpc = {
|
||||
"method": "storeStats",
|
||||
"params": {
|
||||
"stats": f.read()
|
||||
},
|
||||
"jsonrpc": "2.0",
|
||||
"id": stat_filenames[0]
|
||||
}
|
||||
low_priority_send_queue.put_nowait(json.dumps(jsonrpc))
|
||||
os.remove(stat_path)
|
||||
last_scan = curr_scan
|
||||
except Exception:
|
||||
cloudlog.exception("athena.stat_handler.exception")
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
def ws_proxy_recv(ws: WebSocket, local_sock: socket.socket, ssock: socket.socket, end_event: threading.Event, global_end_event: threading.Event) -> None:
|
||||
while not (end_event.is_set() or global_end_event.is_set()):
|
||||
try:
|
||||
r = select.select((ws.sock,), (), (), 30)
|
||||
if r[0]:
|
||||
data = ws.recv()
|
||||
if isinstance(data, str):
|
||||
data = data.encode("utf-8")
|
||||
local_sock.sendall(data)
|
||||
except WebSocketTimeoutException:
|
||||
pass
|
||||
except Exception:
|
||||
cloudlog.exception("athenad.ws_proxy_recv.exception")
|
||||
break
|
||||
|
||||
cloudlog.debug("athena.ws_proxy_recv closing sockets")
|
||||
ssock.close()
|
||||
local_sock.close()
|
||||
ws.close()
|
||||
cloudlog.debug("athena.ws_proxy_recv done closing sockets")
|
||||
|
||||
end_event.set()
|
||||
|
||||
|
||||
def ws_proxy_send(ws: WebSocket, local_sock: socket.socket, signal_sock: socket.socket, end_event: threading.Event) -> None:
|
||||
while not end_event.is_set():
|
||||
try:
|
||||
r, _, _ = select.select((local_sock, signal_sock), (), ())
|
||||
if r:
|
||||
if r[0].fileno() == signal_sock.fileno():
|
||||
# got end signal from ws_proxy_recv
|
||||
end_event.set()
|
||||
break
|
||||
data = local_sock.recv(4096)
|
||||
if not data:
|
||||
# local_sock is dead
|
||||
end_event.set()
|
||||
break
|
||||
|
||||
ws.send(data, ABNF.OPCODE_BINARY)
|
||||
except Exception:
|
||||
cloudlog.exception("athenad.ws_proxy_send.exception")
|
||||
end_event.set()
|
||||
|
||||
cloudlog.debug("athena.ws_proxy_send closing sockets")
|
||||
signal_sock.close()
|
||||
cloudlog.debug("athena.ws_proxy_send done closing sockets")
|
||||
|
||||
|
||||
def ws_recv(ws: WebSocket, end_event: threading.Event) -> None:
|
||||
last_ping = int(time.monotonic() * 1e9)
|
||||
while not end_event.is_set():
|
||||
try:
|
||||
opcode, data = ws.recv_data(control_frame=True)
|
||||
if opcode in (ABNF.OPCODE_TEXT, ABNF.OPCODE_BINARY):
|
||||
if opcode == ABNF.OPCODE_TEXT:
|
||||
data = data.decode("utf-8")
|
||||
recv_queue.put_nowait(data)
|
||||
elif opcode == ABNF.OPCODE_PING:
|
||||
last_ping = int(time.monotonic() * 1e9)
|
||||
Params().put("LastAthenaPingTime", str(last_ping))
|
||||
except WebSocketTimeoutException:
|
||||
ns_since_last_ping = int(time.monotonic() * 1e9) - last_ping
|
||||
if ns_since_last_ping > RECONNECT_TIMEOUT_S * 1e9:
|
||||
cloudlog.exception("athenad.ws_recv.timeout")
|
||||
end_event.set()
|
||||
except Exception:
|
||||
cloudlog.exception("athenad.ws_recv.exception")
|
||||
end_event.set()
|
||||
|
||||
|
||||
def ws_send(ws: WebSocket, end_event: threading.Event) -> None:
|
||||
while not end_event.is_set():
|
||||
try:
|
||||
try:
|
||||
data = send_queue.get_nowait()
|
||||
except queue.Empty:
|
||||
data = low_priority_send_queue.get(timeout=1)
|
||||
for i in range(0, len(data), WS_FRAME_SIZE):
|
||||
frame = data[i:i+WS_FRAME_SIZE]
|
||||
last = i + WS_FRAME_SIZE >= len(data)
|
||||
opcode = ABNF.OPCODE_TEXT if i == 0 else ABNF.OPCODE_CONT
|
||||
ws.send_frame(ABNF.create_frame(frame, opcode, last))
|
||||
except queue.Empty:
|
||||
pass
|
||||
except Exception:
|
||||
cloudlog.exception("athenad.ws_send.exception")
|
||||
end_event.set()
|
||||
|
||||
|
||||
def ws_manage(ws: WebSocket, end_event: threading.Event) -> None:
|
||||
params = Params()
|
||||
onroad_prev = None
|
||||
sock = ws.sock
|
||||
|
||||
while True:
|
||||
onroad = params.get_bool("IsOnroad")
|
||||
if onroad != onroad_prev:
|
||||
onroad_prev = onroad
|
||||
|
||||
if sock is not None:
|
||||
# While not sending data, onroad, we can expect to time out in 7 + (7 * 2) = 21s
|
||||
# offroad, we can expect to time out in 30 + (10 * 3) = 60s
|
||||
# FIXME: TCP_USER_TIMEOUT is effectively 2x for some reason (32s), so it's mostly unused
|
||||
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_USER_TIMEOUT, 16000 if onroad else 0)
|
||||
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPIDLE, 7 if onroad else 30)
|
||||
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPINTVL, 7 if onroad else 10)
|
||||
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPCNT, 2 if onroad else 3)
|
||||
|
||||
if end_event.wait(5):
|
||||
break
|
||||
|
||||
|
||||
def backoff(retries: int) -> int:
|
||||
return random.randrange(0, min(128, int(2 ** retries)))
|
||||
|
||||
|
||||
def main(exit_event: threading.Event = None):
|
||||
try:
|
||||
set_core_affinity([0, 1, 2, 3])
|
||||
except Exception:
|
||||
cloudlog.exception("failed to set core affinity")
|
||||
|
||||
params = Params()
|
||||
dongle_id = params.get("DongleId", encoding='utf-8')
|
||||
UploadQueueCache.initialize(upload_queue)
|
||||
|
||||
ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id
|
||||
api = Api(dongle_id)
|
||||
|
||||
conn_start = None
|
||||
conn_retries = 0
|
||||
while exit_event is None or not exit_event.is_set():
|
||||
try:
|
||||
if conn_start is None:
|
||||
conn_start = time.monotonic()
|
||||
|
||||
cloudlog.event("athenad.main.connecting_ws", ws_uri=ws_uri, retries=conn_retries)
|
||||
ws = create_connection(ws_uri,
|
||||
cookie="jwt=" + api.get_token(),
|
||||
enable_multithread=True,
|
||||
timeout=30.0)
|
||||
cloudlog.event("athenad.main.connected_ws", ws_uri=ws_uri, retries=conn_retries,
|
||||
duration=time.monotonic() - conn_start)
|
||||
conn_start = None
|
||||
|
||||
conn_retries = 0
|
||||
cur_upload_items.clear()
|
||||
|
||||
handle_long_poll(ws, exit_event)
|
||||
|
||||
ws.close()
|
||||
except (KeyboardInterrupt, SystemExit):
|
||||
break
|
||||
except (ConnectionError, TimeoutError, WebSocketException):
|
||||
conn_retries += 1
|
||||
params.remove("LastAthenaPingTime")
|
||||
except Exception:
|
||||
cloudlog.exception("athenad.main.exception")
|
||||
|
||||
conn_retries += 1
|
||||
params.remove("LastAthenaPingTime")
|
||||
|
||||
time.sleep(backoff(conn_retries))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Executable
+43
@@ -0,0 +1,43 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import time
|
||||
from multiprocessing import Process
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.system.manager.process import launcher
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
from openpilot.system.version import get_build_metadata
|
||||
|
||||
ATHENA_MGR_PID_PARAM = "AthenadPid"
|
||||
|
||||
|
||||
def main():
|
||||
params = Params()
|
||||
dongle_id = params.get("DongleId").decode('utf-8')
|
||||
build_metadata = get_build_metadata()
|
||||
|
||||
cloudlog.bind_global(dongle_id=dongle_id,
|
||||
version=build_metadata.openpilot.version,
|
||||
origin=build_metadata.openpilot.git_normalized_origin,
|
||||
branch=build_metadata.channel,
|
||||
commit=build_metadata.openpilot.git_commit,
|
||||
dirty=build_metadata.openpilot.is_dirty,
|
||||
device=HARDWARE.get_device_type())
|
||||
|
||||
try:
|
||||
while 1:
|
||||
cloudlog.info("starting athena daemon")
|
||||
proc = Process(name='athenad', target=launcher, args=('system.athena.athenad', 'athenad'))
|
||||
proc.start()
|
||||
proc.join()
|
||||
cloudlog.event("athenad exited", exitcode=proc.exitcode)
|
||||
time.sleep(5)
|
||||
except Exception:
|
||||
cloudlog.exception("manage_athenad.exception")
|
||||
finally:
|
||||
params.remove(ATHENA_MGR_PID_PARAM)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Executable
+109
@@ -0,0 +1,109 @@
|
||||
#!/usr/bin/env python3
|
||||
import time
|
||||
import json
|
||||
import jwt
|
||||
from pathlib import Path
|
||||
|
||||
from datetime import datetime, timedelta, UTC
|
||||
from openpilot.common.api import api_get
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.spinner import Spinner
|
||||
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
|
||||
from openpilot.system.hardware import HARDWARE, PC
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
import os
|
||||
|
||||
|
||||
UNREGISTERED_DONGLE_ID = "UnregisteredDevice"
|
||||
|
||||
def is_registered_device() -> bool:
|
||||
dongle = Params().get("DongleId", encoding='utf-8')
|
||||
return dongle not in (None, UNREGISTERED_DONGLE_ID)
|
||||
|
||||
|
||||
def register(show_spinner=False) -> str | None:
|
||||
"""
|
||||
All devices built since March 2024 come with all
|
||||
info stored in /persist/. This is kept around
|
||||
only for devices built before then.
|
||||
|
||||
With a backend update to take serial number instead
|
||||
of dongle ID to some endpoints, this can be removed
|
||||
entirely.
|
||||
"""
|
||||
params = Params()
|
||||
|
||||
dongle_id: str | None = params.get("DongleId", encoding='utf8')
|
||||
if dongle_id is None and Path(Paths.persist_root()+"/comma/dongle_id").is_file():
|
||||
# not all devices will have this; added early in comma 3X production (2/28/24)
|
||||
with open(Paths.persist_root()+"/comma/dongle_id") as f:
|
||||
dongle_id = f.read().strip()
|
||||
|
||||
pubkey = Path(Paths.persist_root()+"/comma/id_rsa.pub")
|
||||
if not pubkey.is_file():
|
||||
dongle_id = UNREGISTERED_DONGLE_ID
|
||||
cloudlog.warning(f"missing public key: {pubkey}")
|
||||
elif dongle_id is None:
|
||||
if os.getenv("DISABLE_DRIVER"):
|
||||
params.put("DongleId", UNREGISTERED_DONGLE_ID)
|
||||
return dongle_id
|
||||
if show_spinner:
|
||||
spinner = Spinner()
|
||||
spinner.update("registering device")
|
||||
|
||||
# Create registration token, in the future, this key will make JWTs directly
|
||||
with open(Paths.persist_root()+"/comma/id_rsa.pub") as f1, open(Paths.persist_root()+"/comma/id_rsa") as f2:
|
||||
public_key = f1.read()
|
||||
private_key = f2.read()
|
||||
|
||||
# Block until we get the imei
|
||||
serial = HARDWARE.get_serial()
|
||||
start_time = time.monotonic()
|
||||
imei1: str | None = None
|
||||
imei2: str | None = None
|
||||
while imei1 is None and imei2 is None:
|
||||
try:
|
||||
imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1)
|
||||
except Exception:
|
||||
cloudlog.exception("Error getting imei, trying again...")
|
||||
time.sleep(1)
|
||||
|
||||
if time.monotonic() - start_time > 60 and show_spinner:
|
||||
spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})")
|
||||
|
||||
backoff = 0
|
||||
start_time = time.monotonic()
|
||||
while True:
|
||||
try:
|
||||
register_token = jwt.encode({'register': True, 'exp': datetime.now(UTC).replace(tzinfo=None) + timedelta(hours=1)}, private_key, algorithm='RS256')
|
||||
cloudlog.info("getting pilotauth")
|
||||
resp = api_get("v2/pilotauth/", method='POST', timeout=5,
|
||||
imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token)
|
||||
|
||||
if resp.status_code in (402, 403):
|
||||
cloudlog.info(f"Unable to register device, got {resp.status_code}")
|
||||
dongle_id = UNREGISTERED_DONGLE_ID
|
||||
else:
|
||||
dongleauth = json.loads(resp.text)
|
||||
dongle_id = dongleauth["dongle_id"]
|
||||
break
|
||||
except Exception:
|
||||
cloudlog.exception("failed to authenticate")
|
||||
backoff = min(backoff + 1, 15)
|
||||
time.sleep(backoff)
|
||||
|
||||
if time.monotonic() - start_time > 60 and show_spinner:
|
||||
spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})")
|
||||
|
||||
if show_spinner:
|
||||
spinner.close()
|
||||
|
||||
if dongle_id:
|
||||
params.put("DongleId", dongle_id)
|
||||
set_offroad_alert("Offroad_UnofficialHardware", (dongle_id == UNREGISTERED_DONGLE_ID) and not PC)
|
||||
return dongle_id
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print(register())
|
||||
@@ -0,0 +1,11 @@
|
||||
Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc')
|
||||
|
||||
libs = [common, 'OpenCL', messaging, visionipc, gpucommon]
|
||||
|
||||
if arch != "Darwin":
|
||||
camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/spectra.cc',
|
||||
'cameras/cdm.cc', 'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc'])
|
||||
env.Program('camerad', ['main.cc', camera_obj], LIBS=libs)
|
||||
|
||||
if GetOption("extras") and arch == "x86_64":
|
||||
env.Program('test/test_ae_gray', ['test/test_ae_gray.cc', camera_obj], LIBS=libs)
|
||||
Executable
BIN
Binary file not shown.
File diff suppressed because one or more lines are too long
@@ -0,0 +1,46 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "msgq/visionipc/visionipc_server.h"
|
||||
#include "common/util.h"
|
||||
|
||||
|
||||
const int VIPC_BUFFER_COUNT = 18;
|
||||
|
||||
typedef struct FrameMetadata {
|
||||
uint32_t frame_id;
|
||||
uint32_t request_id;
|
||||
uint64_t timestamp_sof;
|
||||
uint64_t timestamp_eof;
|
||||
float processing_time;
|
||||
} FrameMetadata;
|
||||
|
||||
class SpectraCamera;
|
||||
|
||||
class CameraBuf {
|
||||
private:
|
||||
int frame_buf_count;
|
||||
|
||||
public:
|
||||
VisionIpcServer *vipc_server;
|
||||
VisionStreamType stream_type;
|
||||
|
||||
int cur_buf_idx;
|
||||
FrameMetadata cur_frame_data;
|
||||
VisionBuf *cur_yuv_buf;
|
||||
VisionBuf *cur_camera_buf;
|
||||
std::unique_ptr<VisionBuf[]> camera_bufs_raw;
|
||||
int out_img_width, out_img_height;
|
||||
|
||||
CameraBuf() = default;
|
||||
~CameraBuf();
|
||||
void init(cl_device_id device_id, cl_context context, SpectraCamera *cam, VisionIpcServer * v, int frame_cnt, VisionStreamType type);
|
||||
void sendFrameToVipc();
|
||||
};
|
||||
|
||||
void camerad_thread();
|
||||
kj::Array<uint8_t> get_raw_frame_image(const CameraBuf *b);
|
||||
float calculate_exposure_value(const CameraBuf *b, Rect ae_xywh, int x_skip, int y_skip);
|
||||
int open_v4l_by_name_and_index(const char name[], int index = 0, int flags = O_RDWR | O_NONBLOCK);
|
||||
@@ -0,0 +1,79 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
// our helpers
|
||||
int write_random(uint8_t *dst, const std::vector<uint32_t> &vals);
|
||||
int write_cont(uint8_t *dst, uint32_t reg, const std::vector<uint32_t> &vals);
|
||||
int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel);
|
||||
|
||||
// from drivers/media/platform/msm/camera/cam_cdm/cam_cdm_util.{c,h}
|
||||
|
||||
enum cam_cdm_command {
|
||||
CAM_CDM_CMD_UNUSED = 0x0,
|
||||
CAM_CDM_CMD_DMI = 0x1,
|
||||
CAM_CDM_CMD_NOT_DEFINED = 0x2,
|
||||
CAM_CDM_CMD_REG_CONT = 0x3,
|
||||
CAM_CDM_CMD_REG_RANDOM = 0x4,
|
||||
CAM_CDM_CMD_BUFF_INDIRECT = 0x5,
|
||||
CAM_CDM_CMD_GEN_IRQ = 0x6,
|
||||
CAM_CDM_CMD_WAIT_EVENT = 0x7,
|
||||
CAM_CDM_CMD_CHANGE_BASE = 0x8,
|
||||
CAM_CDM_CMD_PERF_CTRL = 0x9,
|
||||
CAM_CDM_CMD_DMI_32 = 0xa,
|
||||
CAM_CDM_CMD_DMI_64 = 0xb,
|
||||
CAM_CDM_CMD_PRIVATE_BASE = 0xc,
|
||||
CAM_CDM_CMD_SWD_DMI_32 = (CAM_CDM_CMD_PRIVATE_BASE + 0x64),
|
||||
CAM_CDM_CMD_SWD_DMI_64 = (CAM_CDM_CMD_PRIVATE_BASE + 0x65),
|
||||
CAM_CDM_CMD_PRIVATE_BASE_MAX = 0x7F
|
||||
};
|
||||
|
||||
/**
|
||||
* struct cdm_regrandom_cmd - Definition for CDM random register command.
|
||||
* @count: Number of register writes
|
||||
* @reserved: reserved bits
|
||||
* @cmd: Command ID (CDMCmd)
|
||||
*/
|
||||
struct cdm_regrandom_cmd {
|
||||
unsigned int count : 16;
|
||||
unsigned int reserved : 8;
|
||||
unsigned int cmd : 8;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
/**
|
||||
* struct cdm_regcontinuous_cmd - Definition for a CDM register range command.
|
||||
* @count: Number of register writes
|
||||
* @reserved0: reserved bits
|
||||
* @cmd: Command ID (CDMCmd)
|
||||
* @offset: Start address of the range of registers
|
||||
* @reserved1: reserved bits
|
||||
*/
|
||||
struct cdm_regcontinuous_cmd {
|
||||
unsigned int count : 16;
|
||||
unsigned int reserved0 : 8;
|
||||
unsigned int cmd : 8;
|
||||
unsigned int offset : 24;
|
||||
unsigned int reserved1 : 8;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
/**
|
||||
* struct cdm_dmi_cmd - Definition for a CDM DMI command.
|
||||
* @length: Number of bytes in LUT - 1
|
||||
* @reserved: reserved bits
|
||||
* @cmd: Command ID (CDMCmd)
|
||||
* @addr: Address of the LUT in memory
|
||||
* @DMIAddr: Address of the target DMI config register
|
||||
* @DMISel: DMI identifier
|
||||
*/
|
||||
struct cdm_dmi_cmd {
|
||||
unsigned int length : 16;
|
||||
unsigned int reserved : 8;
|
||||
unsigned int cmd : 8;
|
||||
unsigned int addr;
|
||||
unsigned int DMIAddr : 24;
|
||||
unsigned int DMISel : 8;
|
||||
} __attribute__((__packed__));
|
||||
@@ -0,0 +1,68 @@
|
||||
#pragma once
|
||||
|
||||
#include "common/util.h"
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
#include "msgq/visionipc/visionipc_server.h"
|
||||
|
||||
#include "media/cam_isp_ife.h"
|
||||
|
||||
|
||||
typedef enum {
|
||||
ISP_RAW_OUTPUT, // raw frame from sensor
|
||||
ISP_IFE_PROCESSED, // fully processed image through the IFE
|
||||
ISP_BPS_PROCESSED, // fully processed image through the BPS
|
||||
} SpectraOutputType;
|
||||
|
||||
// For the comma 3/3X three camera platform
|
||||
|
||||
struct CameraConfig {
|
||||
int camera_num;
|
||||
VisionStreamType stream_type;
|
||||
float focal_len; // millimeters
|
||||
const char *publish_name;
|
||||
cereal::FrameData::Builder (cereal::Event::Builder::*init_camera_state)();
|
||||
bool enabled;
|
||||
uint32_t phy;
|
||||
bool vignetting_correction;
|
||||
SpectraOutputType output_type;
|
||||
};
|
||||
|
||||
// NOTE: to be able to disable road and wide road, we still have to configure the sensor over i2c
|
||||
// If you don't do this, the strobe GPIO is an output (even in reset it seems!)
|
||||
const CameraConfig WIDE_ROAD_CAMERA_CONFIG = {
|
||||
.camera_num = 0,
|
||||
.stream_type = VISION_STREAM_WIDE_ROAD,
|
||||
.focal_len = 1.71,
|
||||
.publish_name = "wideRoadCameraState",
|
||||
.init_camera_state = &cereal::Event::Builder::initWideRoadCameraState,
|
||||
.enabled = !getenv("DISABLE_WIDE_ROAD"),
|
||||
.phy = CAM_ISP_IFE_IN_RES_PHY_0,
|
||||
.vignetting_correction = false,
|
||||
.output_type = ISP_IFE_PROCESSED,
|
||||
};
|
||||
|
||||
const CameraConfig ROAD_CAMERA_CONFIG = {
|
||||
.camera_num = 1,
|
||||
.stream_type = VISION_STREAM_ROAD,
|
||||
.focal_len = 8.0,
|
||||
.publish_name = "roadCameraState",
|
||||
.init_camera_state = &cereal::Event::Builder::initRoadCameraState,
|
||||
.enabled = !getenv("DISABLE_ROAD"),
|
||||
.phy = CAM_ISP_IFE_IN_RES_PHY_1,
|
||||
.vignetting_correction = true,
|
||||
.output_type = ISP_IFE_PROCESSED,
|
||||
};
|
||||
|
||||
const CameraConfig DRIVER_CAMERA_CONFIG = {
|
||||
.camera_num = 2,
|
||||
.stream_type = VISION_STREAM_DRIVER,
|
||||
.focal_len = 1.71,
|
||||
.publish_name = "driverCameraState",
|
||||
.init_camera_state = &cereal::Event::Builder::initDriverCameraState,
|
||||
.enabled = !getenv("DISABLE_DRIVER"),
|
||||
.phy = CAM_ISP_IFE_IN_RES_PHY_2,
|
||||
.vignetting_correction = false,
|
||||
.output_type = ISP_BPS_PROCESSED,
|
||||
};
|
||||
|
||||
const CameraConfig ALL_CAMERA_CONFIGS[] = {WIDE_ROAD_CAMERA_CONFIG, ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG};
|
||||
@@ -0,0 +1,236 @@
|
||||
#pragma once
|
||||
|
||||
#include "cdm.h"
|
||||
|
||||
#include "system/camerad/cameras/hw.h"
|
||||
#include "system/camerad/sensors/sensor.h"
|
||||
|
||||
int build_common_ife_bps(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches, bool ife) {
|
||||
uint8_t *start = dst;
|
||||
|
||||
/*
|
||||
Common between IFE and BPS.
|
||||
*/
|
||||
|
||||
// IFE -> BPS addresses
|
||||
/*
|
||||
std::map<uint32_t, uint32_t> addrs = {
|
||||
{0xf30, 0x3468},
|
||||
};
|
||||
*/
|
||||
|
||||
// YUV
|
||||
dst += write_cont(dst, ife ? 0xf30 : 0x3468, {
|
||||
0x00680208,
|
||||
0x00000108,
|
||||
0x00400000,
|
||||
0x03ff0000,
|
||||
0x01c01ed8,
|
||||
0x00001f68,
|
||||
0x02000000,
|
||||
0x03ff0000,
|
||||
0x1fb81e88,
|
||||
0x000001c0,
|
||||
0x02000000,
|
||||
0x03ff0000,
|
||||
});
|
||||
|
||||
return dst - start;
|
||||
}
|
||||
|
||||
int build_update(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches) {
|
||||
uint8_t *start = dst;
|
||||
|
||||
// init sequence
|
||||
dst += write_random(dst, {
|
||||
0x2c, 0xffffffff,
|
||||
0x30, 0xffffffff,
|
||||
0x34, 0xffffffff,
|
||||
0x38, 0xffffffff,
|
||||
0x3c, 0xffffffff,
|
||||
});
|
||||
|
||||
// demux cfg
|
||||
dst += write_cont(dst, 0x560, {
|
||||
0x00000001,
|
||||
0x04440444,
|
||||
0x04450445,
|
||||
0x04440444,
|
||||
0x04450445,
|
||||
0x000000ca,
|
||||
0x0000009c,
|
||||
});
|
||||
|
||||
// white balance
|
||||
dst += write_cont(dst, 0x6fc, {
|
||||
0x00800080,
|
||||
0x00000080,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
});
|
||||
|
||||
// module config/enables (e.g. enable debayer, white balance, etc.)
|
||||
dst += write_cont(dst, 0x40, {
|
||||
0x00000c06 | ((uint32_t)(cam.vignetting_correction) << 8),
|
||||
});
|
||||
dst += write_cont(dst, 0x44, {
|
||||
0x00000000,
|
||||
});
|
||||
dst += write_cont(dst, 0x48, {
|
||||
(1 << 3) | (1 << 1),
|
||||
});
|
||||
dst += write_cont(dst, 0x4c, {
|
||||
0x00000019,
|
||||
});
|
||||
dst += write_cont(dst, 0xf00, {
|
||||
0x00000000,
|
||||
});
|
||||
|
||||
// cropping
|
||||
dst += write_cont(dst, 0xe0c, {
|
||||
0x00000e00,
|
||||
});
|
||||
dst += write_cont(dst, 0xe2c, {
|
||||
0x00000e00,
|
||||
});
|
||||
|
||||
// black level scale + offset
|
||||
dst += write_cont(dst, 0x6b0, {
|
||||
((uint32_t)(1 << 11) << 0xf) | (s->black_level << (14 - s->bits_per_pixel)),
|
||||
0x0,
|
||||
0x0,
|
||||
});
|
||||
|
||||
return dst - start;
|
||||
}
|
||||
|
||||
|
||||
int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches) {
|
||||
uint8_t *start = dst;
|
||||
|
||||
// start with the every frame config
|
||||
dst += build_update(dst, cam, s, patches);
|
||||
|
||||
uint64_t addr;
|
||||
|
||||
// setup
|
||||
dst += write_cont(dst, 0x478, {
|
||||
0x00000004,
|
||||
0x004000c0,
|
||||
});
|
||||
dst += write_cont(dst, 0x488, {
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000f0f,
|
||||
});
|
||||
dst += write_cont(dst, 0x49c, {
|
||||
0x00000001,
|
||||
});
|
||||
dst += write_cont(dst, 0xce4, {
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
});
|
||||
|
||||
// linearization
|
||||
dst += write_cont(dst, 0x4dc, {
|
||||
0x00000000,
|
||||
});
|
||||
dst += write_cont(dst, 0x4e0, s->linearization_pts);
|
||||
dst += write_cont(dst, 0x4f0, s->linearization_pts);
|
||||
dst += write_cont(dst, 0x500, s->linearization_pts);
|
||||
dst += write_cont(dst, 0x510, s->linearization_pts);
|
||||
// TODO: this is DMI64 in the dump, does that matter?
|
||||
dst += write_dmi(dst, &addr, s->linearization_lut.size()*sizeof(uint32_t), 0xc24, 9);
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
|
||||
// vignetting correction
|
||||
dst += write_cont(dst, 0x6bc, {
|
||||
0x0b3c0000,
|
||||
0x00670067,
|
||||
0xd3b1300c,
|
||||
0x13b1300c,
|
||||
});
|
||||
dst += write_cont(dst, 0x6d8, {
|
||||
0xec4e4000,
|
||||
0x0100c003,
|
||||
});
|
||||
dst += write_dmi(dst, &addr, s->vignetting_lut.size()*sizeof(uint32_t), 0xc24, 14); // GRR
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
dst += write_dmi(dst, &addr, s->vignetting_lut.size()*sizeof(uint32_t), 0xc24, 15); // GBB
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
|
||||
// debayer
|
||||
dst += write_cont(dst, 0x6f8, {
|
||||
0x00000100,
|
||||
});
|
||||
dst += write_cont(dst, 0x71c, {
|
||||
0x00008000,
|
||||
0x08000066,
|
||||
});
|
||||
|
||||
// color correction
|
||||
dst += write_cont(dst, 0x760, s->color_correct_matrix);
|
||||
|
||||
// gamma
|
||||
dst += write_cont(dst, 0x798, {
|
||||
0x00000000,
|
||||
});
|
||||
dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 26); // G
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 28); // B
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 30); // R
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
|
||||
// output size/scaling
|
||||
dst += write_cont(dst, 0xa3c, {
|
||||
0x00000003,
|
||||
((s->frame_width - 1) << 16) | (s->frame_width - 1),
|
||||
0x30036666,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
s->frame_width - 1,
|
||||
((s->frame_height - 1) << 16) | (s->frame_height - 1),
|
||||
0x30036666,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
s->frame_height - 1,
|
||||
});
|
||||
dst += write_cont(dst, 0xa68, {
|
||||
0x00000003,
|
||||
((s->frame_width/2 - 1) << 16) | (s->frame_width - 1),
|
||||
0x3006cccc,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
s->frame_width - 1,
|
||||
((s->frame_height/2 - 1) << 16) | (s->frame_height - 1),
|
||||
0x3006cccc,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
s->frame_height - 1,
|
||||
});
|
||||
|
||||
// cropping
|
||||
dst += write_cont(dst, 0xe10, {
|
||||
s->frame_height - 1,
|
||||
s->frame_width - 1,
|
||||
});
|
||||
dst += write_cont(dst, 0xe30, {
|
||||
s->frame_height/2 - 1,
|
||||
s->frame_width - 1,
|
||||
});
|
||||
dst += write_cont(dst, 0xe18, {
|
||||
0x0ff00000,
|
||||
0x00000016,
|
||||
});
|
||||
dst += write_cont(dst, 0xe38, {
|
||||
0x0ff00000,
|
||||
0x00000017,
|
||||
});
|
||||
|
||||
dst += build_common_ife_bps(dst, cam, s, patches, true);
|
||||
|
||||
return dst - start;
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,219 @@
|
||||
#pragma once
|
||||
|
||||
#include <sys/mman.h>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <optional>
|
||||
#include <utility>
|
||||
|
||||
#include "media/cam_req_mgr.h"
|
||||
|
||||
#include "common/util.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "system/camerad/cameras/hw.h"
|
||||
#include "system/camerad/cameras/camera_common.h"
|
||||
#include "system/camerad/sensors/sensor.h"
|
||||
|
||||
#define MAX_IFE_BUFS 20
|
||||
|
||||
const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py
|
||||
|
||||
// For use with the Titan 170 ISP in the SDM845
|
||||
// https://github.com/commaai/agnos-kernel-sdm845
|
||||
|
||||
// CSLDeviceType/CSLPacketOpcodesIFE from camx
|
||||
// cam_packet_header.op_code = (device << 24) | (opcode);
|
||||
#define CSLDeviceTypeImageSensor (0x01 << 24)
|
||||
#define CSLDeviceTypeIFE (0x0F << 24)
|
||||
#define CSLDeviceTypeBPS (0x10 << 24)
|
||||
#define OpcodesIFEInitialConfig 0x0
|
||||
#define OpcodesIFEUpdate 0x1
|
||||
|
||||
std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1);
|
||||
int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle);
|
||||
int device_control(int fd, int op_code, int session_handle, int dev_handle);
|
||||
int do_cam_control(int fd, int op_code, void *handle, int size);
|
||||
void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
|
||||
int mmu_hdl = 0, int mmu_hdl2 = 0);
|
||||
void release(int video0_fd, uint32_t handle);
|
||||
|
||||
class MemoryManager {
|
||||
public:
|
||||
void init(int _video0_fd) { video0_fd = _video0_fd; }
|
||||
~MemoryManager();
|
||||
|
||||
template <class T>
|
||||
auto alloc(int len, uint32_t *handle) {
|
||||
return std::unique_ptr<T, std::function<void(void *)>>((T*)alloc_buf(len, handle), [this](void *ptr) { this->free(ptr); });
|
||||
}
|
||||
|
||||
private:
|
||||
void *alloc_buf(int len, uint32_t *handle);
|
||||
void free(void *ptr);
|
||||
|
||||
std::map<void *, uint32_t> handle_lookup;
|
||||
std::map<void *, int> size_lookup;
|
||||
std::map<int, std::queue<void *> > cached_allocations;
|
||||
int video0_fd;
|
||||
};
|
||||
|
||||
class SpectraMaster {
|
||||
public:
|
||||
void init();
|
||||
|
||||
unique_fd video0_fd;
|
||||
unique_fd cam_sync_fd;
|
||||
unique_fd isp_fd;
|
||||
unique_fd icp_fd;
|
||||
int device_iommu = -1;
|
||||
int cdm_iommu = -1;
|
||||
int icp_device_iommu = -1;
|
||||
MemoryManager mem_mgr;
|
||||
};
|
||||
|
||||
class SpectraBuf {
|
||||
public:
|
||||
SpectraBuf() = default;
|
||||
|
||||
~SpectraBuf() {
|
||||
if (video_fd >= 0 && ptr) {
|
||||
munmap(ptr, mmap_size);
|
||||
release(video_fd, handle);
|
||||
}
|
||||
}
|
||||
|
||||
void init(SpectraMaster *m, int s, int a, bool shared_access, int mmu_hdl = 0, int mmu_hdl2 = 0, int count = 1) {
|
||||
video_fd = m->video0_fd;
|
||||
size = s;
|
||||
alignment = a;
|
||||
mmap_size = aligned_size() * count;
|
||||
|
||||
uint32_t flags = CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE;
|
||||
if (shared_access) {
|
||||
flags |= CAM_MEM_FLAG_HW_SHARED_ACCESS;
|
||||
}
|
||||
|
||||
void *p = alloc_w_mmu_hdl(video_fd, mmap_size, (uint32_t*)&handle, alignment, flags, mmu_hdl, mmu_hdl2);
|
||||
ptr = (unsigned char*)p;
|
||||
assert(ptr != NULL);
|
||||
};
|
||||
|
||||
uint32_t aligned_size() {
|
||||
return ALIGNED_SIZE(size, alignment);
|
||||
};
|
||||
|
||||
int video_fd = -1;
|
||||
unsigned char *ptr = nullptr;
|
||||
int size = 0, alignment = 0, handle = 0, mmap_size = 0;
|
||||
};
|
||||
|
||||
class SpectraCamera {
|
||||
public:
|
||||
SpectraCamera(SpectraMaster *master, const CameraConfig &config);
|
||||
~SpectraCamera();
|
||||
|
||||
void camera_open(VisionIpcServer *v, cl_device_id device_id, cl_context ctx);
|
||||
bool handle_camera_event(const cam_req_mgr_message *event_data);
|
||||
void camera_close();
|
||||
void camera_map_bufs();
|
||||
void config_bps(int idx, int request_id);
|
||||
void config_ife(int idx, int request_id, bool init=false);
|
||||
|
||||
int clear_req_queue();
|
||||
void enqueue_frame(uint64_t request_id);
|
||||
|
||||
int sensors_init();
|
||||
void sensors_start();
|
||||
void sensors_poke(int request_id);
|
||||
void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
|
||||
|
||||
bool openSensor();
|
||||
void configISP();
|
||||
void configICP();
|
||||
void configCSIPHY();
|
||||
void linkDevices();
|
||||
void destroySyncObjectAt(int index);
|
||||
|
||||
// *** state ***
|
||||
|
||||
int ife_buf_depth = -1;
|
||||
bool open = false;
|
||||
bool enabled = true;
|
||||
CameraConfig cc;
|
||||
std::unique_ptr<const SensorInfo> sensor;
|
||||
|
||||
// YUV image size
|
||||
uint32_t stride;
|
||||
uint32_t y_height;
|
||||
uint32_t uv_height;
|
||||
uint32_t uv_offset;
|
||||
uint32_t yuv_size;
|
||||
|
||||
unique_fd sensor_fd;
|
||||
unique_fd csiphy_fd;
|
||||
|
||||
int32_t session_handle = -1;
|
||||
int32_t sensor_dev_handle = -1;
|
||||
int32_t isp_dev_handle = -1;
|
||||
int32_t icp_dev_handle = -1;
|
||||
int32_t csiphy_dev_handle = -1;
|
||||
|
||||
int32_t link_handle = -1;
|
||||
|
||||
SpectraBuf ife_cmd;
|
||||
SpectraBuf ife_gamma_lut;
|
||||
SpectraBuf ife_linearization_lut;
|
||||
SpectraBuf ife_vignetting_lut;
|
||||
|
||||
SpectraBuf bps_cmd;
|
||||
SpectraBuf bps_cdm_buffer;
|
||||
SpectraBuf bps_cdm_program_array;
|
||||
SpectraBuf bps_cdm_striping_bl;
|
||||
SpectraBuf bps_iq;
|
||||
SpectraBuf bps_striping;
|
||||
SpectraBuf bps_linearization_lut;
|
||||
std::vector<uint32_t> bps_lin_reg;
|
||||
std::vector<uint32_t> bps_ccm_reg;
|
||||
|
||||
int buf_handle_yuv[MAX_IFE_BUFS] = {};
|
||||
int buf_handle_raw[MAX_IFE_BUFS] = {};
|
||||
int sync_objs_ife[MAX_IFE_BUFS] = {};
|
||||
int sync_objs_bps[MAX_IFE_BUFS] = {};
|
||||
uint64_t request_id_last = 0;
|
||||
uint64_t last_requeue_ts = 0;
|
||||
uint64_t frame_id_raw_last = 0;
|
||||
int invalid_request_count = 0;
|
||||
bool skip_expected = true;
|
||||
|
||||
CameraBuf buf;
|
||||
SpectraMaster *m;
|
||||
|
||||
private:
|
||||
void clearAndRequeue(uint64_t from_request_id);
|
||||
bool validateEvent(uint64_t request_id, uint64_t frame_id_raw);
|
||||
bool waitForFrameReady(uint64_t request_id);
|
||||
bool processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp);
|
||||
static bool syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp);
|
||||
struct SyncData {
|
||||
uint64_t timestamp;
|
||||
uint64_t frame_id_offset = 0;
|
||||
};
|
||||
inline static std::map<int, SyncData> camera_sync_data;
|
||||
inline static bool first_frame_synced = false;
|
||||
|
||||
// a mode for stressing edge cases: realignment, sync failures, etc.
|
||||
inline bool stress_test(std::string log) {
|
||||
static double last_trigger = 0;
|
||||
static double prob = std::stod(util::getenv("SPECTRA_ERROR_PROB", "-1"));
|
||||
static double dt = std::stod(util::getenv("SPECTRA_ERROR_DT", "1"));
|
||||
bool triggered = (prob > 0) && \
|
||||
((static_cast<double>(rand()) / RAND_MAX) < prob) && \
|
||||
(millis_since_boot() - last_trigger) > dt;
|
||||
if (triggered) {
|
||||
last_trigger = millis_since_boot();
|
||||
LOGE("stress test (cam %d): %s", cc.camera_num, log.c_str());
|
||||
}
|
||||
return triggered;
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,34 @@
|
||||
#if SENSOR_ID == 1
|
||||
|
||||
#define VIGNETTE_PROFILE_8DT0MM
|
||||
|
||||
#define BIT_DEPTH 12
|
||||
#define PV_MAX 4096
|
||||
#define BLACK_LVL 168
|
||||
|
||||
float4 normalize_pv(int4 parsed, float vignette_factor) {
|
||||
float4 pv = (convert_float4(parsed) - BLACK_LVL) / (PV_MAX - BLACK_LVL);
|
||||
return clamp(pv*vignette_factor, 0.0, 1.0);
|
||||
}
|
||||
|
||||
float3 color_correct(float3 rgb) {
|
||||
float3 corrected = rgb.x * (float3)(1.82717181, -0.31231438, 0.07307673);
|
||||
corrected += rgb.y * (float3)(-0.5743977, 1.36858544, -0.53183455);
|
||||
corrected += rgb.z * (float3)(-0.25277411, -0.05627105, 1.45875782);
|
||||
return corrected;
|
||||
}
|
||||
|
||||
float3 apply_gamma(float3 rgb, int expo_time) {
|
||||
// tone mapping params
|
||||
const float gamma_k = 0.75;
|
||||
const float gamma_b = 0.125;
|
||||
const float mp = 0.01; // ideally midpoint should be adaptive
|
||||
const float rk = 9 - 100*mp;
|
||||
|
||||
// poly approximation for s curve
|
||||
return (rgb > mp) ?
|
||||
((rk * (rgb-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(rgb-mp))) + gamma_k*mp + gamma_b) :
|
||||
((rk * (rgb-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(rgb-mp))) + gamma_k*mp + gamma_b);
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,121 @@
|
||||
#pragma once
|
||||
|
||||
const struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}};
|
||||
const struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}};
|
||||
|
||||
const struct i2c_random_wr_payload init_array_ar0231[] = {
|
||||
{0x301A, 0x0018}, // RESET_REGISTER
|
||||
|
||||
// **NOTE**: if this is changed, readout_time_ns must be updated in the Sensor config
|
||||
|
||||
// CLOCK Settings
|
||||
// input clock is 19.2 / 2 * 0x37 = 528 MHz
|
||||
// pixclk is 528 / 6 = 88 MHz
|
||||
// full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms
|
||||
// img roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL)) = 22.85 ms
|
||||
{0x302A, 0x0006}, // VT_PIX_CLK_DIV
|
||||
{0x302C, 0x0001}, // VT_SYS_CLK_DIV
|
||||
{0x302E, 0x0002}, // PRE_PLL_CLK_DIV
|
||||
{0x3030, 0x0037}, // PLL_MULTIPLIER
|
||||
{0x3036, 0x000C}, // OP_PIX_CLK_DIV
|
||||
{0x3038, 0x0001}, // OP_SYS_CLK_DIV
|
||||
|
||||
// FORMAT
|
||||
{0x3040, 0xC000}, // READ_MODE
|
||||
{0x3004, 0x0000}, // X_ADDR_START_
|
||||
{0x3008, 0x0787}, // X_ADDR_END_
|
||||
{0x3002, 0x0000}, // Y_ADDR_START_
|
||||
{0x3006, 0x04B7}, // Y_ADDR_END_
|
||||
{0x3032, 0x0000}, // SCALING_MODE
|
||||
{0x30A2, 0x0001}, // X_ODD_INC_
|
||||
{0x30A6, 0x0001}, // Y_ODD_INC_
|
||||
{0x3402, 0x0788}, // X_OUTPUT_CONTROL
|
||||
{0x3404, 0x04B8}, // Y_OUTPUT_CONTROL
|
||||
{0x3064, 0x1982}, // SMIA_TEST
|
||||
{0x30BA, 0x11F2}, // DIGITAL_CTRL
|
||||
|
||||
// Enable external trigger and disable GPIO outputs
|
||||
{0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE
|
||||
{0x340A, 0xE0}, // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE
|
||||
{0x340C, 0x802}, // GPIO_HIDRV_EN | GPIO0_ISEL=2
|
||||
|
||||
// Readout timing
|
||||
{0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR)
|
||||
{0x300A, 0x0855}, // FRAME_LENGTH_LINES
|
||||
{0x3042, 0x0000}, // EXTRA_DELAY
|
||||
|
||||
// Readout Settings
|
||||
{0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
|
||||
{0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12
|
||||
{0x3342, 0x1212}, // MIPI_F1_PDT_EDT
|
||||
{0x3346, 0x1212}, // MIPI_F2_PDT_EDT
|
||||
{0x334A, 0x1212}, // MIPI_F3_PDT_EDT
|
||||
{0x334E, 0x1212}, // MIPI_F4_PDT_EDT
|
||||
{0x3344, 0x0011}, // MIPI_F1_VDT_VC
|
||||
{0x3348, 0x0111}, // MIPI_F2_VDT_VC
|
||||
{0x334C, 0x0211}, // MIPI_F3_VDT_VC
|
||||
{0x3350, 0x0311}, // MIPI_F4_VDT_VC
|
||||
{0x31B0, 0x0053}, // FRAME_PREAMBLE
|
||||
{0x31B2, 0x003B}, // LINE_PREAMBLE
|
||||
{0x301A, 0x001C}, // RESET_REGISTER
|
||||
|
||||
// Noise Corrections
|
||||
{0x3092, 0x0C24}, // ROW_NOISE_CONTROL
|
||||
{0x337A, 0x0C80}, // DBLC_SCALE0
|
||||
{0x3370, 0x03B1}, // DBLC
|
||||
{0x3044, 0x0400}, // DARK_CONTROL
|
||||
|
||||
// Enable temperature sensor
|
||||
{0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG
|
||||
{0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG
|
||||
|
||||
// Enable dead pixel correction using
|
||||
// the 1D line correction scheme
|
||||
{0x31E0, 0x0003},
|
||||
|
||||
// HDR Settings
|
||||
{0x3082, 0x0004}, // OPERATION_MODE_CTRL
|
||||
{0x3238, 0x0444}, // EXPOSURE_RATIO
|
||||
|
||||
{0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN
|
||||
{0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN
|
||||
{0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN
|
||||
{0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN
|
||||
|
||||
// TODO: do these have to be lower than LINE_LENGTH_PCK?
|
||||
{0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_
|
||||
{0x321E, 0x0894}, // FINE_INTEGRATION_TIME2
|
||||
|
||||
{0x31D0, 0x0000}, // COMPANDING, no good in 10 bit?
|
||||
{0x33DA, 0x0000}, // COMPANDING
|
||||
{0x318E, 0x0200}, // PRE_HDR_GAIN_EN
|
||||
|
||||
// DLO Settings
|
||||
{0x3100, 0x4000}, // DLO_CONTROL0
|
||||
{0x3280, 0x0CCC}, // T1 G1
|
||||
{0x3282, 0x0CCC}, // T1 R
|
||||
{0x3284, 0x0CCC}, // T1 B
|
||||
{0x3286, 0x0CCC}, // T1 G2
|
||||
{0x3288, 0x0FA0}, // T2 G1
|
||||
{0x328A, 0x0FA0}, // T2 R
|
||||
{0x328C, 0x0FA0}, // T2 B
|
||||
{0x328E, 0x0FA0}, // T2 G2
|
||||
|
||||
// Initial Gains
|
||||
{0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_
|
||||
{0x3366, 0xFF77}, // ANALOG_GAIN (1x)
|
||||
|
||||
{0x3060, 0x3333}, // ANALOG_COLOR_GAIN
|
||||
|
||||
{0x3362, 0x0000}, // DC GAIN
|
||||
|
||||
{0x305A, 0x00F8}, // red gain
|
||||
{0x3058, 0x0122}, // blue gain
|
||||
{0x3056, 0x009A}, // g1 gain
|
||||
{0x305C, 0x009A}, // g2 gain
|
||||
|
||||
{0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_
|
||||
|
||||
// Initial Integration Time
|
||||
{0x3012, 0x0005},
|
||||
};
|
||||
@@ -0,0 +1,58 @@
|
||||
#if SENSOR_ID == 3
|
||||
|
||||
#define BGGR
|
||||
#define VIGNETTE_PROFILE_4DT6MM
|
||||
|
||||
#define BIT_DEPTH 12
|
||||
#define PV_MAX10 1023
|
||||
#define PV_MAX12 4095
|
||||
#define PV_MAX16 65536 // gamma curve is calibrated to 16bit
|
||||
#define BLACK_LVL 48
|
||||
|
||||
float combine_dual_pvs(float lv, float sv, int expo_time) {
|
||||
float svc = fmax(sv * expo_time, (float)(64 * (PV_MAX10 - BLACK_LVL)));
|
||||
float svd = sv * fmin(expo_time, 8.0) / 8;
|
||||
|
||||
if (expo_time > 64) {
|
||||
if (lv < PV_MAX10 - BLACK_LVL) {
|
||||
return lv / (PV_MAX16 - BLACK_LVL);
|
||||
} else {
|
||||
return (svc / 64) / (PV_MAX16 - BLACK_LVL);
|
||||
}
|
||||
} else {
|
||||
if (lv > 32) {
|
||||
return (lv * 64 / fmax(expo_time, 8.0)) / (PV_MAX16 - BLACK_LVL);
|
||||
} else {
|
||||
return svd / (PV_MAX16 - BLACK_LVL);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float4 normalize_pv_hdr(int4 parsed, int4 short_parsed, float vignette_factor, int expo_time) {
|
||||
float4 pl = convert_float4(parsed - BLACK_LVL);
|
||||
float4 ps = convert_float4(short_parsed - BLACK_LVL);
|
||||
float4 pv;
|
||||
pv.s0 = combine_dual_pvs(pl.s0, ps.s0, expo_time);
|
||||
pv.s1 = combine_dual_pvs(pl.s1, ps.s1, expo_time);
|
||||
pv.s2 = combine_dual_pvs(pl.s2, ps.s2, expo_time);
|
||||
pv.s3 = combine_dual_pvs(pl.s3, ps.s3, expo_time);
|
||||
return clamp(pv*vignette_factor, 0.0, 1.0);
|
||||
}
|
||||
|
||||
float4 normalize_pv(int4 parsed, float vignette_factor) {
|
||||
float4 pv = (convert_float4(parsed) - BLACK_LVL) / (PV_MAX12 - BLACK_LVL);
|
||||
return clamp(pv*vignette_factor, 0.0, 1.0);
|
||||
}
|
||||
|
||||
float3 color_correct(float3 rgb) {
|
||||
float3 corrected = rgb.x * (float3)(1.55361989, -0.268894615, -0.000593219);
|
||||
corrected += rgb.y * (float3)(-0.421217301, 1.51883144, -0.69760146);
|
||||
corrected += rgb.z * (float3)(-0.132402589, -0.249936825, 1.69819468);
|
||||
return corrected;
|
||||
}
|
||||
|
||||
float3 apply_gamma(float3 rgb, int expo_time) {
|
||||
return (10 * rgb) / (1 + 9 * rgb);
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,334 @@
|
||||
#pragma once
|
||||
|
||||
const struct i2c_random_wr_payload start_reg_array_os04c10[] = {{0x100, 1}};
|
||||
const struct i2c_random_wr_payload stop_reg_array_os04c10[] = {{0x100, 0}};
|
||||
|
||||
const struct i2c_random_wr_payload init_array_os04c10[] = {
|
||||
// DP_2688X1520_NEWSTG_MIPI0776Mbps_30FPS_10BIT_FOURLANE
|
||||
{0x0103, 0x01},
|
||||
|
||||
// PLL
|
||||
{0x0301, 0xe4},
|
||||
{0x0303, 0x01},
|
||||
{0x0305, 0xb6},
|
||||
{0x0306, 0x01},
|
||||
{0x0307, 0x17},
|
||||
{0x0323, 0x04},
|
||||
{0x0324, 0x01},
|
||||
{0x0325, 0x62},
|
||||
|
||||
{0x3012, 0x06},
|
||||
{0x3013, 0x02},
|
||||
{0x3016, 0x72},
|
||||
{0x3021, 0x03},
|
||||
{0x3106, 0x21},
|
||||
{0x3107, 0xa1},
|
||||
|
||||
// ?
|
||||
{0x3624, 0x00},
|
||||
{0x3625, 0x4c},
|
||||
{0x3660, 0x04},
|
||||
{0x3666, 0xa5},
|
||||
{0x3667, 0xa5},
|
||||
{0x366a, 0x50},
|
||||
{0x3673, 0x0d},
|
||||
{0x3672, 0x0d},
|
||||
{0x3671, 0x0d},
|
||||
{0x3670, 0x0d},
|
||||
{0x3685, 0x00},
|
||||
{0x3694, 0x0d},
|
||||
{0x3693, 0x0d},
|
||||
{0x3692, 0x0d},
|
||||
{0x3691, 0x0d},
|
||||
{0x3696, 0x4c},
|
||||
{0x3697, 0x4c},
|
||||
{0x3698, 0x00},
|
||||
{0x3699, 0x80},
|
||||
{0x369a, 0x80},
|
||||
{0x369b, 0x1f},
|
||||
{0x369c, 0x1f},
|
||||
{0x369d, 0x80},
|
||||
{0x369e, 0x40},
|
||||
{0x369f, 0x21},
|
||||
{0x36a0, 0x12},
|
||||
{0x36a1, 0xdd},
|
||||
{0x36a2, 0x66},
|
||||
{0x370a, 0x02},
|
||||
{0x370e, 0x00},
|
||||
{0x3710, 0x00},
|
||||
{0x3713, 0x04},
|
||||
{0x3725, 0x02},
|
||||
{0x372a, 0x03},
|
||||
{0x3738, 0xce},
|
||||
{0x3748, 0x02},
|
||||
{0x374a, 0x02},
|
||||
{0x374c, 0x02},
|
||||
{0x374e, 0x02},
|
||||
{0x3756, 0x00},
|
||||
{0x3757, 0x00},
|
||||
{0x3767, 0x00},
|
||||
{0x3771, 0x00},
|
||||
{0x377b, 0x28},
|
||||
{0x377c, 0x00},
|
||||
{0x377d, 0x0c},
|
||||
{0x3781, 0x03},
|
||||
{0x3782, 0x00},
|
||||
{0x3789, 0x14},
|
||||
{0x3795, 0x02},
|
||||
{0x379c, 0x00},
|
||||
{0x379d, 0x00},
|
||||
{0x37b8, 0x04},
|
||||
{0x37ba, 0x03},
|
||||
{0x37bb, 0x00},
|
||||
{0x37bc, 0x04},
|
||||
{0x37be, 0x26},
|
||||
{0x37c4, 0x11},
|
||||
{0x37c5, 0x80},
|
||||
{0x37c6, 0x14},
|
||||
{0x37c7, 0xa8},
|
||||
{0x37da, 0x11},
|
||||
{0x381f, 0x08},
|
||||
// {0x3829, 0x03},
|
||||
// {0x3832, 0x00},
|
||||
{0x3881, 0x00},
|
||||
{0x3888, 0x04},
|
||||
{0x388b, 0x00},
|
||||
{0x3c80, 0x10},
|
||||
{0x3c86, 0x00},
|
||||
{0x3c8c, 0x20},
|
||||
{0x3c9f, 0x01},
|
||||
{0x3d85, 0x1b},
|
||||
{0x3d8c, 0x71},
|
||||
{0x3d8d, 0xe2},
|
||||
{0x3f00, 0x0b},
|
||||
{0x3f06, 0x04},
|
||||
|
||||
// BLC
|
||||
{0x400a, 0x01},
|
||||
{0x400b, 0x50},
|
||||
{0x400e, 0x08},
|
||||
{0x4043, 0x7e},
|
||||
{0x4045, 0x7e},
|
||||
{0x4047, 0x7e},
|
||||
{0x4049, 0x7e},
|
||||
{0x4090, 0x04},
|
||||
{0x40b0, 0x00},
|
||||
{0x40b1, 0x00},
|
||||
{0x40b2, 0x00},
|
||||
{0x40b3, 0x00},
|
||||
{0x40b4, 0x00},
|
||||
{0x40b5, 0x00},
|
||||
{0x40b7, 0x00},
|
||||
{0x40b8, 0x00},
|
||||
{0x40b9, 0x00},
|
||||
{0x40ba, 0x01},
|
||||
|
||||
{0x4301, 0x00},
|
||||
{0x4303, 0x00},
|
||||
{0x4502, 0x04},
|
||||
{0x4503, 0x00},
|
||||
{0x4504, 0x06},
|
||||
{0x4506, 0x00},
|
||||
{0x4507, 0x47},
|
||||
{0x4803, 0x00},
|
||||
{0x480c, 0x32},
|
||||
{0x480e, 0x04},
|
||||
{0x4813, 0xe4},
|
||||
{0x4819, 0x70},
|
||||
{0x481f, 0x30},
|
||||
{0x4823, 0x3f},
|
||||
{0x4825, 0x30},
|
||||
{0x4833, 0x10},
|
||||
{0x484b, 0x27},
|
||||
{0x488b, 0x00},
|
||||
{0x4d00, 0x04},
|
||||
{0x4d01, 0xad},
|
||||
{0x4d02, 0xbc},
|
||||
{0x4d03, 0xa1},
|
||||
{0x4d04, 0x1f},
|
||||
{0x4d05, 0x4c},
|
||||
{0x4d0b, 0x01},
|
||||
{0x4e00, 0x2a},
|
||||
{0x4e0d, 0x00},
|
||||
|
||||
// ISP
|
||||
{0x5001, 0x09},
|
||||
{0x5004, 0x00},
|
||||
{0x5080, 0x04},
|
||||
{0x5036, 0x80},
|
||||
{0x5180, 0x70},
|
||||
{0x5181, 0x10},
|
||||
|
||||
// DPC
|
||||
{0x520a, 0x03},
|
||||
{0x520b, 0x06},
|
||||
{0x520c, 0x0c},
|
||||
|
||||
{0x580b, 0x0f},
|
||||
{0x580d, 0x00},
|
||||
{0x580f, 0x00},
|
||||
{0x5820, 0x00},
|
||||
{0x5821, 0x00},
|
||||
|
||||
{0x301c, 0xf8},
|
||||
{0x301e, 0xb4},
|
||||
{0x301f, 0xf0},
|
||||
{0x3022, 0x61},
|
||||
{0x3109, 0xe7},
|
||||
{0x3600, 0x00},
|
||||
{0x3610, 0x65},
|
||||
{0x3611, 0x85},
|
||||
{0x3613, 0x3a},
|
||||
{0x3615, 0x60},
|
||||
{0x3621, 0xb0},
|
||||
{0x3620, 0x0c},
|
||||
{0x3629, 0x00},
|
||||
{0x3661, 0x04},
|
||||
{0x3664, 0x70},
|
||||
{0x3665, 0x00},
|
||||
{0x3681, 0x80},
|
||||
{0x3682, 0x40},
|
||||
{0x3683, 0x21},
|
||||
{0x3684, 0x12},
|
||||
{0x3700, 0x2a},
|
||||
{0x3701, 0x12},
|
||||
{0x3703, 0x28},
|
||||
{0x3704, 0x0e},
|
||||
{0x3706, 0x9d},
|
||||
{0x3709, 0x4a},
|
||||
{0x370b, 0x48},
|
||||
{0x370c, 0x01},
|
||||
{0x370f, 0x00},
|
||||
{0x3714, 0x28},
|
||||
{0x3716, 0x04},
|
||||
{0x3719, 0x11},
|
||||
{0x371a, 0x1e},
|
||||
{0x3720, 0x00},
|
||||
{0x3724, 0x13},
|
||||
{0x373f, 0xb0},
|
||||
{0x3741, 0x9d},
|
||||
{0x3743, 0x9d},
|
||||
{0x3745, 0x9d},
|
||||
{0x3747, 0x9d},
|
||||
{0x3749, 0x48},
|
||||
{0x374b, 0x48},
|
||||
{0x374d, 0x48},
|
||||
{0x374f, 0x48},
|
||||
{0x3755, 0x10},
|
||||
{0x376c, 0x00},
|
||||
{0x378d, 0x3c},
|
||||
{0x3790, 0x01},
|
||||
{0x3791, 0x01},
|
||||
{0x3798, 0x40},
|
||||
{0x379e, 0x00},
|
||||
{0x379f, 0x04},
|
||||
{0x37a1, 0x10},
|
||||
{0x37a2, 0x1e},
|
||||
{0x37a8, 0x10},
|
||||
{0x37a9, 0x1e},
|
||||
{0x37ac, 0xa0},
|
||||
{0x37b9, 0x01},
|
||||
{0x37bd, 0x01},
|
||||
{0x37bf, 0x26},
|
||||
{0x37c0, 0x11},
|
||||
{0x37c2, 0x14},
|
||||
{0x37cd, 0x19},
|
||||
{0x37e0, 0x08},
|
||||
{0x37e6, 0x04},
|
||||
{0x37e5, 0x02},
|
||||
{0x37e1, 0x0c},
|
||||
{0x3737, 0x04},
|
||||
{0x37d8, 0x02},
|
||||
{0x37e2, 0x10},
|
||||
{0x3739, 0x10},
|
||||
{0x3662, 0x08},
|
||||
{0x37e4, 0x20},
|
||||
{0x37e3, 0x08},
|
||||
{0x37d9, 0x04},
|
||||
{0x4040, 0x00},
|
||||
{0x4041, 0x03},
|
||||
{0x4008, 0x01},
|
||||
{0x4009, 0x06},
|
||||
|
||||
// FSIN
|
||||
{0x3002, 0x22},
|
||||
{0x3663, 0x22},
|
||||
{0x368a, 0x04},
|
||||
{0x3822, 0x44},
|
||||
{0x3823, 0x00},
|
||||
{0x3829, 0x03},
|
||||
{0x3832, 0xf8},
|
||||
{0x382c, 0x00},
|
||||
{0x3844, 0x06},
|
||||
{0x3843, 0x00},
|
||||
{0x382a, 0x00},
|
||||
{0x382b, 0x0c},
|
||||
|
||||
// 2704x1536 -> 2688x1520 out
|
||||
{0x3800, 0x00}, {0x3801, 0x00},
|
||||
{0x3802, 0x00}, {0x3803, 0x00},
|
||||
{0x3804, 0x0a}, {0x3805, 0x8f},
|
||||
{0x3806, 0x05}, {0x3807, 0xff},
|
||||
{0x3808, 0x05}, {0x3809, 0x40},
|
||||
{0x380a, 0x02}, {0x380b, 0xf8},
|
||||
{0x3811, 0x08},
|
||||
{0x3813, 0x08},
|
||||
{0x3814, 0x03},
|
||||
{0x3815, 0x01},
|
||||
{0x3816, 0x03},
|
||||
{0x3817, 0x01},
|
||||
|
||||
{0x380c, 0x0b}, {0x380d, 0xac}, // HTS
|
||||
{0x380e, 0x06}, {0x380f, 0x9c}, // VTS
|
||||
|
||||
{0x3820, 0xb3},
|
||||
{0x3821, 0x01},
|
||||
{0x3880, 0x00},
|
||||
{0x3882, 0x20},
|
||||
{0x3c91, 0x0b},
|
||||
{0x3c94, 0x45},
|
||||
{0x3cad, 0x00},
|
||||
{0x3cae, 0x00},
|
||||
{0x4000, 0xf3},
|
||||
{0x4001, 0x60},
|
||||
{0x4003, 0x40},
|
||||
{0x4300, 0xff},
|
||||
{0x4302, 0x0f},
|
||||
{0x4305, 0x83},
|
||||
{0x4505, 0x84},
|
||||
{0x4809, 0x0e},
|
||||
{0x480a, 0x04},
|
||||
{0x4837, 0x15},
|
||||
{0x4c00, 0x08},
|
||||
{0x4c01, 0x08},
|
||||
{0x4c04, 0x00},
|
||||
{0x4c05, 0x00},
|
||||
{0x5000, 0xf9},
|
||||
// {0x0100, 0x01},
|
||||
// {0x320d, 0x00},
|
||||
// {0x3208, 0xa0},
|
||||
|
||||
// initialize exposure
|
||||
{0x3503, 0x88},
|
||||
|
||||
// long
|
||||
{0x3500, 0x00}, {0x3501, 0x00}, {0x3502, 0x10},
|
||||
{0x3508, 0x00}, {0x3509, 0x80},
|
||||
{0x350a, 0x04}, {0x350b, 0x00},
|
||||
|
||||
// short
|
||||
{0x3510, 0x00}, {0x3511, 0x00}, {0x3512, 0x40},
|
||||
{0x350c, 0x00}, {0x350d, 0x80},
|
||||
{0x350e, 0x04}, {0x350f, 0x00},
|
||||
|
||||
// wb
|
||||
// b
|
||||
{0x5100, 0x06}, {0x5101, 0x7e},
|
||||
{0x5140, 0x06}, {0x5141, 0x7e},
|
||||
// g
|
||||
{0x5102, 0x04}, {0x5103, 0x00},
|
||||
{0x5142, 0x04}, {0x5143, 0x00},
|
||||
// r
|
||||
{0x5104, 0x08}, {0x5105, 0xd6},
|
||||
{0x5144, 0x08}, {0x5145, 0xd6},
|
||||
};
|
||||
@@ -0,0 +1,47 @@
|
||||
#if SENSOR_ID == 2
|
||||
|
||||
#define VIGNETTE_PROFILE_8DT0MM
|
||||
|
||||
#define BIT_DEPTH 12
|
||||
#define BLACK_LVL 64
|
||||
|
||||
float ox_lut_func(int x) {
|
||||
if (x < 512) {
|
||||
return x * 5.94873e-8;
|
||||
} else if (512 <= x && x < 768) {
|
||||
return 3.0458e-05 + (x-512) * 1.19913e-7;
|
||||
} else if (768 <= x && x < 1536) {
|
||||
return 6.1154e-05 + (x-768) * 2.38493e-7;
|
||||
} else if (1536 <= x && x < 1792) {
|
||||
return 0.0002448 + (x-1536) * 9.56930e-7;
|
||||
} else if (1792 <= x && x < 2048) {
|
||||
return 0.00048977 + (x-1792) * 1.91441e-6;
|
||||
} else if (2048 <= x && x < 2304) {
|
||||
return 0.00097984 + (x-2048) * 3.82937e-6;
|
||||
} else if (2304 <= x && x < 2560) {
|
||||
return 0.0019601 + (x-2304) * 7.659055e-6;
|
||||
} else if (2560 <= x && x < 2816) {
|
||||
return 0.0039207 + (x-2560) * 1.525e-5;
|
||||
} else {
|
||||
return 0.0078421 + (exp((x-2816)/273.0) - 1) * 0.0092421;
|
||||
}
|
||||
}
|
||||
|
||||
float4 normalize_pv(int4 parsed, float vignette_factor) {
|
||||
// PWL
|
||||
float4 pv = {ox_lut_func(parsed.s0), ox_lut_func(parsed.s1), ox_lut_func(parsed.s2), ox_lut_func(parsed.s3)};
|
||||
return clamp(pv*vignette_factor*256.0, 0.0, 1.0);
|
||||
}
|
||||
|
||||
float3 color_correct(float3 rgb) {
|
||||
float3 corrected = rgb.x * (float3)(1.5664815, -0.29808738, -0.03973474);
|
||||
corrected += rgb.y * (float3)(-0.48672447, 1.41914433, -0.40295248);
|
||||
corrected += rgb.z * (float3)(-0.07975703, -0.12105695, 1.44268722);
|
||||
return corrected;
|
||||
}
|
||||
|
||||
float3 apply_gamma(float3 rgb, int expo_time) {
|
||||
return -0.507089*exp(-12.54124638*rgb) + 0.9655*powr(rgb, 0.5) - 0.472597*rgb + 0.507089;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,751 @@
|
||||
#pragma once
|
||||
|
||||
const struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}};
|
||||
const struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}};
|
||||
|
||||
const struct i2c_random_wr_payload init_array_ox03c10[] = {
|
||||
{0x103, 1},
|
||||
{0x107, 1},
|
||||
|
||||
// X3C_1920x1280_60fps_HDR4_LFR_PWL12_mipi1200
|
||||
|
||||
// TPM
|
||||
{0x4d5a, 0x1a}, {0x4d09, 0xff}, {0x4d09, 0xdf},
|
||||
|
||||
/*)
|
||||
// group 4
|
||||
{0x3208, 0x04},
|
||||
{0x4620, 0x04},
|
||||
{0x3208, 0x14},
|
||||
|
||||
// group 5
|
||||
{0x3208, 0x05},
|
||||
{0x4620, 0x04},
|
||||
{0x3208, 0x15},
|
||||
|
||||
// group 2
|
||||
{0x3208, 0x02},
|
||||
{0x3507, 0x00},
|
||||
{0x3208, 0x12},
|
||||
|
||||
// delay launch group 2
|
||||
{0x3208, 0xa2},*/
|
||||
|
||||
// **NOTE**: if this is changed, readout_time_ns must be updated in the Sensor config
|
||||
// PLL setup
|
||||
{0x0301, 0xc8}, // pll1_divs, pll1_predivp, pll1_divpix
|
||||
{0x0303, 0x01}, // pll1_prediv
|
||||
{0x0304, 0x01}, {0x0305, 0x2c}, // pll1_loopdiv = 300
|
||||
{0x0306, 0x04}, // pll1_divmipi = 4
|
||||
{0x0307, 0x01}, // pll1_divm = 1
|
||||
{0x0316, 0x00},
|
||||
{0x0317, 0x00},
|
||||
{0x0318, 0x00},
|
||||
{0x0323, 0x05}, // pll2_prediv
|
||||
{0x0324, 0x01}, {0x0325, 0x2c}, // pll2_divp = 300
|
||||
|
||||
// SCLK/PCLK
|
||||
{0x0400, 0xe0}, {0x0401, 0x80},
|
||||
{0x0403, 0xde}, {0x0404, 0x34},
|
||||
{0x0405, 0x3b}, {0x0406, 0xde},
|
||||
{0x0407, 0x08},
|
||||
{0x0408, 0xe0}, {0x0409, 0x7f},
|
||||
{0x040a, 0xde}, {0x040b, 0x34},
|
||||
{0x040c, 0x47}, {0x040d, 0xd8},
|
||||
{0x040e, 0x08},
|
||||
|
||||
// xchk
|
||||
{0x2803, 0xfe}, {0x280b, 0x00}, {0x280c, 0x79},
|
||||
|
||||
// SC ctrl
|
||||
{0x3001, 0x03}, // io_pad_oen
|
||||
{0x3002, 0xfc}, // io_pad_oen
|
||||
{0x3005, 0x80}, // io_pad_out
|
||||
{0x3007, 0x01}, // io_pad_sel
|
||||
{0x3008, 0x80}, // io_pad_sel
|
||||
|
||||
// FSIN (frame sync) with external pulses
|
||||
{0x3009, 0x2},
|
||||
{0x3015, 0x2},
|
||||
{0x383E, 0x80},
|
||||
{0x3881, 0x4},
|
||||
{0x3882, 0x8}, {0x3883, 0x0D},
|
||||
{0x3836, 0x1F}, {0x3837, 0x40},
|
||||
|
||||
// causes issues on some devices
|
||||
//{0x3822, 0x33}, // wait for pulse before first frame
|
||||
|
||||
{0x3892, 0x44},
|
||||
{0x3823, 0x41},
|
||||
|
||||
{0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI
|
||||
{0x3020, 0x05}, // SC_CTRL_20
|
||||
|
||||
// this is not in the datasheet, listed as RSVD
|
||||
// but the camera doesn't work without it
|
||||
{0x3700, 0x28}, {0x3701, 0x15}, {0x3702, 0x19}, {0x3703, 0x23},
|
||||
{0x3704, 0x0a}, {0x3705, 0x00}, {0x3706, 0x3e}, {0x3707, 0x0d},
|
||||
{0x3708, 0x50}, {0x3709, 0x5a}, {0x370a, 0x00}, {0x370b, 0x96},
|
||||
{0x3711, 0x11}, {0x3712, 0x13}, {0x3717, 0x02}, {0x3718, 0x73},
|
||||
{0x372c, 0x40}, {0x3733, 0x01}, {0x3738, 0x36}, {0x3739, 0x36},
|
||||
{0x373a, 0x25}, {0x373b, 0x25}, {0x373f, 0x21}, {0x3740, 0x21},
|
||||
{0x3741, 0x21}, {0x3742, 0x21}, {0x3747, 0x28}, {0x3748, 0x28},
|
||||
{0x3749, 0x19}, {0x3755, 0x1a}, {0x3756, 0x0a}, {0x3757, 0x1c},
|
||||
{0x3765, 0x19}, {0x3766, 0x05}, {0x3767, 0x05}, {0x3768, 0x13},
|
||||
{0x376c, 0x07}, {0x3778, 0x20}, {0x377c, 0xc8}, {0x3781, 0x02},
|
||||
{0x3783, 0x02}, {0x379c, 0x58}, {0x379e, 0x00}, {0x379f, 0x00},
|
||||
{0x37a0, 0x00}, {0x37bc, 0x22}, {0x37c0, 0x01}, {0x37c4, 0x3e},
|
||||
{0x37c5, 0x3e}, {0x37c6, 0x2a}, {0x37c7, 0x28}, {0x37c8, 0x02},
|
||||
{0x37c9, 0x12}, {0x37cb, 0x29}, {0x37cd, 0x29}, {0x37d2, 0x00},
|
||||
{0x37d3, 0x73}, {0x37d6, 0x00}, {0x37d7, 0x6b}, {0x37dc, 0x00},
|
||||
{0x37df, 0x54}, {0x37e2, 0x00}, {0x37e3, 0x00}, {0x37f8, 0x00},
|
||||
{0x37f9, 0x01}, {0x37fa, 0x00}, {0x37fb, 0x19},
|
||||
|
||||
// also RSVD
|
||||
{0x3c03, 0x01}, {0x3c04, 0x01}, {0x3c06, 0x21}, {0x3c08, 0x01},
|
||||
{0x3c09, 0x01}, {0x3c0a, 0x01}, {0x3c0b, 0x21}, {0x3c13, 0x21},
|
||||
{0x3c14, 0x82}, {0x3c16, 0x13}, {0x3c21, 0x00}, {0x3c22, 0xf3},
|
||||
{0x3c37, 0x12}, {0x3c38, 0x31}, {0x3c3c, 0x00}, {0x3c3d, 0x03},
|
||||
{0x3c44, 0x16}, {0x3c5c, 0x8a}, {0x3c5f, 0x03}, {0x3c61, 0x80},
|
||||
{0x3c6f, 0x2b}, {0x3c70, 0x5f}, {0x3c71, 0x2c}, {0x3c72, 0x2c},
|
||||
{0x3c73, 0x2c}, {0x3c76, 0x12},
|
||||
|
||||
// PEC checks
|
||||
{0x3182, 0x12},
|
||||
|
||||
{0x320e, 0x00}, {0x320f, 0x00}, // RSVD
|
||||
{0x3211, 0x61},
|
||||
{0x3215, 0xcd},
|
||||
{0x3219, 0x08},
|
||||
|
||||
{0x3506, 0x20}, {0x3507, 0x00}, // hcg fine exposure
|
||||
{0x350a, 0x01}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain
|
||||
|
||||
{0x3586, 0x40}, {0x3587, 0x00}, // lcg fine exposure
|
||||
{0x358a, 0x01}, {0x358b, 0x00}, {0x358c, 0x00}, // lcg digital gain
|
||||
|
||||
{0x3546, 0x20}, {0x3547, 0x00}, // spd fine exposure
|
||||
{0x354a, 0x01}, {0x354b, 0x00}, {0x354c, 0x00}, // spd digital gain
|
||||
|
||||
{0x35c6, 0xb0}, {0x35c7, 0x00}, // vs fine exposure
|
||||
{0x35ca, 0x01}, {0x35cb, 0x00}, {0x35cc, 0x00}, // vs digital gain
|
||||
|
||||
// also RSVD
|
||||
{0x3600, 0x8f}, {0x3605, 0x16}, {0x3609, 0xf0}, {0x360a, 0x01},
|
||||
{0x360e, 0x1d}, {0x360f, 0x10}, {0x3610, 0x70}, {0x3611, 0x3a},
|
||||
{0x3612, 0x28}, {0x361a, 0x29}, {0x361b, 0x6c}, {0x361c, 0x0b},
|
||||
{0x361d, 0x00}, {0x361e, 0xfc}, {0x362a, 0x00}, {0x364d, 0x0f},
|
||||
{0x364e, 0x18}, {0x364f, 0x12}, {0x3653, 0x1c}, {0x3654, 0x00},
|
||||
{0x3655, 0x1f}, {0x3656, 0x1f}, {0x3657, 0x0c}, {0x3658, 0x0a},
|
||||
{0x3659, 0x14}, {0x365a, 0x18}, {0x365b, 0x14}, {0x365c, 0x10},
|
||||
{0x365e, 0x12}, {0x3674, 0x08}, {0x3677, 0x3a}, {0x3678, 0x3a},
|
||||
{0x3679, 0x19},
|
||||
|
||||
// Y_ADDR_START = 4
|
||||
{0x3802, 0x00}, {0x3803, 0x04},
|
||||
// Y_ADDR_END = 0x50b
|
||||
{0x3806, 0x05}, {0x3807, 0x0b},
|
||||
|
||||
// X_OUTPUT_SIZE = 0x780 = 1920 (changed to 1928)
|
||||
{0x3808, 0x07}, {0x3809, 0x88},
|
||||
|
||||
// Y_OUTPUT_SIZE = 0x500 = 1280 (changed to 1208)
|
||||
{0x380a, 0x04}, {0x380b, 0xb8},
|
||||
|
||||
// horizontal timing 0x447
|
||||
{0x380c, 0x04}, {0x380d, 0x47},
|
||||
|
||||
// rows per frame (was 0x2ae)
|
||||
// 0x8ae = 53.65 ms
|
||||
{0x380e, 0x08}, {0x380f, 0x15},
|
||||
// this should be triggered by FSIN, not free running
|
||||
|
||||
{0x3810, 0x00}, {0x3811, 0x08}, // x cutoff
|
||||
{0x3812, 0x00}, {0x3813, 0x04}, // y cutoff
|
||||
{0x3816, 0x01},
|
||||
{0x3817, 0x01},
|
||||
{0x381c, 0x18},
|
||||
{0x381e, 0x01},
|
||||
{0x381f, 0x01},
|
||||
|
||||
// don't mirror, just flip
|
||||
{0x3820, 0x04},
|
||||
|
||||
{0x3821, 0x19},
|
||||
{0x3832, 0xF0},
|
||||
{0x3834, 0xF0},
|
||||
{0x384c, 0x02},
|
||||
{0x384d, 0x0d},
|
||||
{0x3850, 0x00},
|
||||
{0x3851, 0x42},
|
||||
{0x3852, 0x00},
|
||||
{0x3853, 0x40},
|
||||
{0x3858, 0x04},
|
||||
{0x388c, 0x02},
|
||||
{0x388d, 0x2b},
|
||||
|
||||
// APC
|
||||
{0x3b40, 0x05}, {0x3b41, 0x40}, {0x3b42, 0x00}, {0x3b43, 0x90},
|
||||
{0x3b44, 0x00}, {0x3b45, 0x20}, {0x3b46, 0x00}, {0x3b47, 0x20},
|
||||
{0x3b48, 0x19}, {0x3b49, 0x12}, {0x3b4a, 0x16}, {0x3b4b, 0x2e},
|
||||
{0x3b4c, 0x00}, {0x3b4d, 0x00},
|
||||
{0x3b86, 0x00}, {0x3b87, 0x34}, {0x3b88, 0x00}, {0x3b89, 0x08},
|
||||
{0x3b8a, 0x05}, {0x3b8b, 0x00}, {0x3b8c, 0x07}, {0x3b8d, 0x80},
|
||||
{0x3b8e, 0x00}, {0x3b8f, 0x00}, {0x3b92, 0x05}, {0x3b93, 0x00},
|
||||
{0x3b94, 0x07}, {0x3b95, 0x80}, {0x3b9e, 0x09},
|
||||
|
||||
// OTP
|
||||
{0x3d82, 0x73},
|
||||
{0x3d85, 0x05},
|
||||
{0x3d8a, 0x03},
|
||||
{0x3d8b, 0xff},
|
||||
{0x3d99, 0x00},
|
||||
{0x3d9a, 0x9f},
|
||||
{0x3d9b, 0x00},
|
||||
{0x3d9c, 0xa0},
|
||||
{0x3da4, 0x00},
|
||||
{0x3da7, 0x50},
|
||||
|
||||
// DTR
|
||||
{0x420e, 0x6b},
|
||||
{0x420f, 0x6e},
|
||||
{0x4210, 0x06},
|
||||
{0x4211, 0xc1},
|
||||
{0x421e, 0x02},
|
||||
{0x421f, 0x45},
|
||||
{0x4220, 0xe1},
|
||||
{0x4221, 0x01},
|
||||
{0x4301, 0xff},
|
||||
{0x4307, 0x03},
|
||||
{0x4308, 0x13},
|
||||
{0x430a, 0x13},
|
||||
{0x430d, 0x93},
|
||||
{0x430f, 0x57},
|
||||
{0x4310, 0x95},
|
||||
{0x4311, 0x16},
|
||||
{0x4316, 0x00},
|
||||
|
||||
{0x4317, 0x38}, // both embedded rows are enabled
|
||||
|
||||
{0x4319, 0x03}, // spd dcg
|
||||
{0x431a, 0x00}, // 8 bit mipi
|
||||
{0x431b, 0x00},
|
||||
{0x431d, 0x2a},
|
||||
{0x431e, 0x11},
|
||||
|
||||
{0x431f, 0x20}, // enable PWL (pwl0_en), 12 bits
|
||||
//{0x431f, 0x00}, // disable PWL
|
||||
|
||||
{0x4320, 0x19},
|
||||
{0x4323, 0x80},
|
||||
{0x4324, 0x00},
|
||||
{0x4503, 0x4e},
|
||||
{0x4505, 0x00},
|
||||
{0x4509, 0x00},
|
||||
{0x450a, 0x00},
|
||||
{0x4580, 0xf8},
|
||||
{0x4583, 0x07},
|
||||
{0x4584, 0x6a},
|
||||
{0x4585, 0x08},
|
||||
{0x4586, 0x05},
|
||||
{0x4587, 0x04},
|
||||
{0x4588, 0x73},
|
||||
{0x4589, 0x05},
|
||||
{0x458a, 0x1f},
|
||||
{0x458b, 0x02},
|
||||
{0x458c, 0xdc},
|
||||
{0x458d, 0x03},
|
||||
{0x458e, 0x02},
|
||||
{0x4597, 0x07},
|
||||
{0x4598, 0x40},
|
||||
{0x4599, 0x0e},
|
||||
{0x459a, 0x0e},
|
||||
{0x459b, 0xfb},
|
||||
{0x459c, 0xf3},
|
||||
{0x4602, 0x00},
|
||||
{0x4603, 0x13},
|
||||
{0x4604, 0x00},
|
||||
{0x4609, 0x0a},
|
||||
{0x460a, 0x30},
|
||||
{0x4610, 0x00},
|
||||
{0x4611, 0x70},
|
||||
{0x4612, 0x01},
|
||||
{0x4613, 0x00},
|
||||
{0x4614, 0x00},
|
||||
{0x4615, 0x70},
|
||||
{0x4616, 0x01},
|
||||
{0x4617, 0x00},
|
||||
|
||||
{0x4800, 0x04}, // invert output PCLK
|
||||
{0x480a, 0x22},
|
||||
{0x4813, 0xe4},
|
||||
|
||||
// mipi
|
||||
{0x4814, 0x2a},
|
||||
{0x4837, 0x0d},
|
||||
{0x484b, 0x47},
|
||||
{0x484f, 0x00},
|
||||
{0x4887, 0x51},
|
||||
{0x4d00, 0x4a},
|
||||
{0x4d01, 0x18},
|
||||
{0x4d05, 0xff},
|
||||
{0x4d06, 0x88},
|
||||
{0x4d08, 0x63},
|
||||
{0x4d09, 0xdf},
|
||||
{0x4d15, 0x7d},
|
||||
{0x4d1a, 0x20},
|
||||
{0x4d30, 0x0a},
|
||||
{0x4d31, 0x00},
|
||||
{0x4d34, 0x7d},
|
||||
{0x4d3c, 0x7d},
|
||||
{0x4f00, 0x00},
|
||||
{0x4f01, 0x00},
|
||||
{0x4f02, 0x00},
|
||||
{0x4f03, 0x20},
|
||||
{0x4f04, 0xe0},
|
||||
{0x6a00, 0x00},
|
||||
{0x6a01, 0x20},
|
||||
{0x6a02, 0x00},
|
||||
{0x6a03, 0x20},
|
||||
{0x6a04, 0x02},
|
||||
{0x6a05, 0x80},
|
||||
{0x6a06, 0x01},
|
||||
{0x6a07, 0xe0},
|
||||
{0x6a08, 0xcf},
|
||||
{0x6a09, 0x01},
|
||||
{0x6a0a, 0x40},
|
||||
{0x6a20, 0x00},
|
||||
{0x6a21, 0x02},
|
||||
{0x6a22, 0x00},
|
||||
{0x6a23, 0x00},
|
||||
{0x6a24, 0x00},
|
||||
{0x6a25, 0x00},
|
||||
{0x6a26, 0x00},
|
||||
{0x6a27, 0x00},
|
||||
{0x6a28, 0x00},
|
||||
|
||||
// isp
|
||||
{0x5000, 0x8f},
|
||||
{0x5001, 0x75},
|
||||
{0x5002, 0x7f}, // PWL0
|
||||
//{0x5002, 0x3f}, // PWL disable
|
||||
{0x5003, 0x7a},
|
||||
|
||||
{0x5004, 0x3e},
|
||||
{0x5005, 0x1e},
|
||||
{0x5006, 0x1e},
|
||||
{0x5007, 0x1e},
|
||||
|
||||
{0x5008, 0x00},
|
||||
{0x500c, 0x00},
|
||||
{0x502c, 0x00},
|
||||
{0x502e, 0x00},
|
||||
{0x502f, 0x00},
|
||||
{0x504b, 0x00},
|
||||
{0x5053, 0x00},
|
||||
{0x505b, 0x00},
|
||||
{0x5063, 0x00},
|
||||
{0x5070, 0x00},
|
||||
{0x5074, 0x04},
|
||||
{0x507a, 0x04},
|
||||
{0x507b, 0x09},
|
||||
{0x5500, 0x02},
|
||||
{0x5700, 0x02},
|
||||
{0x5900, 0x02},
|
||||
{0x6007, 0x04},
|
||||
{0x6008, 0x05},
|
||||
{0x6009, 0x02},
|
||||
{0x600b, 0x08},
|
||||
{0x600c, 0x07},
|
||||
{0x600d, 0x88},
|
||||
{0x6016, 0x00},
|
||||
{0x6027, 0x04},
|
||||
{0x6028, 0x05},
|
||||
{0x6029, 0x02},
|
||||
{0x602b, 0x08},
|
||||
{0x602c, 0x07},
|
||||
{0x602d, 0x88},
|
||||
{0x6047, 0x04},
|
||||
{0x6048, 0x05},
|
||||
{0x6049, 0x02},
|
||||
{0x604b, 0x08},
|
||||
{0x604c, 0x07},
|
||||
{0x604d, 0x88},
|
||||
{0x6067, 0x04},
|
||||
{0x6068, 0x05},
|
||||
{0x6069, 0x02},
|
||||
{0x606b, 0x08},
|
||||
{0x606c, 0x07},
|
||||
{0x606d, 0x88},
|
||||
{0x6087, 0x04},
|
||||
{0x6088, 0x05},
|
||||
{0x6089, 0x02},
|
||||
{0x608b, 0x08},
|
||||
{0x608c, 0x07},
|
||||
{0x608d, 0x88},
|
||||
|
||||
// 12-bit PWL0
|
||||
{0x5e00, 0x00},
|
||||
|
||||
// m_ndX_exp[0:32]
|
||||
// 9*2+0xa*3+0xb*2+0xc*2+0xd*2+0xe*2+0xf*2+0x10*2+0x11*2+0x12*4+0x13*3+0x14*3+0x15*3+0x16 = 518
|
||||
{0x5e01, 0x09},
|
||||
{0x5e02, 0x09},
|
||||
{0x5e03, 0x0a},
|
||||
{0x5e04, 0x0a},
|
||||
{0x5e05, 0x0a},
|
||||
{0x5e06, 0x0b},
|
||||
{0x5e07, 0x0b},
|
||||
{0x5e08, 0x0c},
|
||||
{0x5e09, 0x0c},
|
||||
{0x5e0a, 0x0d},
|
||||
{0x5e0b, 0x0d},
|
||||
{0x5e0c, 0x0e},
|
||||
{0x5e0d, 0x0e},
|
||||
{0x5e0e, 0x0f},
|
||||
{0x5e0f, 0x0f},
|
||||
{0x5e10, 0x10},
|
||||
{0x5e11, 0x10},
|
||||
{0x5e12, 0x11},
|
||||
{0x5e13, 0x11},
|
||||
{0x5e14, 0x12},
|
||||
{0x5e15, 0x12},
|
||||
{0x5e16, 0x12},
|
||||
{0x5e17, 0x12},
|
||||
{0x5e18, 0x13},
|
||||
{0x5e19, 0x13},
|
||||
{0x5e1a, 0x13},
|
||||
{0x5e1b, 0x14},
|
||||
{0x5e1c, 0x14},
|
||||
{0x5e1d, 0x14},
|
||||
{0x5e1e, 0x15},
|
||||
{0x5e1f, 0x15},
|
||||
{0x5e20, 0x15},
|
||||
{0x5e21, 0x16},
|
||||
|
||||
// m_ndY_val[0:32]
|
||||
// 0x200+0xff+0x100*3+0x80*12+0x40*16 = 4095
|
||||
{0x5e22, 0x00}, {0x5e23, 0x02}, {0x5e24, 0x00},
|
||||
{0x5e25, 0x00}, {0x5e26, 0x00}, {0x5e27, 0xff},
|
||||
{0x5e28, 0x00}, {0x5e29, 0x01}, {0x5e2a, 0x00},
|
||||
{0x5e2b, 0x00}, {0x5e2c, 0x01}, {0x5e2d, 0x00},
|
||||
{0x5e2e, 0x00}, {0x5e2f, 0x01}, {0x5e30, 0x00},
|
||||
{0x5e31, 0x00}, {0x5e32, 0x00}, {0x5e33, 0x80},
|
||||
{0x5e34, 0x00}, {0x5e35, 0x00}, {0x5e36, 0x80},
|
||||
{0x5e37, 0x00}, {0x5e38, 0x00}, {0x5e39, 0x80},
|
||||
{0x5e3a, 0x00}, {0x5e3b, 0x00}, {0x5e3c, 0x80},
|
||||
{0x5e3d, 0x00}, {0x5e3e, 0x00}, {0x5e3f, 0x80},
|
||||
{0x5e40, 0x00}, {0x5e41, 0x00}, {0x5e42, 0x80},
|
||||
{0x5e43, 0x00}, {0x5e44, 0x00}, {0x5e45, 0x80},
|
||||
{0x5e46, 0x00}, {0x5e47, 0x00}, {0x5e48, 0x80},
|
||||
{0x5e49, 0x00}, {0x5e4a, 0x00}, {0x5e4b, 0x80},
|
||||
{0x5e4c, 0x00}, {0x5e4d, 0x00}, {0x5e4e, 0x80},
|
||||
{0x5e4f, 0x00}, {0x5e50, 0x00}, {0x5e51, 0x80},
|
||||
{0x5e52, 0x00}, {0x5e53, 0x00}, {0x5e54, 0x80},
|
||||
{0x5e55, 0x00}, {0x5e56, 0x00}, {0x5e57, 0x40},
|
||||
{0x5e58, 0x00}, {0x5e59, 0x00}, {0x5e5a, 0x40},
|
||||
{0x5e5b, 0x00}, {0x5e5c, 0x00}, {0x5e5d, 0x40},
|
||||
{0x5e5e, 0x00}, {0x5e5f, 0x00}, {0x5e60, 0x40},
|
||||
{0x5e61, 0x00}, {0x5e62, 0x00}, {0x5e63, 0x40},
|
||||
{0x5e64, 0x00}, {0x5e65, 0x00}, {0x5e66, 0x40},
|
||||
{0x5e67, 0x00}, {0x5e68, 0x00}, {0x5e69, 0x40},
|
||||
{0x5e6a, 0x00}, {0x5e6b, 0x00}, {0x5e6c, 0x40},
|
||||
{0x5e6d, 0x00}, {0x5e6e, 0x00}, {0x5e6f, 0x40},
|
||||
{0x5e70, 0x00}, {0x5e71, 0x00}, {0x5e72, 0x40},
|
||||
{0x5e73, 0x00}, {0x5e74, 0x00}, {0x5e75, 0x40},
|
||||
{0x5e76, 0x00}, {0x5e77, 0x00}, {0x5e78, 0x40},
|
||||
{0x5e79, 0x00}, {0x5e7a, 0x00}, {0x5e7b, 0x40},
|
||||
{0x5e7c, 0x00}, {0x5e7d, 0x00}, {0x5e7e, 0x40},
|
||||
{0x5e7f, 0x00}, {0x5e80, 0x00}, {0x5e81, 0x40},
|
||||
{0x5e82, 0x00}, {0x5e83, 0x00}, {0x5e84, 0x40},
|
||||
|
||||
// disable PWL
|
||||
/*{0x5e01, 0x18}, {0x5e02, 0x00}, {0x5e03, 0x00}, {0x5e04, 0x00},
|
||||
{0x5e05, 0x00}, {0x5e06, 0x00}, {0x5e07, 0x00}, {0x5e08, 0x00},
|
||||
{0x5e09, 0x00}, {0x5e0a, 0x00}, {0x5e0b, 0x00}, {0x5e0c, 0x00},
|
||||
{0x5e0d, 0x00}, {0x5e0e, 0x00}, {0x5e0f, 0x00}, {0x5e10, 0x00},
|
||||
{0x5e11, 0x00}, {0x5e12, 0x00}, {0x5e13, 0x00}, {0x5e14, 0x00},
|
||||
{0x5e15, 0x00}, {0x5e16, 0x00}, {0x5e17, 0x00}, {0x5e18, 0x00},
|
||||
{0x5e19, 0x00}, {0x5e1a, 0x00}, {0x5e1b, 0x00}, {0x5e1c, 0x00},
|
||||
{0x5e1d, 0x00}, {0x5e1e, 0x00}, {0x5e1f, 0x00}, {0x5e20, 0x00},
|
||||
{0x5e21, 0x00},
|
||||
|
||||
{0x5e22, 0x00}, {0x5e23, 0x0f}, {0x5e24, 0xFF},*/
|
||||
|
||||
{0x4001, 0x2b}, // BLC_CTRL_1
|
||||
{0x4008, 0x02}, {0x4009, 0x03},
|
||||
{0x4018, 0x12},
|
||||
{0x4022, 0x40},
|
||||
{0x4023, 0x20},
|
||||
|
||||
// all black level targets are 0x40
|
||||
{0x4026, 0x00}, {0x4027, 0x40},
|
||||
{0x4028, 0x00}, {0x4029, 0x40},
|
||||
{0x402a, 0x00}, {0x402b, 0x40},
|
||||
{0x402c, 0x00}, {0x402d, 0x40},
|
||||
|
||||
{0x407e, 0xcc},
|
||||
{0x407f, 0x18},
|
||||
{0x4080, 0xff},
|
||||
{0x4081, 0xff},
|
||||
{0x4082, 0x01},
|
||||
{0x4083, 0x53},
|
||||
{0x4084, 0x01},
|
||||
{0x4085, 0x2b},
|
||||
{0x4086, 0x00},
|
||||
{0x4087, 0xb3},
|
||||
|
||||
{0x4640, 0x40},
|
||||
{0x4641, 0x11},
|
||||
{0x4642, 0x0e},
|
||||
{0x4643, 0xee},
|
||||
{0x4646, 0x0f},
|
||||
{0x4648, 0x00},
|
||||
{0x4649, 0x03},
|
||||
|
||||
{0x4f00, 0x00},
|
||||
{0x4f01, 0x00},
|
||||
{0x4f02, 0x80},
|
||||
{0x4f03, 0x2c},
|
||||
{0x4f04, 0xf8},
|
||||
|
||||
{0x4d09, 0xff},
|
||||
{0x4d09, 0xdf},
|
||||
|
||||
{0x5003, 0x7a},
|
||||
{0x5b80, 0x08},
|
||||
{0x5c00, 0x08},
|
||||
{0x5c80, 0x00},
|
||||
{0x5bbe, 0x12},
|
||||
{0x5c3e, 0x12},
|
||||
{0x5cbe, 0x12},
|
||||
{0x5b8a, 0x80},
|
||||
{0x5b8b, 0x80},
|
||||
{0x5b8c, 0x80},
|
||||
{0x5b8d, 0x80},
|
||||
{0x5b8e, 0x60},
|
||||
{0x5b8f, 0x80},
|
||||
{0x5b90, 0x80},
|
||||
{0x5b91, 0x80},
|
||||
{0x5b92, 0x80},
|
||||
{0x5b93, 0x20},
|
||||
{0x5b94, 0x80},
|
||||
{0x5b95, 0x80},
|
||||
{0x5b96, 0x80},
|
||||
{0x5b97, 0x20},
|
||||
{0x5b98, 0x00},
|
||||
{0x5b99, 0x80},
|
||||
{0x5b9a, 0x40},
|
||||
{0x5b9b, 0x20},
|
||||
{0x5b9c, 0x00},
|
||||
{0x5b9d, 0x00},
|
||||
{0x5b9e, 0x80},
|
||||
{0x5b9f, 0x00},
|
||||
{0x5ba0, 0x00},
|
||||
{0x5ba1, 0x00},
|
||||
{0x5ba2, 0x00},
|
||||
{0x5ba3, 0x00},
|
||||
{0x5ba4, 0x00},
|
||||
{0x5ba5, 0x00},
|
||||
{0x5ba6, 0x00},
|
||||
{0x5ba7, 0x00},
|
||||
{0x5ba8, 0x02},
|
||||
{0x5ba9, 0x00},
|
||||
{0x5baa, 0x02},
|
||||
{0x5bab, 0x76},
|
||||
{0x5bac, 0x03},
|
||||
{0x5bad, 0x08},
|
||||
{0x5bae, 0x00},
|
||||
{0x5baf, 0x80},
|
||||
{0x5bb0, 0x00},
|
||||
{0x5bb1, 0xc0},
|
||||
{0x5bb2, 0x01},
|
||||
{0x5bb3, 0x00},
|
||||
|
||||
// m_nNormCombineWeight
|
||||
{0x5c0a, 0x80}, {0x5c0b, 0x80}, {0x5c0c, 0x80}, {0x5c0d, 0x80}, {0x5c0e, 0x60},
|
||||
{0x5c0f, 0x80}, {0x5c10, 0x80}, {0x5c11, 0x80}, {0x5c12, 0x60}, {0x5c13, 0x20},
|
||||
{0x5c14, 0x80}, {0x5c15, 0x80}, {0x5c16, 0x80}, {0x5c17, 0x20}, {0x5c18, 0x00},
|
||||
{0x5c19, 0x80}, {0x5c1a, 0x40}, {0x5c1b, 0x20}, {0x5c1c, 0x00}, {0x5c1d, 0x00},
|
||||
{0x5c1e, 0x80}, {0x5c1f, 0x00}, {0x5c20, 0x00}, {0x5c21, 0x00}, {0x5c22, 0x00},
|
||||
{0x5c23, 0x00}, {0x5c24, 0x00}, {0x5c25, 0x00}, {0x5c26, 0x00}, {0x5c27, 0x00},
|
||||
|
||||
// m_nCombinThreL
|
||||
{0x5c28, 0x02}, {0x5c29, 0x00},
|
||||
{0x5c2a, 0x02}, {0x5c2b, 0x76},
|
||||
{0x5c2c, 0x03}, {0x5c2d, 0x08},
|
||||
|
||||
// m_nCombinThreS
|
||||
{0x5c2e, 0x00}, {0x5c2f, 0x80},
|
||||
{0x5c30, 0x00}, {0x5c31, 0xc0},
|
||||
{0x5c32, 0x01}, {0x5c33, 0x00},
|
||||
|
||||
// m_nNormCombineWeight
|
||||
{0x5c8a, 0x80}, {0x5c8b, 0x80}, {0x5c8c, 0x80}, {0x5c8d, 0x80}, {0x5c8e, 0x80},
|
||||
{0x5c8f, 0x80}, {0x5c90, 0x80}, {0x5c91, 0x80}, {0x5c92, 0x80}, {0x5c93, 0x60},
|
||||
{0x5c94, 0x80}, {0x5c95, 0x80}, {0x5c96, 0x80}, {0x5c97, 0x60}, {0x5c98, 0x40},
|
||||
{0x5c99, 0x80}, {0x5c9a, 0x80}, {0x5c9b, 0x80}, {0x5c9c, 0x40}, {0x5c9d, 0x00},
|
||||
{0x5c9e, 0x80}, {0x5c9f, 0x80}, {0x5ca0, 0x80}, {0x5ca1, 0x20}, {0x5ca2, 0x00},
|
||||
{0x5ca3, 0x80}, {0x5ca4, 0x80}, {0x5ca5, 0x00}, {0x5ca6, 0x00}, {0x5ca7, 0x00},
|
||||
|
||||
{0x5ca8, 0x01}, {0x5ca9, 0x00},
|
||||
{0x5caa, 0x02}, {0x5cab, 0x00},
|
||||
{0x5cac, 0x03}, {0x5cad, 0x08},
|
||||
|
||||
{0x5cae, 0x01}, {0x5caf, 0x00},
|
||||
{0x5cb0, 0x02}, {0x5cb1, 0x00},
|
||||
{0x5cb2, 0x03}, {0x5cb3, 0x08},
|
||||
|
||||
// combine ISP
|
||||
{0x5be7, 0x80},
|
||||
{0x5bc9, 0x80},
|
||||
{0x5bca, 0x80},
|
||||
{0x5bcb, 0x80},
|
||||
{0x5bcc, 0x80},
|
||||
{0x5bcd, 0x80},
|
||||
{0x5bce, 0x80},
|
||||
{0x5bcf, 0x80},
|
||||
{0x5bd0, 0x80},
|
||||
{0x5bd1, 0x80},
|
||||
{0x5bd2, 0x20},
|
||||
{0x5bd3, 0x80},
|
||||
{0x5bd4, 0x40},
|
||||
{0x5bd5, 0x20},
|
||||
{0x5bd6, 0x00},
|
||||
{0x5bd7, 0x00},
|
||||
{0x5bd8, 0x00},
|
||||
{0x5bd9, 0x00},
|
||||
{0x5bda, 0x00},
|
||||
{0x5bdb, 0x00},
|
||||
{0x5bdc, 0x00},
|
||||
{0x5bdd, 0x00},
|
||||
{0x5bde, 0x00},
|
||||
{0x5bdf, 0x00},
|
||||
{0x5be0, 0x00},
|
||||
{0x5be1, 0x00},
|
||||
{0x5be2, 0x00},
|
||||
{0x5be3, 0x00},
|
||||
{0x5be4, 0x00},
|
||||
{0x5be5, 0x00},
|
||||
{0x5be6, 0x00},
|
||||
|
||||
// m_nSPDCombineWeight
|
||||
{0x5c49, 0x80}, {0x5c4a, 0x80}, {0x5c4b, 0x80}, {0x5c4c, 0x80}, {0x5c4d, 0x40},
|
||||
{0x5c4e, 0x80}, {0x5c4f, 0x80}, {0x5c50, 0x80}, {0x5c51, 0x60}, {0x5c52, 0x20},
|
||||
{0x5c53, 0x80}, {0x5c54, 0x80}, {0x5c55, 0x80}, {0x5c56, 0x20}, {0x5c57, 0x00},
|
||||
{0x5c58, 0x80}, {0x5c59, 0x40}, {0x5c5a, 0x20}, {0x5c5b, 0x00}, {0x5c5c, 0x00},
|
||||
{0x5c5d, 0x80}, {0x5c5e, 0x00}, {0x5c5f, 0x00}, {0x5c60, 0x00}, {0x5c61, 0x00},
|
||||
{0x5c62, 0x00}, {0x5c63, 0x00}, {0x5c64, 0x00}, {0x5c65, 0x00}, {0x5c66, 0x00},
|
||||
|
||||
// m_nSPDCombineWeight
|
||||
{0x5cc9, 0x80}, {0x5cca, 0x80}, {0x5ccb, 0x80}, {0x5ccc, 0x80}, {0x5ccd, 0x80},
|
||||
{0x5cce, 0x80}, {0x5ccf, 0x80}, {0x5cd0, 0x80}, {0x5cd1, 0x80}, {0x5cd2, 0x60},
|
||||
{0x5cd3, 0x80}, {0x5cd4, 0x80}, {0x5cd5, 0x80}, {0x5cd6, 0x60}, {0x5cd7, 0x40},
|
||||
{0x5cd8, 0x80}, {0x5cd9, 0x80}, {0x5cda, 0x80}, {0x5cdb, 0x40}, {0x5cdc, 0x20},
|
||||
{0x5cdd, 0x80}, {0x5cde, 0x80}, {0x5cdf, 0x80}, {0x5ce0, 0x20}, {0x5ce1, 0x00},
|
||||
{0x5ce2, 0x80}, {0x5ce3, 0x80}, {0x5ce4, 0x80}, {0x5ce5, 0x00}, {0x5ce6, 0x00},
|
||||
|
||||
{0x5d74, 0x01},
|
||||
{0x5d75, 0x00},
|
||||
|
||||
{0x5d1f, 0x81},
|
||||
{0x5d11, 0x00},
|
||||
{0x5d12, 0x10},
|
||||
{0x5d13, 0x10},
|
||||
{0x5d15, 0x05},
|
||||
{0x5d16, 0x05},
|
||||
{0x5d17, 0x05},
|
||||
{0x5d08, 0x03},
|
||||
{0x5d09, 0xb6},
|
||||
{0x5d0a, 0x03},
|
||||
{0x5d0b, 0xb6},
|
||||
{0x5d18, 0x03},
|
||||
{0x5d19, 0xb6},
|
||||
{0x5d62, 0x01},
|
||||
{0x5d40, 0x02},
|
||||
{0x5d41, 0x01},
|
||||
{0x5d63, 0x1f},
|
||||
{0x5d64, 0x00},
|
||||
{0x5d65, 0x80},
|
||||
{0x5d56, 0x00},
|
||||
{0x5d57, 0x20},
|
||||
{0x5d58, 0x00},
|
||||
{0x5d59, 0x20},
|
||||
{0x5d5a, 0x00},
|
||||
{0x5d5b, 0x0c},
|
||||
{0x5d5c, 0x02},
|
||||
{0x5d5d, 0x40},
|
||||
{0x5d5e, 0x02},
|
||||
{0x5d5f, 0x40},
|
||||
{0x5d60, 0x03},
|
||||
{0x5d61, 0x40},
|
||||
{0x5d4a, 0x02},
|
||||
{0x5d4b, 0x40},
|
||||
{0x5d4c, 0x02},
|
||||
{0x5d4d, 0x40},
|
||||
{0x5d4e, 0x02},
|
||||
{0x5d4f, 0x40},
|
||||
{0x5d50, 0x18},
|
||||
{0x5d51, 0x80},
|
||||
{0x5d52, 0x18},
|
||||
{0x5d53, 0x80},
|
||||
{0x5d54, 0x18},
|
||||
{0x5d55, 0x80},
|
||||
{0x5d46, 0x20},
|
||||
{0x5d47, 0x00},
|
||||
{0x5d48, 0x22},
|
||||
{0x5d49, 0x00},
|
||||
{0x5d42, 0x20},
|
||||
{0x5d43, 0x00},
|
||||
{0x5d44, 0x22},
|
||||
{0x5d45, 0x00},
|
||||
|
||||
{0x5004, 0x1e},
|
||||
{0x4221, 0x03}, // this is changed from 1 -> 3
|
||||
|
||||
// DCG exposure coarse
|
||||
// {0x3501, 0x01}, {0x3502, 0xc8},
|
||||
// SPD exposure coarse
|
||||
// {0x3541, 0x01}, {0x3542, 0xc8},
|
||||
// VS exposure coarse
|
||||
// {0x35c1, 0x00}, {0x35c2, 0x01},
|
||||
|
||||
// crc reference
|
||||
{0x420e, 0x66}, {0x420f, 0x5d}, {0x4210, 0xa8}, {0x4211, 0x55},
|
||||
// crc stat check
|
||||
{0x507a, 0x5f}, {0x507b, 0x46},
|
||||
|
||||
// watchdog control
|
||||
{0x4f00, 0x00}, {0x4f01, 0x01}, {0x4f02, 0x80}, {0x4f04, 0x2c},
|
||||
|
||||
// color balance gains
|
||||
// blue
|
||||
{0x5280, 0x06}, {0x5281, 0xCB}, // hcg
|
||||
{0x5480, 0x06}, {0x5481, 0xCB}, // lcg
|
||||
{0x5680, 0x06}, {0x5681, 0xCB}, // spd
|
||||
{0x5880, 0x06}, {0x5881, 0xCB}, // vs
|
||||
|
||||
// green(blue)
|
||||
{0x5282, 0x04}, {0x5283, 0x00},
|
||||
{0x5482, 0x04}, {0x5483, 0x00},
|
||||
{0x5682, 0x04}, {0x5683, 0x00},
|
||||
{0x5882, 0x04}, {0x5883, 0x00},
|
||||
|
||||
// green(red)
|
||||
{0x5284, 0x04}, {0x5285, 0x00},
|
||||
{0x5484, 0x04}, {0x5485, 0x00},
|
||||
{0x5684, 0x04}, {0x5685, 0x00},
|
||||
{0x5884, 0x04}, {0x5885, 0x00},
|
||||
|
||||
// red
|
||||
{0x5286, 0x08}, {0x5287, 0xDE},
|
||||
{0x5486, 0x08}, {0x5487, 0xDE},
|
||||
{0x5686, 0x08}, {0x5687, 0xDE},
|
||||
{0x5886, 0x08}, {0x5887, 0xDE},
|
||||
|
||||
// fixed gains
|
||||
{0x3588, 0x01}, {0x3589, 0x00},
|
||||
{0x35c8, 0x01}, {0x35c9, 0x00},
|
||||
{0x3548, 0x0F}, {0x3549, 0x00},
|
||||
{0x35c1, 0x00},
|
||||
};
|
||||
@@ -0,0 +1,115 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "media/cam_isp.h"
|
||||
#include "media/cam_sensor.h"
|
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
#include "system/camerad/sensors/ar0231_registers.h"
|
||||
#include "system/camerad/sensors/ox03c10_registers.h"
|
||||
#include "system/camerad/sensors/os04c10_registers.h"
|
||||
|
||||
#define ANALOG_GAIN_MAX_CNT 55
|
||||
|
||||
class SensorInfo {
|
||||
public:
|
||||
SensorInfo() = default;
|
||||
virtual std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { return {}; }
|
||||
virtual float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const {return 0; }
|
||||
virtual int getSlaveAddress(int port) const { assert(0); }
|
||||
|
||||
cereal::FrameData::ImageSensor image_sensor = cereal::FrameData::ImageSensor::UNKNOWN;
|
||||
float pixel_size_mm;
|
||||
uint32_t frame_width, frame_height;
|
||||
uint32_t frame_stride;
|
||||
uint32_t frame_offset = 0;
|
||||
uint32_t extra_height = 0;
|
||||
int registers_offset = -1;
|
||||
int stats_offset = -1;
|
||||
int hdr_offset = -1;
|
||||
|
||||
int exposure_time_min;
|
||||
int exposure_time_max;
|
||||
|
||||
float dc_gain_factor;
|
||||
int dc_gain_min_weight;
|
||||
int dc_gain_max_weight;
|
||||
float dc_gain_on_grey;
|
||||
float dc_gain_off_grey;
|
||||
|
||||
float ev_scale = 1.0;
|
||||
float sensor_analog_gains[ANALOG_GAIN_MAX_CNT];
|
||||
int analog_gain_min_idx;
|
||||
int analog_gain_max_idx;
|
||||
int analog_gain_rec_idx;
|
||||
int analog_gain_cost_delta;
|
||||
float analog_gain_cost_low;
|
||||
float analog_gain_cost_high;
|
||||
float target_grey_factor;
|
||||
float min_ev;
|
||||
float max_ev;
|
||||
|
||||
bool data_word;
|
||||
uint32_t probe_reg_addr;
|
||||
uint32_t probe_expected_data;
|
||||
std::vector<i2c_random_wr_payload> start_reg_array;
|
||||
std::vector<i2c_random_wr_payload> init_reg_array;
|
||||
|
||||
uint32_t bits_per_pixel;
|
||||
uint32_t bayer_pattern;
|
||||
uint32_t mipi_format;
|
||||
uint32_t mclk_frequency;
|
||||
uint32_t frame_data_type;
|
||||
|
||||
uint32_t readout_time_ns; // used to recover EOF from SOF
|
||||
|
||||
// ISP image processing params
|
||||
uint32_t black_level;
|
||||
std::vector<uint32_t> color_correct_matrix; // 3x3
|
||||
std::vector<uint32_t> gamma_lut_rgb; // gamma LUTs are length 64 * sizeof(uint32_t); same for r/g/b here
|
||||
void prepare_gamma_lut() {
|
||||
for (int i = 0; i < 64; i++) {
|
||||
gamma_lut_rgb[i] |= ((uint32_t)(gamma_lut_rgb[i+1] - gamma_lut_rgb[i]) << 10);
|
||||
}
|
||||
gamma_lut_rgb.pop_back();
|
||||
}
|
||||
std::vector<uint32_t> linearization_lut; // length 36
|
||||
std::vector<uint32_t> linearization_pts; // length 4
|
||||
std::vector<uint32_t> vignetting_lut; // length 221
|
||||
|
||||
const int num() const {
|
||||
return static_cast<int>(image_sensor);
|
||||
};
|
||||
};
|
||||
|
||||
class AR0231 : public SensorInfo {
|
||||
public:
|
||||
AR0231();
|
||||
std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
|
||||
float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
|
||||
int getSlaveAddress(int port) const override;
|
||||
|
||||
private:
|
||||
mutable std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
|
||||
};
|
||||
|
||||
class OX03C10 : public SensorInfo {
|
||||
public:
|
||||
OX03C10();
|
||||
std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
|
||||
float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
|
||||
int getSlaveAddress(int port) const override;
|
||||
};
|
||||
|
||||
class OS04C10 : public SensorInfo {
|
||||
public:
|
||||
OS04C10();
|
||||
std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
|
||||
float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
|
||||
int getSlaveAddress(int port) const override;
|
||||
};
|
||||
Executable
+125
@@ -0,0 +1,125 @@
|
||||
#!/usr/bin/env python3
|
||||
import subprocess
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
from PIL import Image
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.system.hardware import PC
|
||||
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
|
||||
from openpilot.system.manager.process_config import managed_processes
|
||||
|
||||
|
||||
VISION_STREAMS = {
|
||||
"roadCameraState": VisionStreamType.VISION_STREAM_ROAD,
|
||||
"driverCameraState": VisionStreamType.VISION_STREAM_DRIVER,
|
||||
"wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD,
|
||||
}
|
||||
|
||||
|
||||
def jpeg_write(fn, dat):
|
||||
img = Image.fromarray(dat)
|
||||
img.save(fn, "JPEG")
|
||||
|
||||
|
||||
def yuv_to_rgb(y, u, v):
|
||||
ul = np.repeat(np.repeat(u, 2).reshape(u.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
|
||||
vl = np.repeat(np.repeat(v, 2).reshape(v.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
|
||||
|
||||
yuv = np.dstack((y, ul, vl)).astype(np.int16)
|
||||
yuv[:, :, 1:] -= 128
|
||||
|
||||
m = np.array([
|
||||
[1.00000, 1.00000, 1.00000],
|
||||
[0.00000, -0.39465, 2.03211],
|
||||
[1.13983, -0.58060, 0.00000],
|
||||
])
|
||||
rgb = np.dot(yuv, m).clip(0, 255)
|
||||
return rgb.astype(np.uint8)
|
||||
|
||||
|
||||
def extract_image(buf):
|
||||
y = np.array(buf.data[:buf.uv_offset], dtype=np.uint8).reshape((-1, buf.stride))[:buf.height, :buf.width]
|
||||
u = np.array(buf.data[buf.uv_offset::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2]
|
||||
v = np.array(buf.data[buf.uv_offset+1::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2]
|
||||
|
||||
return yuv_to_rgb(y, u, v)
|
||||
|
||||
|
||||
def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
|
||||
sockets = [s for s in (frame, front_frame) if s is not None]
|
||||
sm = messaging.SubMaster(sockets)
|
||||
vipc_clients = {s: VisionIpcClient("camerad", VISION_STREAMS[s], True) for s in sockets}
|
||||
|
||||
# wait 4 sec from camerad startup for focus and exposure
|
||||
while sm[sockets[0]].frameId < int(4. / DT_MDL):
|
||||
sm.update()
|
||||
|
||||
for client in vipc_clients.values():
|
||||
client.connect(True)
|
||||
|
||||
# grab images
|
||||
rear, front = None, None
|
||||
if frame is not None:
|
||||
c = vipc_clients[frame]
|
||||
rear = extract_image(c.recv())
|
||||
if front_frame is not None:
|
||||
c = vipc_clients[front_frame]
|
||||
front = extract_image(c.recv())
|
||||
return rear, front
|
||||
|
||||
|
||||
def snapshot():
|
||||
params = Params()
|
||||
|
||||
if (not params.get_bool("IsOffroad")) or params.get_bool("IsTakingSnapshot"):
|
||||
print("Already taking snapshot")
|
||||
return None, None
|
||||
|
||||
front_camera_allowed = params.get_bool("RecordFront")
|
||||
params.put_bool("IsTakingSnapshot", True)
|
||||
set_offroad_alert("Offroad_IsTakingSnapshot", True)
|
||||
time.sleep(2.0) # Give hardwared time to read the param, or if just started give camerad time to start
|
||||
|
||||
# Check if camerad is already started
|
||||
try:
|
||||
subprocess.check_call(["pgrep", "camerad"])
|
||||
print("Camerad already running")
|
||||
params.put_bool("IsTakingSnapshot", False)
|
||||
params.remove("Offroad_IsTakingSnapshot")
|
||||
return None, None
|
||||
except subprocess.CalledProcessError:
|
||||
pass
|
||||
|
||||
try:
|
||||
# Allow testing on replay on PC
|
||||
if not PC:
|
||||
managed_processes['camerad'].start()
|
||||
|
||||
frame = "wideRoadCameraState"
|
||||
front_frame = "driverCameraState" if front_camera_allowed else None
|
||||
rear, front = get_snapshots(frame, front_frame)
|
||||
finally:
|
||||
managed_processes['camerad'].stop()
|
||||
params.put_bool("IsTakingSnapshot", False)
|
||||
set_offroad_alert("Offroad_IsTakingSnapshot", False)
|
||||
|
||||
if not front_camera_allowed:
|
||||
front = None
|
||||
|
||||
return rear, front
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
pic, fpic = snapshot()
|
||||
if pic is not None:
|
||||
print(pic.shape)
|
||||
jpeg_write("/tmp/back.jpg", pic)
|
||||
if fpic is not None:
|
||||
jpeg_write("/tmp/front.jpg", fpic)
|
||||
else:
|
||||
print("Error taking snapshot")
|
||||
@@ -0,0 +1,2 @@
|
||||
jpegs/
|
||||
test_ae_gray
|
||||
Executable
+27
@@ -0,0 +1,27 @@
|
||||
#!/usr/bin/env python3
|
||||
# type: ignore
|
||||
import cereal.messaging as messaging
|
||||
|
||||
all_sockets = ['roadCameraState', 'driverCameraState', 'wideRoadCameraState']
|
||||
prev_id = [None,None,None]
|
||||
this_id = [None,None,None]
|
||||
dt = [None,None,None]
|
||||
num_skipped = [0,0,0]
|
||||
|
||||
if __name__ == "__main__":
|
||||
sm = messaging.SubMaster(all_sockets)
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
for i in range(len(all_sockets)):
|
||||
if not sm.updated[all_sockets[i]]:
|
||||
continue
|
||||
this_id[i] = sm[all_sockets[i]].frameId
|
||||
if prev_id[i] is None:
|
||||
prev_id[i] = this_id[i]
|
||||
continue
|
||||
dt[i] = this_id[i] - prev_id[i]
|
||||
if dt[i] != 1:
|
||||
num_skipped[i] += dt[i] - 1
|
||||
print(all_sockets[i] ,dt[i] - 1, num_skipped[i])
|
||||
prev_id[i] = this_id[i]
|
||||
Executable
+16
@@ -0,0 +1,16 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
#echo 4294967295 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
|
||||
|
||||
# no CCI and UTIL, very spammy
|
||||
echo 0xfffdbfff | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
|
||||
#echo 0 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
|
||||
|
||||
sudo dmesg -C
|
||||
scons -u -j8 --minimal .
|
||||
export DEBUG_FRAMES=1
|
||||
export DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1
|
||||
#export DISABLE_DRIVER=1
|
||||
export LOGPRINT=debug
|
||||
./camerad
|
||||
+24
@@ -0,0 +1,24 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import os
|
||||
from tqdm import tqdm
|
||||
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("route", help="The route name")
|
||||
args = parser.parse_args()
|
||||
|
||||
out_path = os.path.join("jpegs", f"{args.route.replace('|', '_').replace('/', '_')}")
|
||||
os.makedirs(out_path, exist_ok=True)
|
||||
|
||||
lr = LogReader(args.route)
|
||||
|
||||
for msg in tqdm(lr):
|
||||
if msg.which() == 'thumbnail':
|
||||
with open(os.path.join(out_path, f"{msg.thumbnail.frameId}.jpg"), 'wb') as f:
|
||||
f.write(msg.thumbnail.thumbnail)
|
||||
elif msg.which() == 'navThumbnail':
|
||||
with open(os.path.join(out_path, f"nav_{msg.navThumbnail.frameId}.jpg"), 'wb') as f:
|
||||
f.write(msg.navThumbnail.thumbnail)
|
||||
Executable
+13
@@ -0,0 +1,13 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
cd /sys/kernel/debug/tracing
|
||||
echo "" > trace
|
||||
echo 1 > tracing_on
|
||||
#echo Y > /sys/kernel/debug/camera_icp/a5_debug_q
|
||||
echo 0x1 > /sys/kernel/debug/camera_icp/a5_debug_type
|
||||
echo 1 > /sys/kernel/debug/tracing/events/camera/enable
|
||||
echo 0xffffffff > /sys/kernel/debug/camera_icp/a5_debug_lvl
|
||||
echo 1 > /sys/kernel/debug/tracing/events/camera/cam_icp_fw_dbg/enable
|
||||
|
||||
cat /sys/kernel/debug/tracing/trace_pipe
|
||||
Executable
+2
@@ -0,0 +1,2 @@
|
||||
#!/usr/bin/env bash
|
||||
DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1 DEBUG_FRAMES=1 LOGPRINT=debug LD_PRELOAD=/data/tici_test_scripts/isp/interceptor/tmpioctl.so ./camerad
|
||||
Executable
+9
@@ -0,0 +1,9 @@
|
||||
#!/bin/sh
|
||||
cd ..
|
||||
while :; do
|
||||
./camerad &
|
||||
pid="$!"
|
||||
sleep 2
|
||||
kill -2 $pid
|
||||
wait $pid
|
||||
done
|
||||
@@ -0,0 +1,98 @@
|
||||
import os
|
||||
import time
|
||||
import pytest
|
||||
import numpy as np
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal.services import SERVICE_LIST
|
||||
from openpilot.system.manager.process_config import managed_processes
|
||||
from openpilot.tools.lib.log_time_series import msgs_to_time_series
|
||||
|
||||
TEST_TIMESPAN = 10
|
||||
CAMERAS = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
|
||||
|
||||
|
||||
def run_and_log(procs, services, duration):
|
||||
logs = []
|
||||
|
||||
try:
|
||||
for p in procs:
|
||||
managed_processes[p].start()
|
||||
socks = [messaging.sub_sock(s, conflate=False, timeout=100) for s in services]
|
||||
|
||||
start_time = time.monotonic()
|
||||
while time.monotonic() - start_time < duration:
|
||||
for s in socks:
|
||||
logs.extend(messaging.drain_sock(s))
|
||||
for p in procs:
|
||||
assert managed_processes[p].proc.is_alive()
|
||||
finally:
|
||||
for p in procs:
|
||||
managed_processes[p].stop()
|
||||
|
||||
return logs
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def logs():
|
||||
logs = run_and_log(["camerad", ], CAMERAS, TEST_TIMESPAN)
|
||||
ts = msgs_to_time_series(logs)
|
||||
|
||||
for cam in CAMERAS:
|
||||
expected_frames = SERVICE_LIST[cam].frequency * TEST_TIMESPAN
|
||||
cnt = len(ts[cam]['t'])
|
||||
assert expected_frames*0.8 < cnt < expected_frames*1.2, f"unexpected frame count {cam}: {expected_frames=}, got {cnt}"
|
||||
|
||||
dts = np.abs(np.diff([ts[cam]['timestampSof']/1e6]) - 1000/SERVICE_LIST[cam].frequency)
|
||||
assert (dts < 1.0).all(), f"{cam} dts(ms) out of spec: max diff {dts.max()}, 99 percentile {np.percentile(dts, 99)}"
|
||||
return ts
|
||||
|
||||
@pytest.mark.tici
|
||||
class TestCamerad:
|
||||
def test_frame_skips(self, logs):
|
||||
for c in CAMERAS:
|
||||
assert set(np.diff(logs[c]['frameId'])) == {1, }, f"{c} has frame skips"
|
||||
|
||||
def test_frame_sync(self, logs):
|
||||
n = range(len(logs['roadCameraState']['t'][:-10]))
|
||||
|
||||
frame_ids = {i: [logs[cam]['frameId'][i] for cam in CAMERAS] for i in n}
|
||||
assert all(len(set(v)) == 1 for v in frame_ids.values()), "frame IDs not aligned"
|
||||
|
||||
frame_times = {i: [logs[cam]['timestampSof'][i] for cam in CAMERAS] for i in n}
|
||||
diffs = {i: (max(ts) - min(ts))/1e6 for i, ts in frame_times.items()}
|
||||
|
||||
laggy_frames = {k: v for k, v in diffs.items() if v > 1.1}
|
||||
assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}"
|
||||
|
||||
def test_sanity_checks(self, logs):
|
||||
self._sanity_checks(logs)
|
||||
|
||||
def _sanity_checks(self, ts):
|
||||
for c in CAMERAS:
|
||||
assert c in ts
|
||||
assert len(ts[c]['t']) > 20
|
||||
|
||||
# not a valid request id
|
||||
assert 0 not in ts[c]['requestId']
|
||||
|
||||
# should monotonically increase
|
||||
assert np.all(np.diff(ts[c]['frameId']) >= 1)
|
||||
assert np.all(np.diff(ts[c]['requestId']) >= 1)
|
||||
|
||||
# EOF > SOF
|
||||
assert np.all((ts[c]['timestampEof'] - ts[c]['timestampSof']) > 0)
|
||||
|
||||
# logMonoTime > SOF
|
||||
assert np.all((ts[c]['t'] - ts[c]['timestampSof']/1e9) > 1e-7)
|
||||
assert np.all((ts[c]['t'] - ts[c]['timestampEof']/1e9) > 1e-7)
|
||||
|
||||
def test_stress_test(self):
|
||||
os.environ['SPECTRA_ERROR_PROB'] = '0.008'
|
||||
logs = run_and_log(["camerad", ], CAMERAS, 10)
|
||||
ts = msgs_to_time_series(logs)
|
||||
|
||||
# we should see some jumps from introduced errors
|
||||
assert np.max([ np.max(np.diff(ts[c]['frameId'])) for c in CAMERAS ]) > 1
|
||||
assert np.max([ np.max(np.diff(ts[c]['requestId'])) for c in CAMERAS ]) > 1
|
||||
|
||||
self._sanity_checks(ts)
|
||||
@@ -0,0 +1,51 @@
|
||||
import time
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.test.helpers import with_processes
|
||||
from openpilot.system.camerad.snapshot.snapshot import get_snapshots
|
||||
|
||||
TEST_TIME = 45
|
||||
REPEAT = 5
|
||||
|
||||
@pytest.mark.tici
|
||||
class TestCamerad:
|
||||
@classmethod
|
||||
def setup_class(cls):
|
||||
pass
|
||||
|
||||
def _numpy_rgb2gray(self, im):
|
||||
ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8)
|
||||
return ret
|
||||
|
||||
def _is_exposure_okay(self, i, med_mean=None):
|
||||
if med_mean is None:
|
||||
med_mean = np.array([[0.2,0.4],[0.2,0.6]])
|
||||
h, w = i.shape[:2]
|
||||
i = i[h//10:9*h//10,w//10:9*w//10]
|
||||
med_ex, mean_ex = med_mean
|
||||
i = self._numpy_rgb2gray(i)
|
||||
i_median = np.median(i) / 255.
|
||||
i_mean = np.mean(i) / 255.
|
||||
print([i_median, i_mean])
|
||||
return med_ex[0] < i_median < med_ex[1] and mean_ex[0] < i_mean < mean_ex[1]
|
||||
|
||||
@with_processes(['camerad'])
|
||||
def test_camera_operation(self):
|
||||
passed = 0
|
||||
start = time.time()
|
||||
while time.time() - start < TEST_TIME and passed < REPEAT:
|
||||
rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState")
|
||||
wpic, _ = get_snapshots(frame="wideRoadCameraState")
|
||||
|
||||
res = self._is_exposure_okay(rpic)
|
||||
res = res and self._is_exposure_okay(dpic)
|
||||
res = res and self._is_exposure_okay(wpic)
|
||||
|
||||
if passed > 0 and not res:
|
||||
passed = -passed # fails test if any failure after first sus
|
||||
break
|
||||
|
||||
passed += int(res)
|
||||
time.sleep(2)
|
||||
assert passed >= REPEAT
|
||||
@@ -0,0 +1 @@
|
||||
eon/rat
|
||||
@@ -0,0 +1,16 @@
|
||||
import os
|
||||
from typing import cast
|
||||
|
||||
from openpilot.system.hardware.base import HardwareBase
|
||||
from openpilot.system.hardware.tici.hardware import Tici
|
||||
from openpilot.system.hardware.pc.hardware import Pc
|
||||
|
||||
TICI = os.path.isfile('/TICI')
|
||||
AGNOS = os.path.isfile('/AGNOS')
|
||||
PC = not TICI
|
||||
|
||||
|
||||
if TICI:
|
||||
HARDWARE = cast(HardwareBase, Tici())
|
||||
else:
|
||||
HARDWARE = cast(HardwareBase, Pc())
|
||||
@@ -0,0 +1,40 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
// no-op base hw class
|
||||
class HardwareNone {
|
||||
public:
|
||||
static constexpr float MAX_VOLUME = 0.7;
|
||||
static constexpr float MIN_VOLUME = 0.2;
|
||||
|
||||
static std::string get_os_version() { return ""; }
|
||||
static std::string get_name() { return ""; }
|
||||
static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::UNKNOWN; }
|
||||
static int get_voltage() { return 0; }
|
||||
static int get_current() { return 0; }
|
||||
|
||||
static std::string get_serial() { return "cccccc"; }
|
||||
|
||||
static std::map<std::string, std::string> get_init_logs() {
|
||||
return {};
|
||||
}
|
||||
|
||||
static void reboot() {}
|
||||
static void poweroff() {}
|
||||
static void set_brightness(int percent) {}
|
||||
static void set_ir_power(int percentage) {}
|
||||
static void set_display_power(bool on) {}
|
||||
|
||||
static bool get_ssh_enabled() { return false; }
|
||||
static void set_ssh_enabled(bool enabled) {}
|
||||
|
||||
static bool PC() { return false; }
|
||||
static bool TICI() { return false; }
|
||||
static bool AGNOS() { return false; }
|
||||
};
|
||||
@@ -0,0 +1,181 @@
|
||||
import os
|
||||
from abc import abstractmethod, ABC
|
||||
from dataclasses import dataclass, fields
|
||||
|
||||
from cereal import log
|
||||
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
|
||||
@dataclass
|
||||
class ThermalZone:
|
||||
# a zone from /sys/class/thermal/thermal_zone*
|
||||
name: str # a.k.a type
|
||||
scale: float = 1000. # scale to get degrees in C
|
||||
zone_number = -1
|
||||
|
||||
def read(self) -> float:
|
||||
if self.zone_number < 0:
|
||||
for n in os.listdir("/sys/devices/virtual/thermal"):
|
||||
if not n.startswith("thermal_zone"):
|
||||
continue
|
||||
with open(os.path.join("/sys/devices/virtual/thermal", n, "type")) as f:
|
||||
if f.read().strip() == self.name:
|
||||
self.zone_number = int(n.removeprefix("thermal_zone"))
|
||||
break
|
||||
|
||||
try:
|
||||
with open(f"/sys/devices/virtual/thermal/thermal_zone{self.zone_number}/temp") as f:
|
||||
return int(f.read()) / self.scale
|
||||
except FileNotFoundError:
|
||||
return 0
|
||||
|
||||
@dataclass
|
||||
class ThermalConfig:
|
||||
cpu: list[ThermalZone] | None = None
|
||||
gpu: list[ThermalZone] | None = None
|
||||
pmic: list[ThermalZone] | None = None
|
||||
memory: ThermalZone | None = None
|
||||
intake: ThermalZone | None = None
|
||||
exhaust: ThermalZone | None = None
|
||||
case: ThermalZone | None = None
|
||||
|
||||
def get_msg(self):
|
||||
ret = {}
|
||||
for f in fields(ThermalConfig):
|
||||
v = getattr(self, f.name)
|
||||
if v is not None:
|
||||
if isinstance(v, list):
|
||||
ret[f.name + "TempC"] = [x.read() for x in v]
|
||||
else:
|
||||
ret[f.name + "TempC"] = v.read()
|
||||
return ret
|
||||
|
||||
class HardwareBase(ABC):
|
||||
@staticmethod
|
||||
def get_cmdline() -> dict[str, str]:
|
||||
with open('/proc/cmdline') as f:
|
||||
cmdline = f.read()
|
||||
return {kv[0]: kv[1] for kv in [s.split('=') for s in cmdline.split(' ')] if len(kv) == 2}
|
||||
|
||||
@staticmethod
|
||||
def read_param_file(path, parser, default=0):
|
||||
try:
|
||||
with open(path) as f:
|
||||
return parser(f.read())
|
||||
except Exception:
|
||||
return default
|
||||
|
||||
def booted(self) -> bool:
|
||||
return True
|
||||
|
||||
@abstractmethod
|
||||
def reboot(self, reason=None):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def uninstall(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_os_version(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_device_type(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_imei(self, slot) -> str:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_serial(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_network_info(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_network_type(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_sim_info(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_network_strength(self, network_type):
|
||||
pass
|
||||
|
||||
def get_network_metered(self, network_type) -> bool:
|
||||
return network_type not in (NetworkType.none, NetworkType.wifi, NetworkType.ethernet)
|
||||
|
||||
@staticmethod
|
||||
def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_current_power_draw(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_som_power_draw(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def shutdown(self):
|
||||
pass
|
||||
|
||||
def get_thermal_config(self):
|
||||
return ThermalConfig()
|
||||
|
||||
@abstractmethod
|
||||
def set_screen_brightness(self, percentage):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_screen_brightness(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def set_power_save(self, powersave_enabled):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_gpu_usage_percent(self):
|
||||
pass
|
||||
|
||||
def get_modem_version(self):
|
||||
return None
|
||||
|
||||
@abstractmethod
|
||||
def get_modem_temperatures(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_nvme_temperatures(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def initialize_hardware(self):
|
||||
pass
|
||||
|
||||
def configure_modem(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_networks(self):
|
||||
pass
|
||||
|
||||
def has_internal_panda(self) -> bool:
|
||||
return False
|
||||
|
||||
def reset_internal_panda(self):
|
||||
pass
|
||||
|
||||
def recover_internal_panda(self):
|
||||
pass
|
||||
|
||||
def get_modem_data_usage(self):
|
||||
return -1, -1
|
||||
Executable
+38
@@ -0,0 +1,38 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from abc import ABC, abstractmethod
|
||||
|
||||
from openpilot.common.realtime import DT_HW
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.pid import PIDController
|
||||
|
||||
class BaseFanController(ABC):
|
||||
@abstractmethod
|
||||
def update(self, cur_temp: float, ignition: bool) -> int:
|
||||
pass
|
||||
|
||||
|
||||
class TiciFanController(BaseFanController):
|
||||
def __init__(self) -> None:
|
||||
super().__init__()
|
||||
cloudlog.info("Setting up TICI fan handler")
|
||||
|
||||
self.last_ignition = False
|
||||
self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_HW))
|
||||
|
||||
def update(self, cur_temp: float, ignition: bool) -> int:
|
||||
self.controller.neg_limit = -(100 if ignition else 30)
|
||||
self.controller.pos_limit = -(30 if ignition else 0)
|
||||
|
||||
if ignition != self.last_ignition:
|
||||
self.controller.reset()
|
||||
|
||||
error = 70 - cur_temp
|
||||
fan_pwr_out = -int(self.controller.update(
|
||||
error=error,
|
||||
feedforward=np.interp(cur_temp, [60.0, 100.0], [0, -100])
|
||||
))
|
||||
|
||||
self.last_ignition = ignition
|
||||
return fan_pwr_out
|
||||
|
||||
Executable
+482
@@ -0,0 +1,482 @@
|
||||
#!/usr/bin/env python3
|
||||
import fcntl
|
||||
import os
|
||||
import json
|
||||
import queue
|
||||
import struct
|
||||
import threading
|
||||
import time
|
||||
from collections import OrderedDict, namedtuple
|
||||
from pathlib import Path
|
||||
|
||||
import psutil
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log
|
||||
from cereal.services import SERVICE_LIST
|
||||
from openpilot.common.dict_helpers import strip_deprecated_keys
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_HW
|
||||
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
|
||||
from openpilot.system.hardware import HARDWARE, TICI, AGNOS
|
||||
from openpilot.system.loggerd.config import get_available_percent
|
||||
from openpilot.system.statsd import statlog
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.hardware.power_monitoring import PowerMonitoring
|
||||
from openpilot.system.hardware.fan_controller import TiciFanController
|
||||
from openpilot.system.version import terms_version, training_version
|
||||
|
||||
ThermalStatus = log.DeviceState.ThermalStatus
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
NetworkStrength = log.DeviceState.NetworkStrength
|
||||
CURRENT_TAU = 15. # 15s time constant
|
||||
TEMP_TAU = 5. # 5s time constant
|
||||
DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert
|
||||
PANDA_STATES_TIMEOUT = round(1000 / SERVICE_LIST['pandaStates'].frequency * 1.5) # 1.5x the expected pandaState frequency
|
||||
|
||||
ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp'])
|
||||
HardwareState = namedtuple("HardwareState", ['network_type', 'network_info', 'network_strength', 'network_stats',
|
||||
'network_metered', 'nvme_temps', 'modem_temps'])
|
||||
|
||||
# List of thermal bands. We will stay within this region as long as we are within the bounds.
|
||||
# When exiting the bounds, we'll jump to the lower or higher band. Bands are ordered in the dict.
|
||||
THERMAL_BANDS = OrderedDict({
|
||||
ThermalStatus.green: ThermalBand(None, 80.0),
|
||||
ThermalStatus.yellow: ThermalBand(75.0, 96.0),
|
||||
ThermalStatus.red: ThermalBand(88.0, 107.),
|
||||
ThermalStatus.danger: ThermalBand(94.0, None),
|
||||
})
|
||||
|
||||
# Override to highest thermal band when offroad and above this temp
|
||||
OFFROAD_DANGER_TEMP = 75
|
||||
|
||||
prev_offroad_states: dict[str, tuple[bool, str | None]] = {}
|
||||
|
||||
|
||||
|
||||
def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: str | None=None):
|
||||
if prev_offroad_states.get(offroad_alert, None) == (show_alert, extra_text):
|
||||
return
|
||||
prev_offroad_states[offroad_alert] = (show_alert, extra_text)
|
||||
set_offroad_alert(offroad_alert, show_alert, extra_text)
|
||||
|
||||
def touch_thread(end_event):
|
||||
count = 0
|
||||
|
||||
pm = messaging.PubMaster(["touch"])
|
||||
|
||||
event_format = "llHHi"
|
||||
event_size = struct.calcsize(event_format)
|
||||
event_frame = []
|
||||
|
||||
with open("/dev/input/by-path/platform-894000.i2c-event", "rb") as event_file:
|
||||
fcntl.fcntl(event_file, fcntl.F_SETFL, os.O_NONBLOCK)
|
||||
while not end_event.is_set():
|
||||
if (count % int(1. / DT_HW)) == 0:
|
||||
event = event_file.read(event_size)
|
||||
if event:
|
||||
(sec, usec, etype, code, value) = struct.unpack(event_format, event)
|
||||
if etype != 0 or code != 0 or value != 0:
|
||||
touch = log.Touch.new_message()
|
||||
touch.sec = sec
|
||||
touch.usec = usec
|
||||
touch.type = etype
|
||||
touch.code = code
|
||||
touch.value = value
|
||||
event_frame.append(touch)
|
||||
else: # end of frame, push new log
|
||||
msg = messaging.new_message('touch', len(event_frame), valid=True)
|
||||
msg.touch = event_frame
|
||||
pm.send('touch', msg)
|
||||
event_frame = []
|
||||
continue
|
||||
|
||||
count += 1
|
||||
time.sleep(DT_HW)
|
||||
|
||||
|
||||
def hw_state_thread(end_event, hw_queue):
|
||||
"""Handles non critical hardware state, and sends over queue"""
|
||||
count = 0
|
||||
prev_hw_state = None
|
||||
|
||||
modem_version = None
|
||||
modem_configured = False
|
||||
modem_restarted = False
|
||||
modem_missing_count = 0
|
||||
|
||||
while not end_event.is_set():
|
||||
# these are expensive calls. update every 10s
|
||||
if (count % int(10. / DT_HW)) == 0:
|
||||
try:
|
||||
network_type = HARDWARE.get_network_type()
|
||||
modem_temps = HARDWARE.get_modem_temperatures()
|
||||
if len(modem_temps) == 0 and prev_hw_state is not None:
|
||||
modem_temps = prev_hw_state.modem_temps
|
||||
|
||||
# Log modem version once
|
||||
if AGNOS and (modem_version is None):
|
||||
modem_version = HARDWARE.get_modem_version()
|
||||
|
||||
if modem_version is not None:
|
||||
cloudlog.event("modem version", version=modem_version)
|
||||
else:
|
||||
if not modem_restarted:
|
||||
# TODO: we may be able to remove this with a MM update
|
||||
# ModemManager's probing on startup can fail
|
||||
# rarely, restart the service to probe again.
|
||||
modem_missing_count += 1
|
||||
if modem_missing_count > 3:
|
||||
modem_restarted = True
|
||||
cloudlog.event("restarting ModemManager")
|
||||
os.system("sudo systemctl restart --no-block ModemManager")
|
||||
|
||||
tx, rx = HARDWARE.get_modem_data_usage()
|
||||
|
||||
hw_state = HardwareState(
|
||||
network_type=network_type,
|
||||
network_info=HARDWARE.get_network_info(),
|
||||
network_strength=HARDWARE.get_network_strength(network_type),
|
||||
network_stats={'wwanTx': tx, 'wwanRx': rx},
|
||||
network_metered=HARDWARE.get_network_metered(network_type),
|
||||
nvme_temps=HARDWARE.get_nvme_temperatures(),
|
||||
modem_temps=modem_temps,
|
||||
)
|
||||
|
||||
try:
|
||||
hw_queue.put_nowait(hw_state)
|
||||
except queue.Full:
|
||||
pass
|
||||
|
||||
if not modem_configured and HARDWARE.get_modem_version() is not None:
|
||||
cloudlog.warning("configuring modem")
|
||||
HARDWARE.configure_modem()
|
||||
modem_configured = True
|
||||
|
||||
prev_hw_state = hw_state
|
||||
except Exception:
|
||||
cloudlog.exception("Error getting hardware state")
|
||||
|
||||
count += 1
|
||||
time.sleep(DT_HW)
|
||||
|
||||
|
||||
def hardware_thread(end_event, hw_queue) -> None:
|
||||
pm = messaging.PubMaster(['deviceState'])
|
||||
sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates"], poll="pandaStates")
|
||||
|
||||
count = 0
|
||||
|
||||
onroad_conditions: dict[str, bool] = {
|
||||
"ignition": False,
|
||||
}
|
||||
startup_conditions: dict[str, bool] = {}
|
||||
startup_conditions_prev: dict[str, bool] = {}
|
||||
|
||||
off_ts: float | None = None
|
||||
started_ts: float | None = None
|
||||
started_seen = False
|
||||
startup_blocked_ts: float | None = None
|
||||
thermal_status = ThermalStatus.yellow
|
||||
|
||||
last_hw_state = HardwareState(
|
||||
network_type=NetworkType.none,
|
||||
network_info=None,
|
||||
network_metered=False,
|
||||
network_strength=NetworkStrength.unknown,
|
||||
network_stats={'wwanTx': -1, 'wwanRx': -1},
|
||||
nvme_temps=[],
|
||||
modem_temps=[],
|
||||
)
|
||||
|
||||
all_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_HW, initialized=False)
|
||||
offroad_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_HW, initialized=False)
|
||||
should_start_prev = False
|
||||
in_car = False
|
||||
engaged_prev = False
|
||||
|
||||
params = Params()
|
||||
power_monitor = PowerMonitoring()
|
||||
|
||||
HARDWARE.initialize_hardware()
|
||||
thermal_config = HARDWARE.get_thermal_config()
|
||||
|
||||
fan_controller = None
|
||||
|
||||
dp_device_go_off_road = False
|
||||
|
||||
while not end_event.is_set():
|
||||
sm.update(PANDA_STATES_TIMEOUT)
|
||||
|
||||
pandaStates = sm['pandaStates']
|
||||
peripheralState = sm['peripheralState']
|
||||
peripheral_panda_present = peripheralState.pandaType != log.PandaState.PandaType.unknown
|
||||
|
||||
if sm.updated['pandaStates'] and len(pandaStates) > 0:
|
||||
|
||||
# Set ignition based on any panda connected
|
||||
onroad_conditions["ignition"] = any(ps.ignitionLine or ps.ignitionCan for ps in pandaStates if ps.pandaType != log.PandaState.PandaType.unknown)
|
||||
|
||||
pandaState = pandaStates[0]
|
||||
|
||||
in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected
|
||||
|
||||
# Setup fan handler on first connect to panda
|
||||
if fan_controller is None and peripheral_panda_present:
|
||||
if TICI:
|
||||
fan_controller = TiciFanController()
|
||||
|
||||
elif (time.monotonic() - sm.recv_time['pandaStates']) > DISCONNECT_TIMEOUT:
|
||||
if onroad_conditions["ignition"]:
|
||||
onroad_conditions["ignition"] = False
|
||||
cloudlog.error("panda timed out onroad")
|
||||
|
||||
# Run at 2Hz, plus either edge of ignition
|
||||
ign_edge = (started_ts is not None) != onroad_conditions["ignition"]
|
||||
if (sm.frame % round(SERVICE_LIST['pandaStates'].frequency * DT_HW) != 0) and not ign_edge:
|
||||
continue
|
||||
|
||||
msg = messaging.new_message('deviceState', valid=True)
|
||||
msg.deviceState = thermal_config.get_msg()
|
||||
msg.deviceState.deviceType = HARDWARE.get_device_type()
|
||||
|
||||
try:
|
||||
last_hw_state = hw_queue.get_nowait()
|
||||
except queue.Empty:
|
||||
pass
|
||||
|
||||
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
|
||||
msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent))
|
||||
msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent()))
|
||||
online_cpu_usage = [int(round(n)) for n in psutil.cpu_percent(percpu=True)]
|
||||
offline_cpu_usage = [0., ] * (len(msg.deviceState.cpuTempC) - len(online_cpu_usage))
|
||||
msg.deviceState.cpuUsagePercent = online_cpu_usage + offline_cpu_usage
|
||||
|
||||
msg.deviceState.networkType = last_hw_state.network_type
|
||||
msg.deviceState.networkMetered = last_hw_state.network_metered
|
||||
msg.deviceState.networkStrength = last_hw_state.network_strength
|
||||
msg.deviceState.networkStats = last_hw_state.network_stats
|
||||
if last_hw_state.network_info is not None:
|
||||
msg.deviceState.networkInfo = last_hw_state.network_info
|
||||
|
||||
msg.deviceState.nvmeTempC = last_hw_state.nvme_temps
|
||||
msg.deviceState.modemTempC = last_hw_state.modem_temps
|
||||
|
||||
msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness()
|
||||
|
||||
# this subset is only used for offroad
|
||||
temp_sources = [
|
||||
msg.deviceState.memoryTempC,
|
||||
max(msg.deviceState.cpuTempC, default=0.),
|
||||
max(msg.deviceState.gpuTempC, default=0.),
|
||||
]
|
||||
offroad_comp_temp = offroad_temp_filter.update(max(temp_sources))
|
||||
|
||||
# this drives the thermal status while onroad
|
||||
temp_sources.append(max(msg.deviceState.pmicTempC, default=0.))
|
||||
all_comp_temp = all_temp_filter.update(max(temp_sources))
|
||||
msg.deviceState.maxTempC = all_comp_temp
|
||||
|
||||
if fan_controller is not None:
|
||||
msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"])
|
||||
|
||||
is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (time.monotonic() - off_ts > 60 * 5))
|
||||
if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP:
|
||||
# if device is offroad and already hot without the extra onroad load,
|
||||
# we want to cool down first before increasing load
|
||||
thermal_status = ThermalStatus.danger
|
||||
else:
|
||||
current_band = THERMAL_BANDS[thermal_status]
|
||||
band_idx = list(THERMAL_BANDS.keys()).index(thermal_status)
|
||||
if current_band.min_temp is not None and all_comp_temp < current_band.min_temp:
|
||||
thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1]
|
||||
elif current_band.max_temp is not None and all_comp_temp > current_band.max_temp:
|
||||
thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1]
|
||||
|
||||
# **** starting logic ****
|
||||
|
||||
startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate")
|
||||
startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
|
||||
startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version
|
||||
|
||||
# with 2% left, we killall, otherwise the phone will take a long time to boot
|
||||
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
|
||||
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version
|
||||
startup_conditions["not_driver_view"] = not params.get_bool("IsDriverViewEnabled")
|
||||
startup_conditions["not_taking_snapshot"] = not params.get_bool("IsTakingSnapshot")
|
||||
|
||||
# must be at an engageable thermal band to go onroad
|
||||
startup_conditions["device_temp_engageable"] = thermal_status < ThermalStatus.red
|
||||
|
||||
# ensure device is fully booted
|
||||
startup_conditions["device_booted"] = startup_conditions.get("device_booted", False) or HARDWARE.booted()
|
||||
|
||||
# if the temperature enters the danger zone, go offroad to cool down
|
||||
onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
|
||||
extra_text = f"{offroad_comp_temp:.1f}C"
|
||||
show_alert = (not onroad_conditions["device_temp_good"] or not startup_conditions["device_temp_engageable"]) and onroad_conditions["ignition"]
|
||||
set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", show_alert, extra_text=extra_text)
|
||||
|
||||
# TODO: this should move to TICI.initialize_hardware, but we currently can't import params there
|
||||
if TICI and HARDWARE.get_device_type() == "tici" and not os.getenv("DISABLE_DRIVER"):
|
||||
if not os.path.isfile("/persist/comma/living-in-the-moment"):
|
||||
if not Path("/data/media").is_mount():
|
||||
set_offroad_alert_if_changed("Offroad_StorageMissing", True)
|
||||
else:
|
||||
# check for bad NVMe
|
||||
try:
|
||||
with open("/sys/block/nvme0n1/device/model") as f:
|
||||
model = f.read().strip()
|
||||
if not model.startswith("Samsung SSD 980") and params.get("Offroad_BadNvme") is None:
|
||||
set_offroad_alert_if_changed("Offroad_BadNvme", True)
|
||||
cloudlog.event("Unsupported NVMe", model=model, error=True)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# Handle offroad/onroad transition
|
||||
if count % 6 == 0:
|
||||
dp_device_go_off_road = params.get_bool("dp_device_go_off_road")
|
||||
should_start = not dp_device_go_off_road and all(onroad_conditions.values())
|
||||
if started_ts is None:
|
||||
should_start = should_start and all(startup_conditions.values())
|
||||
|
||||
if should_start != should_start_prev or (count == 0):
|
||||
params.put_bool("IsEngaged", False)
|
||||
engaged_prev = False
|
||||
HARDWARE.set_power_save(not should_start)
|
||||
|
||||
if sm.updated['selfdriveState']:
|
||||
engaged = sm['selfdriveState'].enabled
|
||||
if engaged != engaged_prev:
|
||||
params.put_bool("IsEngaged", engaged)
|
||||
engaged_prev = engaged
|
||||
|
||||
try:
|
||||
with open('/dev/kmsg', 'w') as kmsg:
|
||||
kmsg.write(f"<3>[hardware] engaged: {engaged}\n")
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
if should_start:
|
||||
off_ts = None
|
||||
if started_ts is None:
|
||||
started_ts = time.monotonic()
|
||||
started_seen = True
|
||||
if startup_blocked_ts is not None:
|
||||
cloudlog.event("Startup after block", block_duration=(time.monotonic() - startup_blocked_ts),
|
||||
startup_conditions=startup_conditions, onroad_conditions=onroad_conditions,
|
||||
startup_conditions_prev=startup_conditions_prev, error=True)
|
||||
startup_blocked_ts = None
|
||||
else:
|
||||
if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev):
|
||||
cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions, error=True)
|
||||
startup_conditions_prev = startup_conditions.copy()
|
||||
startup_blocked_ts = time.monotonic()
|
||||
|
||||
started_ts = None
|
||||
if off_ts is None:
|
||||
off_ts = time.monotonic()
|
||||
|
||||
# Offroad power monitoring
|
||||
voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage
|
||||
power_monitor.calculate(voltage, onroad_conditions["ignition"])
|
||||
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
|
||||
msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
|
||||
current_power_draw = HARDWARE.get_current_power_draw()
|
||||
statlog.sample("power_draw", current_power_draw)
|
||||
msg.deviceState.powerDrawW = current_power_draw
|
||||
|
||||
som_power_draw = HARDWARE.get_som_power_draw()
|
||||
statlog.sample("som_power_draw", som_power_draw)
|
||||
msg.deviceState.somPowerDrawW = som_power_draw
|
||||
|
||||
# Check if we need to shut down
|
||||
if power_monitor.should_shutdown(onroad_conditions["ignition"], in_car, off_ts, started_seen):
|
||||
cloudlog.warning(f"shutting device down, offroad since {off_ts}")
|
||||
params.put_bool("DoShutdown", True)
|
||||
|
||||
msg.deviceState.started = started_ts is not None
|
||||
msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0))
|
||||
|
||||
last_ping = params.get("LastAthenaPingTime")
|
||||
if last_ping is not None:
|
||||
msg.deviceState.lastAthenaPingTime = int(last_ping)
|
||||
|
||||
msg.deviceState.thermalStatus = thermal_status
|
||||
pm.send("deviceState", msg)
|
||||
|
||||
# Log to statsd
|
||||
statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent)
|
||||
statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent)
|
||||
statlog.gauge("memory_usage_percent", msg.deviceState.memoryUsagePercent)
|
||||
for i, usage in enumerate(msg.deviceState.cpuUsagePercent):
|
||||
statlog.gauge(f"cpu{i}_usage_percent", usage)
|
||||
for i, temp in enumerate(msg.deviceState.cpuTempC):
|
||||
statlog.gauge(f"cpu{i}_temperature", temp)
|
||||
for i, temp in enumerate(msg.deviceState.gpuTempC):
|
||||
statlog.gauge(f"gpu{i}_temperature", temp)
|
||||
statlog.gauge("memory_temperature", msg.deviceState.memoryTempC)
|
||||
for i, temp in enumerate(msg.deviceState.pmicTempC):
|
||||
statlog.gauge(f"pmic{i}_temperature", temp)
|
||||
for i, temp in enumerate(last_hw_state.nvme_temps):
|
||||
statlog.gauge(f"nvme_temperature{i}", temp)
|
||||
for i, temp in enumerate(last_hw_state.modem_temps):
|
||||
statlog.gauge(f"modem_temperature{i}", temp)
|
||||
statlog.gauge("fan_speed_percent_desired", msg.deviceState.fanSpeedPercentDesired)
|
||||
statlog.gauge("screen_brightness_percent", msg.deviceState.screenBrightnessPercent)
|
||||
|
||||
# report to server once every 10 minutes
|
||||
rising_edge_started = should_start and not should_start_prev
|
||||
if rising_edge_started or (count % int(600. / DT_HW)) == 0:
|
||||
dat = {
|
||||
'count': count,
|
||||
'pandaStates': [strip_deprecated_keys(p.to_dict()) for p in pandaStates],
|
||||
'peripheralState': strip_deprecated_keys(peripheralState.to_dict()),
|
||||
'location': (strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None),
|
||||
'deviceState': strip_deprecated_keys(msg.to_dict())
|
||||
}
|
||||
cloudlog.event("STATUS_PACKET", **dat)
|
||||
|
||||
# save last one before going onroad
|
||||
if rising_edge_started:
|
||||
try:
|
||||
params.put("LastOffroadStatusPacket", json.dumps(dat))
|
||||
except Exception:
|
||||
cloudlog.exception("failed to save offroad status")
|
||||
|
||||
params.put_bool_nonblocking("NetworkMetered", msg.deviceState.networkMetered)
|
||||
|
||||
count += 1
|
||||
should_start_prev = should_start
|
||||
|
||||
|
||||
def main():
|
||||
hw_queue = queue.Queue(maxsize=1)
|
||||
end_event = threading.Event()
|
||||
|
||||
threads = [
|
||||
threading.Thread(target=hw_state_thread, args=(end_event, hw_queue)),
|
||||
threading.Thread(target=hardware_thread, args=(end_event, hw_queue)),
|
||||
]
|
||||
|
||||
if TICI:
|
||||
threads.append(threading.Thread(target=touch_thread, args=(end_event,)))
|
||||
|
||||
for t in threads:
|
||||
t.start()
|
||||
|
||||
try:
|
||||
while True:
|
||||
time.sleep(1)
|
||||
if not all(t.is_alive() for t in threads):
|
||||
break
|
||||
finally:
|
||||
end_event.set()
|
||||
|
||||
for t in threads:
|
||||
t.join()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,58 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "system/hardware/base.h"
|
||||
#include "common/util.h"
|
||||
|
||||
#if QCOM2
|
||||
#include "system/hardware/tici/hardware.h"
|
||||
#define Hardware HardwareTici
|
||||
#else
|
||||
#include "system/hardware/pc/hardware.h"
|
||||
#define Hardware HardwarePC
|
||||
#endif
|
||||
|
||||
namespace Path {
|
||||
inline std::string openpilot_prefix() {
|
||||
return util::getenv("OPENPILOT_PREFIX", "");
|
||||
}
|
||||
|
||||
inline std::string comma_home() {
|
||||
return util::getenv("HOME") + "/.comma" + Path::openpilot_prefix();
|
||||
}
|
||||
|
||||
inline std::string log_root() {
|
||||
if (const char *env = getenv("LOG_ROOT")) {
|
||||
return env;
|
||||
}
|
||||
return Hardware::PC() ? Path::comma_home() + "/media/0/realdata" : "/data/media/0/realdata";
|
||||
}
|
||||
|
||||
inline std::string params() {
|
||||
return util::getenv("PARAMS_ROOT", Hardware::PC() ? (Path::comma_home() + "/params") : "/data/params");
|
||||
}
|
||||
|
||||
inline std::string rsa_file() {
|
||||
return Hardware::PC() ? Path::comma_home() + "/persist/comma/id_rsa" : "/persist/comma/id_rsa";
|
||||
}
|
||||
|
||||
inline std::string swaglog_ipc() {
|
||||
return "ipc:///tmp/logmessage" + Path::openpilot_prefix();
|
||||
}
|
||||
|
||||
inline std::string download_cache_root() {
|
||||
if (const char *env = getenv("COMMA_CACHE")) {
|
||||
return env;
|
||||
}
|
||||
return "/tmp/comma_download_cache" + Path::openpilot_prefix() + "/";
|
||||
}
|
||||
|
||||
inline std::string shm_path() {
|
||||
#ifdef __APPLE__
|
||||
return"/tmp";
|
||||
#else
|
||||
return "/dev/shm";
|
||||
#endif
|
||||
}
|
||||
} // namespace Path
|
||||
@@ -0,0 +1,65 @@
|
||||
import os
|
||||
import platform
|
||||
from pathlib import Path
|
||||
|
||||
from openpilot.system.hardware import PC
|
||||
|
||||
DEFAULT_DOWNLOAD_CACHE_ROOT = "/tmp/comma_download_cache"
|
||||
|
||||
class Paths:
|
||||
@staticmethod
|
||||
def comma_home() -> str:
|
||||
return os.path.join(str(Path.home()), ".comma" + os.environ.get("OPENPILOT_PREFIX", ""))
|
||||
|
||||
@staticmethod
|
||||
def log_root() -> str:
|
||||
if os.environ.get('LOG_ROOT', False):
|
||||
return os.environ['LOG_ROOT']
|
||||
elif PC:
|
||||
return str(Path(Paths.comma_home()) / "media" / "0" / "realdata")
|
||||
else:
|
||||
return '/data/media/0/realdata/'
|
||||
|
||||
@staticmethod
|
||||
def swaglog_root() -> str:
|
||||
if PC:
|
||||
return os.path.join(Paths.comma_home(), "log")
|
||||
else:
|
||||
return "/data/log/"
|
||||
|
||||
@staticmethod
|
||||
def swaglog_ipc() -> str:
|
||||
return "ipc:///tmp/logmessage" + os.environ.get("OPENPILOT_PREFIX", "")
|
||||
|
||||
@staticmethod
|
||||
def download_cache_root() -> str:
|
||||
if os.environ.get('COMMA_CACHE', False):
|
||||
return os.environ['COMMA_CACHE'] + "/"
|
||||
return DEFAULT_DOWNLOAD_CACHE_ROOT + os.environ.get("OPENPILOT_PREFIX", "") + "/"
|
||||
|
||||
@staticmethod
|
||||
def persist_root() -> str:
|
||||
if PC:
|
||||
return os.path.join(Paths.comma_home(), "persist")
|
||||
else:
|
||||
return "/persist/"
|
||||
|
||||
@staticmethod
|
||||
def stats_root() -> str:
|
||||
if PC:
|
||||
return str(Path(Paths.comma_home()) / "stats")
|
||||
else:
|
||||
return "/data/stats/"
|
||||
|
||||
@staticmethod
|
||||
def config_root() -> str:
|
||||
if PC:
|
||||
return Paths.comma_home()
|
||||
else:
|
||||
return "/tmp/.comma"
|
||||
|
||||
@staticmethod
|
||||
def shm_path() -> str:
|
||||
if PC and platform.system() == "Darwin":
|
||||
return "/tmp" # This is not really shared memory on macOS, but it's the closest we can get
|
||||
return "/dev/shm"
|
||||
@@ -0,0 +1,15 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "system/hardware/base.h"
|
||||
|
||||
class HardwarePC : public HardwareNone {
|
||||
public:
|
||||
static std::string get_os_version() { return "openpilot for PC"; }
|
||||
static std::string get_name() { return "pc"; }
|
||||
static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; }
|
||||
static bool PC() { return true; }
|
||||
static bool TICI() { return util::getenv("TICI", 0) == 1; }
|
||||
static bool AGNOS() { return util::getenv("TICI", 0) == 1; }
|
||||
};
|
||||
@@ -0,0 +1,78 @@
|
||||
import random
|
||||
|
||||
from cereal import log
|
||||
from openpilot.system.hardware.base import HardwareBase
|
||||
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
NetworkStrength = log.DeviceState.NetworkStrength
|
||||
|
||||
|
||||
class Pc(HardwareBase):
|
||||
def get_os_version(self):
|
||||
return None
|
||||
|
||||
def get_device_type(self):
|
||||
return "pc"
|
||||
|
||||
def reboot(self, reason=None):
|
||||
print("REBOOT!")
|
||||
|
||||
def uninstall(self):
|
||||
print("uninstall")
|
||||
|
||||
def get_imei(self, slot):
|
||||
return f"{random.randint(0, 1 << 32):015d}"
|
||||
|
||||
def get_serial(self):
|
||||
return "cccccccc"
|
||||
|
||||
def get_network_info(self):
|
||||
return None
|
||||
|
||||
def get_network_type(self):
|
||||
return NetworkType.wifi
|
||||
|
||||
def get_sim_info(self):
|
||||
return {
|
||||
'sim_id': '',
|
||||
'mcc_mnc': None,
|
||||
'network_type': ["Unknown"],
|
||||
'sim_state': ["ABSENT"],
|
||||
'data_connected': False
|
||||
}
|
||||
|
||||
def get_network_strength(self, network_type):
|
||||
return NetworkStrength.unknown
|
||||
|
||||
def get_current_power_draw(self):
|
||||
return 0
|
||||
|
||||
def get_som_power_draw(self):
|
||||
return 0
|
||||
|
||||
def shutdown(self):
|
||||
print("SHUTDOWN!")
|
||||
|
||||
def set_screen_brightness(self, percentage):
|
||||
pass
|
||||
|
||||
def get_screen_brightness(self):
|
||||
return 0
|
||||
|
||||
def set_power_save(self, powersave_enabled):
|
||||
pass
|
||||
|
||||
def get_gpu_usage_percent(self):
|
||||
return 0
|
||||
|
||||
def get_modem_temperatures(self):
|
||||
return []
|
||||
|
||||
def get_nvme_temperatures(self):
|
||||
return []
|
||||
|
||||
def initialize_hardware(self):
|
||||
pass
|
||||
|
||||
def get_networks(self):
|
||||
return None
|
||||
@@ -0,0 +1,132 @@
|
||||
import time
|
||||
import threading
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.statsd import statlog
|
||||
|
||||
CAR_VOLTAGE_LOW_PASS_K = 0.011 # LPF gain for 45s tau (dt/tau / (dt/tau + 1))
|
||||
|
||||
# While driving, a battery charges completely in about 30-60 minutes
|
||||
CAR_BATTERY_CAPACITY_uWh = 30e6
|
||||
CAR_CHARGING_RATE_W = 45
|
||||
|
||||
VBATT_PAUSE_CHARGING = 11.8 # Lower limit on the LPF car battery voltage
|
||||
MAX_TIME_OFFROAD_S = 30*3600
|
||||
MIN_ON_TIME_S = 3600
|
||||
DELAY_SHUTDOWN_TIME_S = 300 # Wait at least DELAY_SHUTDOWN_TIME_S seconds after offroad_time to shutdown.
|
||||
VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S = 60
|
||||
|
||||
class PowerMonitoring:
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.last_measurement_time = None # Used for integration delta
|
||||
self.last_save_time = 0 # Used for saving current value in a param
|
||||
self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad
|
||||
self.next_pulsed_measurement_time = None
|
||||
self.car_voltage_mV = 12e3 # Low-passed version of peripheralState voltage
|
||||
self.car_voltage_instant_mV = 12e3 # Last value of peripheralState voltage
|
||||
self.integration_lock = threading.Lock()
|
||||
self.dp_device_auto_shutdown_in = int(self.params.get("dp_device_auto_shutdown_in") or -5) * 60
|
||||
self.dp_device_auto_shutdown = self.dp_device_auto_shutdown_in >= 0
|
||||
|
||||
car_battery_capacity_uWh = self.params.get("CarBatteryCapacity")
|
||||
if car_battery_capacity_uWh is None:
|
||||
car_battery_capacity_uWh = 0
|
||||
|
||||
# Reset capacity if it's low
|
||||
self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh))
|
||||
|
||||
# Calculation tick
|
||||
def calculate(self, voltage: int | None, ignition: bool):
|
||||
try:
|
||||
now = time.monotonic()
|
||||
|
||||
# If peripheralState is None, we're probably not in a car, so we don't care
|
||||
if voltage is None:
|
||||
with self.integration_lock:
|
||||
self.last_measurement_time = None
|
||||
self.next_pulsed_measurement_time = None
|
||||
self.power_used_uWh = 0
|
||||
return
|
||||
|
||||
# Low-pass battery voltage
|
||||
self.car_voltage_instant_mV = voltage
|
||||
self.car_voltage_mV = ((voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K)))
|
||||
statlog.gauge("car_voltage", self.car_voltage_mV / 1e3)
|
||||
|
||||
# Cap the car battery power and save it in a param every 10-ish seconds
|
||||
self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh, 0)
|
||||
self.car_battery_capacity_uWh = min(self.car_battery_capacity_uWh, CAR_BATTERY_CAPACITY_uWh)
|
||||
if now - self.last_save_time >= 10:
|
||||
self.params.put_nonblocking("CarBatteryCapacity", str(int(self.car_battery_capacity_uWh)))
|
||||
self.last_save_time = now
|
||||
|
||||
# First measurement, set integration time
|
||||
with self.integration_lock:
|
||||
if self.last_measurement_time is None:
|
||||
self.last_measurement_time = now
|
||||
return
|
||||
|
||||
if ignition:
|
||||
# If there is ignition, we integrate the charging rate of the car
|
||||
with self.integration_lock:
|
||||
self.power_used_uWh = 0
|
||||
integration_time_h = (now - self.last_measurement_time) / 3600
|
||||
if integration_time_h < 0:
|
||||
raise ValueError(f"Negative integration time: {integration_time_h}h")
|
||||
self.car_battery_capacity_uWh += (CAR_CHARGING_RATE_W * 1e6 * integration_time_h)
|
||||
self.last_measurement_time = now
|
||||
else:
|
||||
# Get current power draw somehow
|
||||
current_power = HARDWARE.get_current_power_draw()
|
||||
|
||||
# Do the integration
|
||||
self._perform_integration(now, current_power)
|
||||
except Exception:
|
||||
cloudlog.exception("Power monitoring calculation failed")
|
||||
|
||||
def _perform_integration(self, t: float, current_power: float) -> None:
|
||||
with self.integration_lock:
|
||||
try:
|
||||
if self.last_measurement_time:
|
||||
integration_time_h = (t - self.last_measurement_time) / 3600
|
||||
power_used = (current_power * 1000000) * integration_time_h
|
||||
if power_used < 0:
|
||||
raise ValueError(f"Negative power used! Integration time: {integration_time_h} h Current Power: {power_used} uWh")
|
||||
self.power_used_uWh += power_used
|
||||
self.car_battery_capacity_uWh -= power_used
|
||||
self.last_measurement_time = t
|
||||
except Exception:
|
||||
cloudlog.exception("Integration failed")
|
||||
|
||||
# Get the power usage
|
||||
def get_power_used(self) -> int:
|
||||
return int(self.power_used_uWh)
|
||||
|
||||
def get_car_battery_capacity(self) -> int:
|
||||
return int(self.car_battery_capacity_uWh)
|
||||
|
||||
# See if we need to shutdown
|
||||
def should_shutdown(self, ignition: bool, in_car: bool, offroad_timestamp: float | None, started_seen: bool):
|
||||
if offroad_timestamp is None:
|
||||
return False
|
||||
|
||||
now = time.monotonic()
|
||||
should_shutdown = False
|
||||
offroad_time = (now - offroad_timestamp)
|
||||
if started_seen and self.dp_device_auto_shutdown and offroad_time > self.dp_device_auto_shutdown_in:
|
||||
return True
|
||||
low_voltage_shutdown = (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3) and
|
||||
offroad_time > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S)
|
||||
should_shutdown |= offroad_time > MAX_TIME_OFFROAD_S
|
||||
should_shutdown |= low_voltage_shutdown
|
||||
should_shutdown |= (self.car_battery_capacity_uWh <= 0)
|
||||
should_shutdown &= not ignition
|
||||
should_shutdown &= (not self.params.get_bool("DisablePowerDown"))
|
||||
should_shutdown &= in_car
|
||||
should_shutdown &= offroad_time > DELAY_SHUTDOWN_TIME_S
|
||||
should_shutdown |= self.params.get_bool("ForcePowerDown")
|
||||
should_shutdown &= started_seen or (now > MIN_ON_TIME_S)
|
||||
return should_shutdown
|
||||
@@ -0,0 +1,84 @@
|
||||
[
|
||||
{
|
||||
"name": "xbl",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/xbl-468f1ad6ab55e198647ff9191f91bd2918db9c0a3e27bae5673b4c5575c1254c.img.xz",
|
||||
"hash": "468f1ad6ab55e198647ff9191f91bd2918db9c0a3e27bae5673b4c5575c1254c",
|
||||
"hash_raw": "468f1ad6ab55e198647ff9191f91bd2918db9c0a3e27bae5673b4c5575c1254c",
|
||||
"size": 3282256,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "d35a86e7b8ddd9279b513a6f27da1521aa0f89fb93987ea74d57d0f0bbbbd247"
|
||||
},
|
||||
{
|
||||
"name": "xbl_config",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/xbl_config-92b675dc2862ed15c732d91d9eb307d7e852e349217db8bee8f8829db543686b.img.xz",
|
||||
"hash": "92b675dc2862ed15c732d91d9eb307d7e852e349217db8bee8f8829db543686b",
|
||||
"hash_raw": "92b675dc2862ed15c732d91d9eb307d7e852e349217db8bee8f8829db543686b",
|
||||
"size": 98124,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "623f1568072ee2d687ba8449a3d894c1c83dc4131b2e79eff35696885f70a419"
|
||||
},
|
||||
{
|
||||
"name": "abl",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz",
|
||||
"hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
|
||||
"hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
|
||||
"size": 274432,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6"
|
||||
},
|
||||
{
|
||||
"name": "aop",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/aop-f0fcf7611d0890a72984f15a516dd37fa532dfcb70d428a8406838cf74ce23d5.img.xz",
|
||||
"hash": "f0fcf7611d0890a72984f15a516dd37fa532dfcb70d428a8406838cf74ce23d5",
|
||||
"hash_raw": "f0fcf7611d0890a72984f15a516dd37fa532dfcb70d428a8406838cf74ce23d5",
|
||||
"size": 184364,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "bf74feca486f650589f6b7c90eab73274e35a68b5e00bfc1de0ed5f5484d4b3d"
|
||||
},
|
||||
{
|
||||
"name": "devcfg",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/devcfg-225b24ea7b1d2fee7f7d2da21386920ddacac2e33e9e938168436292f4eae180.img.xz",
|
||||
"hash": "225b24ea7b1d2fee7f7d2da21386920ddacac2e33e9e938168436292f4eae180",
|
||||
"hash_raw": "225b24ea7b1d2fee7f7d2da21386920ddacac2e33e9e938168436292f4eae180",
|
||||
"size": 40336,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "70f682b59ca0fe2f197d1486bd8be7b9b7e560798ad40ddef83b9f0a2f497938"
|
||||
},
|
||||
{
|
||||
"name": "boot",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/boot-9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892.img.xz",
|
||||
"hash": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892",
|
||||
"hash_raw": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892",
|
||||
"size": 18479104,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "41d31b862fec1b87879b508c405adb9d7b5c0a3324f7350bd904f451605b06cf"
|
||||
},
|
||||
{
|
||||
"name": "system",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img.xz",
|
||||
"hash": "c56256a64e6d7e16886e39a4263ffb686ed0f03d3a665c3552f54a39723f8824",
|
||||
"hash_raw": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde",
|
||||
"size": 4404019200,
|
||||
"sparse": true,
|
||||
"full_check": false,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "ed2e11f52beb8559223bf9fb989fd4ef5d2ce66eeb11ae0053fff8e41903a533",
|
||||
"alt": {
|
||||
"hash": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img",
|
||||
"size": 4404019200
|
||||
}
|
||||
}
|
||||
]
|
||||
Executable
+337
@@ -0,0 +1,337 @@
|
||||
#!/usr/bin/env python3
|
||||
import hashlib
|
||||
import json
|
||||
import lzma
|
||||
import os
|
||||
import struct
|
||||
import subprocess
|
||||
import time
|
||||
from collections.abc import Generator
|
||||
|
||||
import requests
|
||||
|
||||
import openpilot.system.updated.casync.casync as casync
|
||||
|
||||
SPARSE_CHUNK_FMT = struct.Struct('H2xI4x')
|
||||
CAIBX_URL = "https://commadist.azureedge.net/agnosupdate/"
|
||||
|
||||
AGNOS_MANIFEST_FILE = "system/hardware/tici/agnos.json"
|
||||
|
||||
|
||||
class StreamingDecompressor:
|
||||
def __init__(self, url: str) -> None:
|
||||
self.buf = b""
|
||||
|
||||
self.req = requests.get(url, stream=True, headers={'Accept-Encoding': None}, timeout=60)
|
||||
self.it = self.req.iter_content(chunk_size=1024 * 1024)
|
||||
self.decompressor = lzma.LZMADecompressor(format=lzma.FORMAT_AUTO)
|
||||
self.eof = False
|
||||
self.sha256 = hashlib.sha256()
|
||||
|
||||
def read(self, length: int) -> bytes:
|
||||
while len(self.buf) < length and not self.eof:
|
||||
if self.decompressor.needs_input:
|
||||
self.req.raise_for_status()
|
||||
|
||||
try:
|
||||
compressed = next(self.it)
|
||||
except StopIteration:
|
||||
self.eof = True
|
||||
break
|
||||
else:
|
||||
compressed = b''
|
||||
|
||||
self.buf += self.decompressor.decompress(compressed, max_length=length)
|
||||
|
||||
if self.decompressor.eof:
|
||||
self.eof = True
|
||||
break
|
||||
|
||||
result = self.buf[:length]
|
||||
self.buf = self.buf[length:]
|
||||
|
||||
self.sha256.update(result)
|
||||
return result
|
||||
|
||||
|
||||
def unsparsify(f: StreamingDecompressor) -> Generator[bytes, None, None]:
|
||||
# https://source.android.com/devices/bootloader/images#sparse-format
|
||||
magic = struct.unpack("I", f.read(4))[0]
|
||||
assert(magic == 0xed26ff3a)
|
||||
|
||||
# Version
|
||||
major = struct.unpack("H", f.read(2))[0]
|
||||
minor = struct.unpack("H", f.read(2))[0]
|
||||
assert(major == 1 and minor == 0)
|
||||
|
||||
f.read(2) # file header size
|
||||
f.read(2) # chunk header size
|
||||
|
||||
block_sz = struct.unpack("I", f.read(4))[0]
|
||||
f.read(4) # total blocks
|
||||
num_chunks = struct.unpack("I", f.read(4))[0]
|
||||
f.read(4) # crc checksum
|
||||
|
||||
for _ in range(num_chunks):
|
||||
chunk_type, out_blocks = SPARSE_CHUNK_FMT.unpack(f.read(12))
|
||||
|
||||
if chunk_type == 0xcac1: # Raw
|
||||
# TODO: yield in smaller chunks. Yielding only block_sz is too slow. Largest observed data chunk is 252 MB.
|
||||
yield f.read(out_blocks * block_sz)
|
||||
elif chunk_type == 0xcac2: # Fill
|
||||
filler = f.read(4) * (block_sz // 4)
|
||||
for _ in range(out_blocks):
|
||||
yield filler
|
||||
elif chunk_type == 0xcac3: # Don't care
|
||||
yield b""
|
||||
else:
|
||||
raise Exception("Unhandled sparse chunk type")
|
||||
|
||||
|
||||
# noop wrapper with same API as unsparsify() for non sparse images
|
||||
def noop(f: StreamingDecompressor) -> Generator[bytes, None, None]:
|
||||
while len(chunk := f.read(1024 * 1024)) > 0:
|
||||
yield chunk
|
||||
|
||||
|
||||
def get_target_slot_number() -> int:
|
||||
current_slot = subprocess.check_output(["abctl", "--boot_slot"], encoding='utf-8').strip()
|
||||
return 1 if current_slot == "_a" else 0
|
||||
|
||||
|
||||
def slot_number_to_suffix(slot_number: int) -> str:
|
||||
assert slot_number in (0, 1)
|
||||
return '_a' if slot_number == 0 else '_b'
|
||||
|
||||
|
||||
def get_partition_path(target_slot_number: int, partition: dict) -> str:
|
||||
path = f"/dev/disk/by-partlabel/{partition['name']}"
|
||||
|
||||
if partition.get('has_ab', True):
|
||||
path += slot_number_to_suffix(target_slot_number)
|
||||
|
||||
return path
|
||||
|
||||
|
||||
def get_raw_hash(path: str, partition_size: int) -> str:
|
||||
raw_hash = hashlib.sha256()
|
||||
pos, chunk_size = 0, 1024 * 1024
|
||||
|
||||
with open(path, 'rb+') as out:
|
||||
while pos < partition_size:
|
||||
n = min(chunk_size, partition_size - pos)
|
||||
raw_hash.update(out.read(n))
|
||||
pos += n
|
||||
|
||||
return raw_hash.hexdigest().lower()
|
||||
|
||||
|
||||
def verify_partition(target_slot_number: int, partition: dict[str, str | int], force_full_check: bool = False) -> bool:
|
||||
full_check = partition['full_check'] or force_full_check
|
||||
path = get_partition_path(target_slot_number, partition)
|
||||
|
||||
if not isinstance(partition['size'], int):
|
||||
return False
|
||||
|
||||
partition_size: int = partition['size']
|
||||
|
||||
if not isinstance(partition['hash_raw'], str):
|
||||
return False
|
||||
|
||||
partition_hash: str = partition['hash_raw']
|
||||
|
||||
if full_check:
|
||||
return get_raw_hash(path, partition_size) == partition_hash.lower()
|
||||
else:
|
||||
with open(path, 'rb+') as out:
|
||||
out.seek(partition_size)
|
||||
return out.read(64) == partition_hash.lower().encode()
|
||||
|
||||
|
||||
def clear_partition_hash(target_slot_number: int, partition: dict) -> None:
|
||||
path = get_partition_path(target_slot_number, partition)
|
||||
with open(path, 'wb+') as out:
|
||||
partition_size = partition['size']
|
||||
|
||||
out.seek(partition_size)
|
||||
out.write(b"\x00" * 64)
|
||||
os.sync()
|
||||
|
||||
|
||||
def extract_compressed_image(target_slot_number: int, partition: dict, cloudlog):
|
||||
path = get_partition_path(target_slot_number, partition)
|
||||
downloader = StreamingDecompressor(partition['url'])
|
||||
|
||||
with open(path, 'wb+') as out:
|
||||
# Flash partition
|
||||
last_p = 0
|
||||
raw_hash = hashlib.sha256()
|
||||
f = unsparsify if partition['sparse'] else noop
|
||||
for chunk in f(downloader):
|
||||
raw_hash.update(chunk)
|
||||
out.write(chunk)
|
||||
p = int(out.tell() / partition['size'] * 100)
|
||||
if p != last_p:
|
||||
last_p = p
|
||||
print(f"Installing {partition['name']}: {p}", flush=True)
|
||||
|
||||
if raw_hash.hexdigest().lower() != partition['hash_raw'].lower():
|
||||
raise Exception(f"Raw hash mismatch '{raw_hash.hexdigest().lower()}'")
|
||||
|
||||
if downloader.sha256.hexdigest().lower() != partition['hash'].lower():
|
||||
raise Exception("Uncompressed hash mismatch")
|
||||
|
||||
if out.tell() != partition['size']:
|
||||
raise Exception("Uncompressed size mismatch")
|
||||
|
||||
os.sync()
|
||||
|
||||
|
||||
def extract_casync_image(target_slot_number: int, partition: dict, cloudlog):
|
||||
path = get_partition_path(target_slot_number, partition)
|
||||
seed_path = path[:-1] + ('b' if path[-1] == 'a' else 'a')
|
||||
|
||||
target = casync.parse_caibx(partition['casync_caibx'])
|
||||
|
||||
sources: list[tuple[str, casync.ChunkReader, casync.ChunkDict]] = []
|
||||
|
||||
# First source is the current partition.
|
||||
try:
|
||||
raw_hash = get_raw_hash(seed_path, partition['size'])
|
||||
caibx_url = f"{CAIBX_URL}{partition['name']}-{raw_hash}.caibx"
|
||||
|
||||
try:
|
||||
cloudlog.info(f"casync fetching {caibx_url}")
|
||||
sources += [('seed', casync.FileChunkReader(seed_path), casync.build_chunk_dict(casync.parse_caibx(caibx_url)))]
|
||||
except requests.RequestException:
|
||||
cloudlog.error(f"casync failed to load {caibx_url}")
|
||||
except Exception:
|
||||
cloudlog.exception("casync failed to hash seed partition")
|
||||
|
||||
# Second source is the target partition, this allows for resuming
|
||||
sources += [('target', casync.FileChunkReader(path), casync.build_chunk_dict(target))]
|
||||
|
||||
# Finally we add the remote source to download any missing chunks
|
||||
sources += [('remote', casync.RemoteChunkReader(partition['casync_store']), casync.build_chunk_dict(target))]
|
||||
|
||||
last_p = 0
|
||||
|
||||
def progress(cur):
|
||||
nonlocal last_p
|
||||
p = int(cur / partition['size'] * 100)
|
||||
if p != last_p:
|
||||
last_p = p
|
||||
print(f"Installing {partition['name']}: {p}", flush=True)
|
||||
|
||||
stats = casync.extract(target, sources, path, progress)
|
||||
cloudlog.error(f'casync done {json.dumps(stats)}')
|
||||
|
||||
os.sync()
|
||||
if not verify_partition(target_slot_number, partition, force_full_check=True):
|
||||
raise Exception(f"Raw hash mismatch '{partition['hash_raw'].lower()}'")
|
||||
|
||||
|
||||
def flash_partition(target_slot_number: int, partition: dict, cloudlog, standalone=False):
|
||||
cloudlog.info(f"Downloading and writing {partition['name']}")
|
||||
|
||||
if verify_partition(target_slot_number, partition):
|
||||
cloudlog.info(f"Already flashed {partition['name']}")
|
||||
return
|
||||
|
||||
# Clear hash before flashing in case we get interrupted
|
||||
full_check = partition['full_check']
|
||||
if not full_check:
|
||||
clear_partition_hash(target_slot_number, partition)
|
||||
|
||||
path = get_partition_path(target_slot_number, partition)
|
||||
|
||||
if ('casync_caibx' in partition) and not standalone:
|
||||
extract_casync_image(target_slot_number, partition, cloudlog)
|
||||
else:
|
||||
extract_compressed_image(target_slot_number, partition, cloudlog)
|
||||
|
||||
# Write hash after successful flash
|
||||
if not full_check:
|
||||
with open(path, 'wb+') as out:
|
||||
out.seek(partition['size'])
|
||||
out.write(partition['hash_raw'].lower().encode())
|
||||
|
||||
|
||||
def swap(manifest_path: str, target_slot_number: int, cloudlog) -> None:
|
||||
update = json.load(open(manifest_path))
|
||||
for partition in update:
|
||||
if not partition.get('full_check', False):
|
||||
clear_partition_hash(target_slot_number, partition)
|
||||
|
||||
while True:
|
||||
out = subprocess.check_output(f"abctl --set_active {target_slot_number}", shell=True, stderr=subprocess.STDOUT, encoding='utf8')
|
||||
if ("No such file or directory" not in out) and ("lun as boot lun" in out):
|
||||
cloudlog.info(f"Swap successful {out}")
|
||||
break
|
||||
else:
|
||||
cloudlog.error(f"Swap failed {out}")
|
||||
|
||||
|
||||
def flash_agnos_update(manifest_path: str, target_slot_number: int, cloudlog, standalone=False) -> None:
|
||||
update = json.load(open(manifest_path))
|
||||
|
||||
cloudlog.info(f"Target slot {target_slot_number}")
|
||||
|
||||
# set target slot as unbootable
|
||||
os.system(f"abctl --set_unbootable {target_slot_number}")
|
||||
|
||||
for partition in update:
|
||||
success = False
|
||||
|
||||
for retries in range(10):
|
||||
try:
|
||||
flash_partition(target_slot_number, partition, cloudlog, standalone)
|
||||
success = True
|
||||
break
|
||||
|
||||
except requests.exceptions.RequestException:
|
||||
cloudlog.exception("Failed")
|
||||
cloudlog.info(f"Failed to download {partition['name']}, retrying ({retries})")
|
||||
time.sleep(10)
|
||||
|
||||
if not success:
|
||||
cloudlog.info(f"Failed to flash {partition['name']}, aborting")
|
||||
raise Exception("Maximum retries exceeded")
|
||||
|
||||
cloudlog.info(f"AGNOS ready on slot {target_slot_number}")
|
||||
|
||||
|
||||
def verify_agnos_update(manifest_path: str, target_slot_number: int) -> bool:
|
||||
update = json.load(open(manifest_path))
|
||||
return all(verify_partition(target_slot_number, partition) for partition in update)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import argparse
|
||||
import logging
|
||||
|
||||
parser = argparse.ArgumentParser(description="Flash and verify AGNOS update",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("--verify", action="store_true", help="Verify and perform swap if update ready")
|
||||
parser.add_argument("--swap", action="store_true", help="Verify and perform swap, downloads if necessary")
|
||||
parser.add_argument("manifest", help="Manifest json")
|
||||
args = parser.parse_args()
|
||||
|
||||
logging.basicConfig(level=logging.INFO)
|
||||
|
||||
target_slot_number = get_target_slot_number()
|
||||
if args.verify:
|
||||
if verify_agnos_update(args.manifest, target_slot_number):
|
||||
swap(args.manifest, target_slot_number, logging)
|
||||
exit(0)
|
||||
exit(1)
|
||||
elif args.swap:
|
||||
while not verify_agnos_update(args.manifest, target_slot_number):
|
||||
logging.error("Verification failed. Flashing AGNOS")
|
||||
flash_agnos_update(args.manifest, target_slot_number, logging, standalone=True)
|
||||
|
||||
logging.warning(f"Verification succeeded. Swapping to slot {target_slot_number}")
|
||||
swap(args.manifest, target_slot_number, logging)
|
||||
else:
|
||||
flash_agnos_update(args.manifest, target_slot_number, logging, standalone=True)
|
||||
@@ -0,0 +1,400 @@
|
||||
[
|
||||
{
|
||||
"name": "gpt_main_0",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/gpt_main_0-8928a31fd9ee20f8703649f89833eba9b55e84b6415e67799c777b163c95a0bd.img.xz",
|
||||
"hash": "8928a31fd9ee20f8703649f89833eba9b55e84b6415e67799c777b163c95a0bd",
|
||||
"hash_raw": "8928a31fd9ee20f8703649f89833eba9b55e84b6415e67799c777b163c95a0bd",
|
||||
"size": 24576,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "8928a31fd9ee20f8703649f89833eba9b55e84b6415e67799c777b163c95a0bd",
|
||||
"gpt": {
|
||||
"lun": 0,
|
||||
"start_sector": 0,
|
||||
"num_sectors": 6
|
||||
}
|
||||
},
|
||||
{
|
||||
"name": "gpt_main_1",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/gpt_main_1-fe8ef7653db588d7420a625920ca06927dfcb0ed8aff3e3a1c74a52a24398ba6.img.xz",
|
||||
"hash": "fe8ef7653db588d7420a625920ca06927dfcb0ed8aff3e3a1c74a52a24398ba6",
|
||||
"hash_raw": "fe8ef7653db588d7420a625920ca06927dfcb0ed8aff3e3a1c74a52a24398ba6",
|
||||
"size": 24576,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "fe8ef7653db588d7420a625920ca06927dfcb0ed8aff3e3a1c74a52a24398ba6",
|
||||
"gpt": {
|
||||
"lun": 1,
|
||||
"start_sector": 0,
|
||||
"num_sectors": 6
|
||||
}
|
||||
},
|
||||
{
|
||||
"name": "gpt_main_2",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/gpt_main_2-5ccfc7240c8cbfa2f1a018a2e376cf274a6baf858c9bfe71951d8e28cab53c21.img.xz",
|
||||
"hash": "5ccfc7240c8cbfa2f1a018a2e376cf274a6baf858c9bfe71951d8e28cab53c21",
|
||||
"hash_raw": "5ccfc7240c8cbfa2f1a018a2e376cf274a6baf858c9bfe71951d8e28cab53c21",
|
||||
"size": 24576,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "5ccfc7240c8cbfa2f1a018a2e376cf274a6baf858c9bfe71951d8e28cab53c21",
|
||||
"gpt": {
|
||||
"lun": 2,
|
||||
"start_sector": 0,
|
||||
"num_sectors": 6
|
||||
}
|
||||
},
|
||||
{
|
||||
"name": "gpt_main_3",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/gpt_main_3-c707979fa21e89519328f4f30c2b21c9c453401ca8303f914c1873d410a95159.img.xz",
|
||||
"hash": "c707979fa21e89519328f4f30c2b21c9c453401ca8303f914c1873d410a95159",
|
||||
"hash_raw": "c707979fa21e89519328f4f30c2b21c9c453401ca8303f914c1873d410a95159",
|
||||
"size": 24576,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "c707979fa21e89519328f4f30c2b21c9c453401ca8303f914c1873d410a95159",
|
||||
"gpt": {
|
||||
"lun": 3,
|
||||
"start_sector": 0,
|
||||
"num_sectors": 6
|
||||
}
|
||||
},
|
||||
{
|
||||
"name": "gpt_main_4",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/gpt_main_4-e9405dcd785dbe79412184e1894a9c51ab7deb33bb612166c4c42a3d2bf42a0e.img.xz",
|
||||
"hash": "e9405dcd785dbe79412184e1894a9c51ab7deb33bb612166c4c42a3d2bf42a0e",
|
||||
"hash_raw": "e9405dcd785dbe79412184e1894a9c51ab7deb33bb612166c4c42a3d2bf42a0e",
|
||||
"size": 24576,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "e9405dcd785dbe79412184e1894a9c51ab7deb33bb612166c4c42a3d2bf42a0e",
|
||||
"gpt": {
|
||||
"lun": 4,
|
||||
"start_sector": 0,
|
||||
"num_sectors": 6
|
||||
}
|
||||
},
|
||||
{
|
||||
"name": "gpt_main_5",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/gpt_main_5-21ae965f05b2fa8d02e04f1eb74718f9779864f6eacdeb859757d6435e8ccce3.img.xz",
|
||||
"hash": "21ae965f05b2fa8d02e04f1eb74718f9779864f6eacdeb859757d6435e8ccce3",
|
||||
"hash_raw": "21ae965f05b2fa8d02e04f1eb74718f9779864f6eacdeb859757d6435e8ccce3",
|
||||
"size": 24576,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "21ae965f05b2fa8d02e04f1eb74718f9779864f6eacdeb859757d6435e8ccce3",
|
||||
"gpt": {
|
||||
"lun": 5,
|
||||
"start_sector": 0,
|
||||
"num_sectors": 6
|
||||
}
|
||||
},
|
||||
{
|
||||
"name": "persist",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/persist-9814b07851292f510f3794b767489f38ab379a99f0ea75dc620ad2d3a496d54d.img.xz",
|
||||
"hash": "9814b07851292f510f3794b767489f38ab379a99f0ea75dc620ad2d3a496d54d",
|
||||
"hash_raw": "9814b07851292f510f3794b767489f38ab379a99f0ea75dc620ad2d3a496d54d",
|
||||
"size": 33554432,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "9814b07851292f510f3794b767489f38ab379a99f0ea75dc620ad2d3a496d54d"
|
||||
},
|
||||
{
|
||||
"name": "systemrw",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/systemrw-8ce150ca38ef64a0885fc2fe816e5b63bae8adb4df5d809c5b318e6996366c7e.img.xz",
|
||||
"hash": "8ce150ca38ef64a0885fc2fe816e5b63bae8adb4df5d809c5b318e6996366c7e",
|
||||
"hash_raw": "8ce150ca38ef64a0885fc2fe816e5b63bae8adb4df5d809c5b318e6996366c7e",
|
||||
"size": 16777216,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "8ce150ca38ef64a0885fc2fe816e5b63bae8adb4df5d809c5b318e6996366c7e"
|
||||
},
|
||||
{
|
||||
"name": "cache",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/cache-ebfbaaa2f96dc4e5fea4f126364e5bf5b3b44c12cbc753b62fdd8baab82f70b4.img.xz",
|
||||
"hash": "ebfbaaa2f96dc4e5fea4f126364e5bf5b3b44c12cbc753b62fdd8baab82f70b4",
|
||||
"hash_raw": "ebfbaaa2f96dc4e5fea4f126364e5bf5b3b44c12cbc753b62fdd8baab82f70b4",
|
||||
"size": 134217728,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "ebfbaaa2f96dc4e5fea4f126364e5bf5b3b44c12cbc753b62fdd8baab82f70b4"
|
||||
},
|
||||
{
|
||||
"name": "xbl",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/xbl-468f1ad6ab55e198647ff9191f91bd2918db9c0a3e27bae5673b4c5575c1254c.img.xz",
|
||||
"hash": "468f1ad6ab55e198647ff9191f91bd2918db9c0a3e27bae5673b4c5575c1254c",
|
||||
"hash_raw": "468f1ad6ab55e198647ff9191f91bd2918db9c0a3e27bae5673b4c5575c1254c",
|
||||
"size": 3282256,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "d35a86e7b8ddd9279b513a6f27da1521aa0f89fb93987ea74d57d0f0bbbbd247"
|
||||
},
|
||||
{
|
||||
"name": "xbl_config",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/xbl_config-92b675dc2862ed15c732d91d9eb307d7e852e349217db8bee8f8829db543686b.img.xz",
|
||||
"hash": "92b675dc2862ed15c732d91d9eb307d7e852e349217db8bee8f8829db543686b",
|
||||
"hash_raw": "92b675dc2862ed15c732d91d9eb307d7e852e349217db8bee8f8829db543686b",
|
||||
"size": 98124,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "623f1568072ee2d687ba8449a3d894c1c83dc4131b2e79eff35696885f70a419"
|
||||
},
|
||||
{
|
||||
"name": "abl",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz",
|
||||
"hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
|
||||
"hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
|
||||
"size": 274432,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6"
|
||||
},
|
||||
{
|
||||
"name": "aop",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/aop-f0fcf7611d0890a72984f15a516dd37fa532dfcb70d428a8406838cf74ce23d5.img.xz",
|
||||
"hash": "f0fcf7611d0890a72984f15a516dd37fa532dfcb70d428a8406838cf74ce23d5",
|
||||
"hash_raw": "f0fcf7611d0890a72984f15a516dd37fa532dfcb70d428a8406838cf74ce23d5",
|
||||
"size": 184364,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "bf74feca486f650589f6b7c90eab73274e35a68b5e00bfc1de0ed5f5484d4b3d"
|
||||
},
|
||||
{
|
||||
"name": "bluetooth",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/bluetooth-9bb766d2d2ce0cc4491664b3010fe1ef62f8ffc1e362d55f78e48c4141f75533.img.xz",
|
||||
"hash": "9bb766d2d2ce0cc4491664b3010fe1ef62f8ffc1e362d55f78e48c4141f75533",
|
||||
"hash_raw": "9bb766d2d2ce0cc4491664b3010fe1ef62f8ffc1e362d55f78e48c4141f75533",
|
||||
"size": 1048576,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "9bb766d2d2ce0cc4491664b3010fe1ef62f8ffc1e362d55f78e48c4141f75533"
|
||||
},
|
||||
{
|
||||
"name": "cmnlib64",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/cmnlib64-1a876bd151bb9635f18719c4a17f953079de6e11d3eaec800968fc75669e0dc3.img.xz",
|
||||
"hash": "1a876bd151bb9635f18719c4a17f953079de6e11d3eaec800968fc75669e0dc3",
|
||||
"hash_raw": "1a876bd151bb9635f18719c4a17f953079de6e11d3eaec800968fc75669e0dc3",
|
||||
"size": 524288,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "1a876bd151bb9635f18719c4a17f953079de6e11d3eaec800968fc75669e0dc3"
|
||||
},
|
||||
{
|
||||
"name": "cmnlib",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/cmnlib-63df823e8a5fae01d66cb2b8c20f0d2ddb5c5f2425e5d0992a64676273ba1c82.img.xz",
|
||||
"hash": "63df823e8a5fae01d66cb2b8c20f0d2ddb5c5f2425e5d0992a64676273ba1c82",
|
||||
"hash_raw": "63df823e8a5fae01d66cb2b8c20f0d2ddb5c5f2425e5d0992a64676273ba1c82",
|
||||
"size": 524288,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "63df823e8a5fae01d66cb2b8c20f0d2ddb5c5f2425e5d0992a64676273ba1c82"
|
||||
},
|
||||
{
|
||||
"name": "devcfg",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/devcfg-225b24ea7b1d2fee7f7d2da21386920ddacac2e33e9e938168436292f4eae180.img.xz",
|
||||
"hash": "225b24ea7b1d2fee7f7d2da21386920ddacac2e33e9e938168436292f4eae180",
|
||||
"hash_raw": "225b24ea7b1d2fee7f7d2da21386920ddacac2e33e9e938168436292f4eae180",
|
||||
"size": 40336,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "70f682b59ca0fe2f197d1486bd8be7b9b7e560798ad40ddef83b9f0a2f497938"
|
||||
},
|
||||
{
|
||||
"name": "devinfo",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/devinfo-143869c499a7e878fbeab756e9c53074195770cc41d6d0d10e45c043141389a3.img.xz",
|
||||
"hash": "143869c499a7e878fbeab756e9c53074195770cc41d6d0d10e45c043141389a3",
|
||||
"hash_raw": "143869c499a7e878fbeab756e9c53074195770cc41d6d0d10e45c043141389a3",
|
||||
"size": 4096,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "143869c499a7e878fbeab756e9c53074195770cc41d6d0d10e45c043141389a3"
|
||||
},
|
||||
{
|
||||
"name": "dsp",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/dsp-4b15fbd2f45581f1553f33f01649e450b24aa19d5deff2ac7dcb16a534d9c248.img.xz",
|
||||
"hash": "4b15fbd2f45581f1553f33f01649e450b24aa19d5deff2ac7dcb16a534d9c248",
|
||||
"hash_raw": "4b15fbd2f45581f1553f33f01649e450b24aa19d5deff2ac7dcb16a534d9c248",
|
||||
"size": 33554432,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "4b15fbd2f45581f1553f33f01649e450b24aa19d5deff2ac7dcb16a534d9c248"
|
||||
},
|
||||
{
|
||||
"name": "hyp",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/hyp-ff5ece6a4e3d2b4d898c77ffe193fc8bbc8acebe78263996ecf52373d8088927.img.xz",
|
||||
"hash": "ff5ece6a4e3d2b4d898c77ffe193fc8bbc8acebe78263996ecf52373d8088927",
|
||||
"hash_raw": "ff5ece6a4e3d2b4d898c77ffe193fc8bbc8acebe78263996ecf52373d8088927",
|
||||
"size": 524288,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "ff5ece6a4e3d2b4d898c77ffe193fc8bbc8acebe78263996ecf52373d8088927"
|
||||
},
|
||||
{
|
||||
"name": "keymaster",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/keymaster-5c968c76f29b9a4d66fbe57e639bac6b7a2c83b1758e25abbaf5d276b8a6af04.img.xz",
|
||||
"hash": "5c968c76f29b9a4d66fbe57e639bac6b7a2c83b1758e25abbaf5d276b8a6af04",
|
||||
"hash_raw": "5c968c76f29b9a4d66fbe57e639bac6b7a2c83b1758e25abbaf5d276b8a6af04",
|
||||
"size": 524288,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "5c968c76f29b9a4d66fbe57e639bac6b7a2c83b1758e25abbaf5d276b8a6af04"
|
||||
},
|
||||
{
|
||||
"name": "limits",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/limits-94951a0f7aa55fb6cb975535ce4ebbfe6d695f04cb5424677b01c10dfa2e94e1.img.xz",
|
||||
"hash": "94951a0f7aa55fb6cb975535ce4ebbfe6d695f04cb5424677b01c10dfa2e94e1",
|
||||
"hash_raw": "94951a0f7aa55fb6cb975535ce4ebbfe6d695f04cb5424677b01c10dfa2e94e1",
|
||||
"size": 4096,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "94951a0f7aa55fb6cb975535ce4ebbfe6d695f04cb5424677b01c10dfa2e94e1"
|
||||
},
|
||||
{
|
||||
"name": "logfs",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/logfs-b8b5ac87f3d954404fc7ecbdd9ee3b5b0cf5691e5006e6ec55db4c899ff61220.img.xz",
|
||||
"hash": "b8b5ac87f3d954404fc7ecbdd9ee3b5b0cf5691e5006e6ec55db4c899ff61220",
|
||||
"hash_raw": "b8b5ac87f3d954404fc7ecbdd9ee3b5b0cf5691e5006e6ec55db4c899ff61220",
|
||||
"size": 8388608,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "b8b5ac87f3d954404fc7ecbdd9ee3b5b0cf5691e5006e6ec55db4c899ff61220"
|
||||
},
|
||||
{
|
||||
"name": "modem",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/modem-a3d014f0896d77a2df7e5a80a70f43a51a047b9d03cfc675b6f0e31a6ecc4994.img.xz",
|
||||
"hash": "a3d014f0896d77a2df7e5a80a70f43a51a047b9d03cfc675b6f0e31a6ecc4994",
|
||||
"hash_raw": "a3d014f0896d77a2df7e5a80a70f43a51a047b9d03cfc675b6f0e31a6ecc4994",
|
||||
"size": 125829120,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "a3d014f0896d77a2df7e5a80a70f43a51a047b9d03cfc675b6f0e31a6ecc4994"
|
||||
},
|
||||
{
|
||||
"name": "qupfw",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/qupfw-64cc7c29d5d69b04267452b8b4ddba9f4809e68f476fc162ca283f58537afe4a.img.xz",
|
||||
"hash": "64cc7c29d5d69b04267452b8b4ddba9f4809e68f476fc162ca283f58537afe4a",
|
||||
"hash_raw": "64cc7c29d5d69b04267452b8b4ddba9f4809e68f476fc162ca283f58537afe4a",
|
||||
"size": 65536,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "64cc7c29d5d69b04267452b8b4ddba9f4809e68f476fc162ca283f58537afe4a"
|
||||
},
|
||||
{
|
||||
"name": "splash",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/splash-5c61260048f22ede6e6343fabb27f6ff73f9271f4751a01aaf7abf097afc1f08.img.xz",
|
||||
"hash": "5c61260048f22ede6e6343fabb27f6ff73f9271f4751a01aaf7abf097afc1f08",
|
||||
"hash_raw": "5c61260048f22ede6e6343fabb27f6ff73f9271f4751a01aaf7abf097afc1f08",
|
||||
"size": 34226176,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "5c61260048f22ede6e6343fabb27f6ff73f9271f4751a01aaf7abf097afc1f08"
|
||||
},
|
||||
{
|
||||
"name": "storsec",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/storsec-4494d86f68b125fbf2c004c824b1c6dbe71e61a65d2a1cc7db13c553edcb3fce.img.xz",
|
||||
"hash": "4494d86f68b125fbf2c004c824b1c6dbe71e61a65d2a1cc7db13c553edcb3fce",
|
||||
"hash_raw": "4494d86f68b125fbf2c004c824b1c6dbe71e61a65d2a1cc7db13c553edcb3fce",
|
||||
"size": 131072,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "4494d86f68b125fbf2c004c824b1c6dbe71e61a65d2a1cc7db13c553edcb3fce"
|
||||
},
|
||||
{
|
||||
"name": "tz",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/tz-e9443bf187641661bfa6c96702b9ab0156e72fb7482500f8799ba9ee2503cb16.img.xz",
|
||||
"hash": "e9443bf187641661bfa6c96702b9ab0156e72fb7482500f8799ba9ee2503cb16",
|
||||
"hash_raw": "e9443bf187641661bfa6c96702b9ab0156e72fb7482500f8799ba9ee2503cb16",
|
||||
"size": 2097152,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "e9443bf187641661bfa6c96702b9ab0156e72fb7482500f8799ba9ee2503cb16"
|
||||
},
|
||||
{
|
||||
"name": "boot",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/boot-9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892.img.xz",
|
||||
"hash": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892",
|
||||
"hash_raw": "9b07cc366919890cc88bdd45c8c7e643bf66557caf9ad6a1373accc6dcacd892",
|
||||
"size": 18479104,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "41d31b862fec1b87879b508c405adb9d7b5c0a3324f7350bd904f451605b06cf"
|
||||
},
|
||||
{
|
||||
"name": "system",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img.xz",
|
||||
"hash": "c56256a64e6d7e16886e39a4263ffb686ed0f03d3a665c3552f54a39723f8824",
|
||||
"hash_raw": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde",
|
||||
"size": 4404019200,
|
||||
"sparse": true,
|
||||
"full_check": false,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "ed2e11f52beb8559223bf9fb989fd4ef5d2ce66eeb11ae0053fff8e41903a533",
|
||||
"alt": {
|
||||
"hash": "02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-02a6f40cc305faf703ab8f993a49d720043e4df1c0787d60dcf87eedb9f2ffde.img",
|
||||
"size": 4404019200
|
||||
}
|
||||
},
|
||||
{
|
||||
"name": "userdata_90",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/userdata_90-344c1f5da1ece113e0f07c1964a95646dbfe96f49cfd64ac7375430c9247ab33.img.xz",
|
||||
"hash": "f94d635a74d1b7a7a6774f7673be0672219f253025406d0b2595ff3ef41b68f8",
|
||||
"hash_raw": "344c1f5da1ece113e0f07c1964a95646dbfe96f49cfd64ac7375430c9247ab33",
|
||||
"size": 96636764160,
|
||||
"sparse": true,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "96e376ed61e8604adce6fea4b3a7302b966c1975f768f66e4a81428dce7e78f0"
|
||||
},
|
||||
{
|
||||
"name": "userdata_89",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/userdata_89-a2840d1aeac1b2229a82d91be1b1546b3ec294cb185b6ad5ca502848d1c9c8fb.img.xz",
|
||||
"hash": "d86482e90ac0e08e31d0b699b6728043dc3db2ab0feaec99a6a393085824424a",
|
||||
"hash_raw": "a2840d1aeac1b2229a82d91be1b1546b3ec294cb185b6ad5ca502848d1c9c8fb",
|
||||
"size": 95563022336,
|
||||
"sparse": true,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "b6754cf40297e4a924315aa81d062ea108b86c93e6ad3c3c98559863a297e2b1"
|
||||
},
|
||||
{
|
||||
"name": "userdata_30",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/userdata_30-1f4a795d4c7d052e94326e668be84929856fa2945ddbdf097e1ee722defeabd1.img.xz",
|
||||
"hash": "2df53cb6d115bcb58998a7346e53030ae89683c215f1c28aaa1b524cb145ac65",
|
||||
"hash_raw": "1f4a795d4c7d052e94326e668be84929856fa2945ddbdf097e1ee722defeabd1",
|
||||
"size": 32212254720,
|
||||
"sparse": true,
|
||||
"full_check": true,
|
||||
"has_ab": false,
|
||||
"ondevice_hash": "585e9a650d53be65600589ce0b6c71edf8e6678f65d16dc6ac2dd75f900bc6cc"
|
||||
}
|
||||
]
|
||||
Executable
+158
@@ -0,0 +1,158 @@
|
||||
#!/usr/bin/env python3
|
||||
import time
|
||||
from smbus2 import SMBus
|
||||
from collections import namedtuple
|
||||
|
||||
# https://datasheets.maximintegrated.com/en/ds/MAX98089.pdf
|
||||
|
||||
AmpConfig = namedtuple('AmpConfig', ['name', 'value', 'register', 'offset', 'mask'])
|
||||
EQParams = namedtuple('EQParams', ['K', 'k1', 'k2', 'c1', 'c2'])
|
||||
|
||||
def configs_from_eq_params(base, eq_params):
|
||||
return [
|
||||
AmpConfig("K (high)", (eq_params.K >> 8), base, 0, 0xFF),
|
||||
AmpConfig("K (low)", (eq_params.K & 0xFF), base + 1, 0, 0xFF),
|
||||
AmpConfig("k1 (high)", (eq_params.k1 >> 8), base + 2, 0, 0xFF),
|
||||
AmpConfig("k1 (low)", (eq_params.k1 & 0xFF), base + 3, 0, 0xFF),
|
||||
AmpConfig("k2 (high)", (eq_params.k2 >> 8), base + 4, 0, 0xFF),
|
||||
AmpConfig("k2 (low)", (eq_params.k2 & 0xFF), base + 5, 0, 0xFF),
|
||||
AmpConfig("c1 (high)", (eq_params.c1 >> 8), base + 6, 0, 0xFF),
|
||||
AmpConfig("c1 (low)", (eq_params.c1 & 0xFF), base + 7, 0, 0xFF),
|
||||
AmpConfig("c2 (high)", (eq_params.c2 >> 8), base + 8, 0, 0xFF),
|
||||
AmpConfig("c2 (low)", (eq_params.c2 & 0xFF), base + 9, 0, 0xFF),
|
||||
]
|
||||
|
||||
BASE_CONFIG = [
|
||||
AmpConfig("MCLK prescaler", 0b01, 0x10, 4, 0b00110000),
|
||||
AmpConfig("PM: enable speakers", 0b11, 0x4D, 4, 0b00110000),
|
||||
AmpConfig("PM: enable DACs", 0b11, 0x4D, 0, 0b00000011),
|
||||
AmpConfig("Enable PLL1", 0b1, 0x12, 7, 0b10000000),
|
||||
AmpConfig("Enable PLL2", 0b1, 0x1A, 7, 0b10000000),
|
||||
AmpConfig("DAI1: I2S mode", 0b00100, 0x14, 2, 0b01111100),
|
||||
AmpConfig("DAI2: I2S mode", 0b00100, 0x1C, 2, 0b01111100),
|
||||
AmpConfig("DAI1 Passband filtering: music mode", 0b1, 0x18, 7, 0b10000000),
|
||||
AmpConfig("DAI1 voice mode gain (DV1G)", 0b00, 0x2F, 4, 0b00110000),
|
||||
AmpConfig("DAI1 attenuation (DV1)", 0x0, 0x2F, 0, 0b00001111),
|
||||
AmpConfig("DAI2 attenuation (DV2)", 0x0, 0x31, 0, 0b00001111),
|
||||
AmpConfig("DAI2: DC blocking", 0b1, 0x20, 0, 0b00000001),
|
||||
AmpConfig("DAI2: High sample rate", 0b0, 0x20, 3, 0b00001000),
|
||||
AmpConfig("ALC enable", 0b1, 0x43, 7, 0b10000000),
|
||||
AmpConfig("ALC/excursion limiter release time", 0b101, 0x43, 4, 0b01110000),
|
||||
AmpConfig("ALC multiband enable", 0b1, 0x43, 3, 0b00001000),
|
||||
AmpConfig("DAI1 EQ enable", 0b0, 0x49, 0, 0b00000001),
|
||||
AmpConfig("DAI2 EQ clip detection disabled", 0b1, 0x32, 4, 0b00010000),
|
||||
AmpConfig("DAI2 EQ attenuation", 0x5, 0x32, 0, 0b00001111),
|
||||
AmpConfig("Excursion limiter upper corner freq", 0b100, 0x41, 4, 0b01110000),
|
||||
AmpConfig("Excursion limiter lower corner freq", 0b00, 0x41, 0, 0b00000011),
|
||||
AmpConfig("Excursion limiter threshold", 0b000, 0x42, 0, 0b00001111),
|
||||
AmpConfig("Distortion limit (THDCLP)", 0x6, 0x46, 4, 0b11110000),
|
||||
AmpConfig("Distortion limiter release time constant", 0b0, 0x46, 0, 0b00000001),
|
||||
AmpConfig("Right DAC input mixer: DAI1 left", 0b0, 0x22, 3, 0b00001000),
|
||||
AmpConfig("Right DAC input mixer: DAI1 right", 0b0, 0x22, 2, 0b00000100),
|
||||
AmpConfig("Right DAC input mixer: DAI2 left", 0b1, 0x22, 1, 0b00000010),
|
||||
AmpConfig("Right DAC input mixer: DAI2 right", 0b0, 0x22, 0, 0b00000001),
|
||||
AmpConfig("DAI1 audio port selector", 0b10, 0x16, 6, 0b11000000),
|
||||
AmpConfig("DAI2 audio port selector", 0b01, 0x1E, 6, 0b11000000),
|
||||
AmpConfig("Enable left digital microphone", 0b1, 0x48, 5, 0b00100000),
|
||||
AmpConfig("Enable right digital microphone", 0b1, 0x48, 4, 0b00010000),
|
||||
AmpConfig("Enhanced volume smoothing disabled", 0b0, 0x49, 7, 0b10000000),
|
||||
AmpConfig("Volume adjustment smoothing disabled", 0b0, 0x49, 6, 0b01000000),
|
||||
AmpConfig("Zero-crossing detection disabled", 0b0, 0x49, 5, 0b00100000),
|
||||
]
|
||||
|
||||
CONFIGS = {
|
||||
"tici": [
|
||||
AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
|
||||
AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100),
|
||||
AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111),
|
||||
AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010),
|
||||
|
||||
*configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)),
|
||||
*configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)),
|
||||
*configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)),
|
||||
*configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807)),
|
||||
*configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B)),
|
||||
],
|
||||
"tizi": [
|
||||
AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111),
|
||||
AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
|
||||
AmpConfig("Left Speaker Mixer Gain", 0b00, 0x2D, 0, 0b00000011),
|
||||
AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100),
|
||||
AmpConfig("Left speaker output volume", 0x17, 0x3D, 0, 0b00011111),
|
||||
AmpConfig("Right speaker output volume", 0x17, 0x3E, 0, 0b00011111),
|
||||
|
||||
AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010),
|
||||
AmpConfig("DAI2: DC blocking", 0b0, 0x20, 0, 0b00000001),
|
||||
AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000),
|
||||
AmpConfig("DAI2 EQ attenuation", 0x2, 0x32, 0, 0b00001111),
|
||||
AmpConfig("Excursion limiter upper corner freq", 0b001, 0x41, 4, 0b01110000),
|
||||
AmpConfig("Excursion limiter threshold", 0b100, 0x42, 0, 0b00001111),
|
||||
AmpConfig("Distortion limit (THDCLP)", 0x0, 0x46, 4, 0b11110000),
|
||||
AmpConfig("Distortion limiter release time constant", 0b1, 0x46, 0, 0b00000001),
|
||||
AmpConfig("Left DAC input mixer: DAI1 left", 0b0, 0x22, 7, 0b10000000),
|
||||
AmpConfig("Left DAC input mixer: DAI1 right", 0b0, 0x22, 6, 0b01000000),
|
||||
AmpConfig("Left DAC input mixer: DAI2 left", 0b1, 0x22, 5, 0b00100000),
|
||||
AmpConfig("Left DAC input mixer: DAI2 right", 0b0, 0x22, 4, 0b00010000),
|
||||
AmpConfig("Right DAC input mixer: DAI2 left", 0b0, 0x22, 1, 0b00000010),
|
||||
AmpConfig("Right DAC input mixer: DAI2 right", 0b1, 0x22, 0, 0b00000001),
|
||||
AmpConfig("Volume adjustment smoothing disabled", 0b1, 0x49, 6, 0b01000000),
|
||||
],
|
||||
}
|
||||
|
||||
class Amplifier:
|
||||
AMP_I2C_BUS = 0
|
||||
AMP_ADDRESS = 0x10
|
||||
|
||||
def __init__(self, debug=False):
|
||||
self.debug = debug
|
||||
|
||||
def _get_shutdown_config(self, amp_disabled: bool) -> AmpConfig:
|
||||
return AmpConfig("Global shutdown", 0b0 if amp_disabled else 0b1, 0x51, 7, 0b10000000)
|
||||
|
||||
def _set_configs(self, configs: list[AmpConfig]) -> None:
|
||||
with SMBus(self.AMP_I2C_BUS) as bus:
|
||||
for config in configs:
|
||||
if self.debug:
|
||||
print(f"Setting \"{config.name}\" to {config.value}:")
|
||||
|
||||
old_value = bus.read_byte_data(self.AMP_ADDRESS, config.register, force=True)
|
||||
new_value = (old_value & (~config.mask)) | ((config.value << config.offset) & config.mask)
|
||||
bus.write_byte_data(self.AMP_ADDRESS, config.register, new_value, force=True)
|
||||
|
||||
if self.debug:
|
||||
print(f" Changed {hex(config.register)}: {hex(old_value)} -> {hex(new_value)}")
|
||||
|
||||
def set_configs(self, configs: list[AmpConfig]) -> bool:
|
||||
# retry in case panda is using the amp
|
||||
tries = 15
|
||||
backoff = 0.
|
||||
for i in range(tries):
|
||||
try:
|
||||
self._set_configs(configs)
|
||||
return True
|
||||
except OSError:
|
||||
backoff += 0.1
|
||||
time.sleep(backoff)
|
||||
print(f"Failed to set amp config, {tries - i - 1} retries left")
|
||||
return False
|
||||
|
||||
def set_global_shutdown(self, amp_disabled: bool) -> bool:
|
||||
return self.set_configs([self._get_shutdown_config(amp_disabled), ])
|
||||
|
||||
def initialize_configuration(self, model: str) -> bool:
|
||||
cfgs = [
|
||||
self._get_shutdown_config(True),
|
||||
*BASE_CONFIG,
|
||||
*CONFIGS[model],
|
||||
self._get_shutdown_config(False),
|
||||
]
|
||||
return self.set_configs(cfgs)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
with open("/sys/firmware/devicetree/base/model") as f:
|
||||
model = f.read().strip('\x00')
|
||||
model = model.split('comma ')[-1]
|
||||
|
||||
amp = Amplifier()
|
||||
amp.initialize_configuration(model)
|
||||
@@ -0,0 +1,30 @@
|
||||
[connection]
|
||||
id=esim
|
||||
uuid=fff6553c-3284-4707-a6b1-acc021caaafb
|
||||
type=gsm
|
||||
permissions=
|
||||
autoconnect=true
|
||||
autoconnect-retries=100
|
||||
autoconnect-priority=2
|
||||
metered=1
|
||||
|
||||
[gsm]
|
||||
apn=
|
||||
home-only=false
|
||||
auto-config=true
|
||||
sim-id=
|
||||
|
||||
[ipv4]
|
||||
route-metric=1000
|
||||
dns-priority=1000
|
||||
dns-search=
|
||||
method=auto
|
||||
|
||||
[ipv6]
|
||||
ddr-gen-mode=stable-privacy
|
||||
dns-search=
|
||||
route-metric=1000
|
||||
dns-priority=1000
|
||||
method=auto
|
||||
|
||||
[proxy]
|
||||
Executable
+115
@@ -0,0 +1,115 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import math
|
||||
import time
|
||||
import binascii
|
||||
import requests
|
||||
import serial
|
||||
import subprocess
|
||||
|
||||
|
||||
def post(url, payload):
|
||||
print()
|
||||
print("POST to", url)
|
||||
r = requests.post(
|
||||
url,
|
||||
data=payload,
|
||||
verify=False,
|
||||
headers={
|
||||
"Content-Type": "application/json",
|
||||
"X-Admin-Protocol": "gsma/rsp/v2.2.0",
|
||||
"charset": "utf-8",
|
||||
"User-Agent": "gsma-rsp-lpad",
|
||||
},
|
||||
)
|
||||
print("resp", r)
|
||||
print("resp text", repr(r.text))
|
||||
print()
|
||||
r.raise_for_status()
|
||||
|
||||
ret = f"HTTP/1.1 {r.status_code}"
|
||||
ret += ''.join(f"{k}: {v}" for k, v in r.headers.items() if k != 'Connection')
|
||||
return ret.encode() + r.content
|
||||
|
||||
|
||||
class LPA:
|
||||
def __init__(self):
|
||||
self.dev = serial.Serial('/dev/ttyUSB2', baudrate=57600, timeout=1, bytesize=8)
|
||||
self.dev.reset_input_buffer()
|
||||
self.dev.reset_output_buffer()
|
||||
assert "OK" in self.at("AT")
|
||||
|
||||
def at(self, cmd):
|
||||
print(f"==> {cmd}")
|
||||
self.dev.write(cmd.encode() + b'\r\n')
|
||||
|
||||
r = b""
|
||||
cnt = 0
|
||||
while b"OK" not in r and b"ERROR" not in r and cnt < 20:
|
||||
r += self.dev.read(8192).strip()
|
||||
cnt += 1
|
||||
r = r.decode()
|
||||
print(f"<== {repr(r)}")
|
||||
return r
|
||||
|
||||
def download_ota(self, qr):
|
||||
return self.at(f'AT+QESIM="ota","{qr}"')
|
||||
|
||||
def download(self, qr):
|
||||
smdp = qr.split('$')[1]
|
||||
out = self.at(f'AT+QESIM="download","{qr}"')
|
||||
for _ in range(5):
|
||||
line = out.split("+QESIM: ")[1].split("\r\n\r\nOK")[0]
|
||||
|
||||
parts = [x.strip().strip('"') for x in line.split(',', maxsplit=4)]
|
||||
print(repr(parts))
|
||||
trans, ret, url, payloadlen, payload = parts
|
||||
assert trans == "trans" and ret == "0"
|
||||
assert len(payload) == int(payloadlen)
|
||||
|
||||
r = post(f"https://{smdp}/{url}", payload)
|
||||
to_send = binascii.hexlify(r).decode()
|
||||
|
||||
chunk_len = 1400
|
||||
for i in range(math.ceil(len(to_send) / chunk_len)):
|
||||
state = 1 if (i+1)*chunk_len < len(to_send) else 0
|
||||
data = to_send[i * chunk_len : (i+1)*chunk_len]
|
||||
out = self.at(f'AT+QESIM="trans",{len(to_send)},{state},{i},{len(data)},"{data}"')
|
||||
assert "OK" in out
|
||||
|
||||
if '+QESIM:"download",1' in out:
|
||||
raise Exception("profile install failed")
|
||||
elif '+QESIM:"download",0' in out:
|
||||
print("done, successfully loaded")
|
||||
break
|
||||
|
||||
def enable(self, iccid):
|
||||
self.at(f'AT+QESIM="enable","{iccid}"')
|
||||
|
||||
def disable(self, iccid):
|
||||
self.at(f'AT+QESIM="disable","{iccid}"')
|
||||
|
||||
def delete(self, iccid):
|
||||
self.at(f'AT+QESIM="delete","{iccid}"')
|
||||
|
||||
def list_profiles(self):
|
||||
out = self.at('AT+QESIM="list"')
|
||||
return out.strip().splitlines()[1:]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import sys
|
||||
|
||||
if "RESTART" in os.environ:
|
||||
subprocess.check_call("sudo systemctl stop ModemManager", shell=True)
|
||||
subprocess.check_call("/usr/comma/lte/lte.sh stop_blocking", shell=True)
|
||||
subprocess.check_call("/usr/comma/lte/lte.sh start", shell=True)
|
||||
while not os.path.exists('/dev/ttyUSB2'):
|
||||
time.sleep(1)
|
||||
time.sleep(3)
|
||||
|
||||
lpa = LPA()
|
||||
print(lpa.list_profiles())
|
||||
if len(sys.argv) > 1:
|
||||
lpa.download(sys.argv[1])
|
||||
print(lpa.list_profiles())
|
||||
@@ -0,0 +1,112 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdlib>
|
||||
#include <cassert>
|
||||
#include <fstream>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <algorithm> // for std::clamp
|
||||
|
||||
#include "common/params.h"
|
||||
#include "common/util.h"
|
||||
#include "system/hardware/base.h"
|
||||
|
||||
class HardwareTici : public HardwareNone {
|
||||
public:
|
||||
static constexpr float MAX_VOLUME = 0.9;
|
||||
static constexpr float MIN_VOLUME = 0.1;
|
||||
static bool TICI() { return true; }
|
||||
static bool AGNOS() { return true; }
|
||||
static std::string get_os_version() {
|
||||
return "AGNOS " + util::read_file("/VERSION");
|
||||
}
|
||||
|
||||
static std::string get_name() {
|
||||
std::string model = util::read_file("/sys/firmware/devicetree/base/model");
|
||||
return util::strip(model.substr(std::string("comma ").size()));
|
||||
}
|
||||
|
||||
static cereal::InitData::DeviceType get_device_type() {
|
||||
static const std::map<std::string, cereal::InitData::DeviceType> device_map = {
|
||||
{"tici", cereal::InitData::DeviceType::TICI},
|
||||
{"tizi", cereal::InitData::DeviceType::TIZI},
|
||||
{"mici", cereal::InitData::DeviceType::MICI}
|
||||
};
|
||||
auto it = device_map.find(get_name());
|
||||
assert(it != device_map.end());
|
||||
return it->second;
|
||||
}
|
||||
|
||||
static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); }
|
||||
static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); }
|
||||
|
||||
static std::string get_serial() {
|
||||
static std::string serial("");
|
||||
if (serial.empty()) {
|
||||
std::ifstream stream("/proc/cmdline");
|
||||
std::string cmdline;
|
||||
std::getline(stream, cmdline);
|
||||
|
||||
auto start = cmdline.find("serialno=");
|
||||
if (start == std::string::npos) {
|
||||
serial = "cccccc";
|
||||
} else {
|
||||
auto end = cmdline.find(" ", start + 9);
|
||||
serial = cmdline.substr(start + 9, end - start - 9);
|
||||
}
|
||||
}
|
||||
return serial;
|
||||
}
|
||||
|
||||
static void reboot() { std::system("sudo reboot"); }
|
||||
static void poweroff() { std::system("sudo poweroff"); }
|
||||
static void set_brightness(int percent) {
|
||||
float max = std::stof(util::read_file("/sys/class/backlight/panel0-backlight/max_brightness"));
|
||||
std::ofstream("/sys/class/backlight/panel0-backlight/brightness") << int(percent * (max / 100.0f)) << "\n";
|
||||
}
|
||||
static void set_display_power(bool on) {
|
||||
std::ofstream("/sys/class/backlight/panel0-backlight/bl_power") << (on ? "0" : "4") << "\n";
|
||||
}
|
||||
|
||||
static void set_ir_power(int percent) {
|
||||
auto device = get_device_type();
|
||||
if (device == cereal::InitData::DeviceType::TICI ||
|
||||
device == cereal::InitData::DeviceType::TIZI) {
|
||||
return;
|
||||
}
|
||||
|
||||
int value = util::map_val(std::clamp(percent, 0, 100), 0, 100, 0, 255);
|
||||
std::ofstream("/sys/class/leds/led:switch_2/brightness") << 0 << "\n";
|
||||
std::ofstream("/sys/class/leds/led:torch_2/brightness") << value << "\n";
|
||||
std::ofstream("/sys/class/leds/led:switch_2/brightness") << value << "\n";
|
||||
}
|
||||
|
||||
static std::map<std::string, std::string> get_init_logs() {
|
||||
std::map<std::string, std::string> ret = {
|
||||
{"/BUILD", util::read_file("/BUILD")},
|
||||
{"lsblk", util::check_output("lsblk -o NAME,SIZE,STATE,VENDOR,MODEL,REV,SERIAL")},
|
||||
{"SOM ID", util::read_file("/sys/devices/platform/vendor/vendor:gpio-som-id/som_id")},
|
||||
};
|
||||
|
||||
std::string bs = util::check_output("abctl --boot_slot");
|
||||
ret["boot slot"] = bs.substr(0, bs.find_first_of("\n"));
|
||||
|
||||
std::string temp = util::read_file("/dev/disk/by-partlabel/ssd");
|
||||
temp.erase(temp.find_last_not_of(std::string("\0\r\n", 3))+1);
|
||||
ret["boot temp"] = temp;
|
||||
|
||||
// TODO: log something from system and boot
|
||||
for (std::string part : {"xbl", "abl", "aop", "devcfg", "xbl_config"}) {
|
||||
for (std::string slot : {"a", "b"}) {
|
||||
std::string partition = part + "_" + slot;
|
||||
std::string hash = util::check_output("sha256sum /dev/disk/by-partlabel/" + partition);
|
||||
ret[partition] = hash.substr(0, hash.find_first_of(" "));
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); }
|
||||
static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); }
|
||||
};
|
||||
@@ -0,0 +1,602 @@
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import subprocess
|
||||
import time
|
||||
import tempfile
|
||||
from enum import IntEnum
|
||||
from functools import cached_property, lru_cache
|
||||
from pathlib import Path
|
||||
|
||||
from cereal import log
|
||||
from openpilot.common.gpio import gpio_set, gpio_init, get_irqs_for_action
|
||||
from openpilot.system.hardware.base import HardwareBase, ThermalConfig, ThermalZone
|
||||
from openpilot.system.hardware.tici import iwlist
|
||||
from openpilot.system.hardware.tici.pins import GPIO
|
||||
from openpilot.system.hardware.tici.amplifier import Amplifier
|
||||
|
||||
NM = 'org.freedesktop.NetworkManager'
|
||||
NM_CON_ACT = NM + '.Connection.Active'
|
||||
NM_DEV = NM + '.Device'
|
||||
NM_DEV_WL = NM + '.Device.Wireless'
|
||||
NM_DEV_STATS = NM + '.Device.Statistics'
|
||||
NM_AP = NM + '.AccessPoint'
|
||||
DBUS_PROPS = 'org.freedesktop.DBus.Properties'
|
||||
|
||||
MM = 'org.freedesktop.ModemManager1'
|
||||
MM_MODEM = MM + ".Modem"
|
||||
MM_MODEM_SIMPLE = MM + ".Modem.Simple"
|
||||
MM_SIM = MM + ".Sim"
|
||||
|
||||
class MM_MODEM_STATE(IntEnum):
|
||||
FAILED = -1
|
||||
UNKNOWN = 0
|
||||
INITIALIZING = 1
|
||||
LOCKED = 2
|
||||
DISABLED = 3
|
||||
DISABLING = 4
|
||||
ENABLING = 5
|
||||
ENABLED = 6
|
||||
SEARCHING = 7
|
||||
REGISTERED = 8
|
||||
DISCONNECTING = 9
|
||||
CONNECTING = 10
|
||||
CONNECTED = 11
|
||||
|
||||
class NMMetered(IntEnum):
|
||||
NM_METERED_UNKNOWN = 0
|
||||
NM_METERED_YES = 1
|
||||
NM_METERED_NO = 2
|
||||
NM_METERED_GUESS_YES = 3
|
||||
NM_METERED_GUESS_NO = 4
|
||||
|
||||
TIMEOUT = 0.1
|
||||
REFRESH_RATE_MS = 1000
|
||||
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
NetworkStrength = log.DeviceState.NetworkStrength
|
||||
|
||||
# https://developer.gnome.org/ModemManager/unstable/ModemManager-Flags-and-Enumerations.html#MMModemAccessTechnology
|
||||
MM_MODEM_ACCESS_TECHNOLOGY_UMTS = 1 << 5
|
||||
MM_MODEM_ACCESS_TECHNOLOGY_LTE = 1 << 14
|
||||
|
||||
|
||||
def sudo_write(val, path):
|
||||
try:
|
||||
with open(path, 'w') as f:
|
||||
f.write(str(val))
|
||||
except PermissionError:
|
||||
os.system(f"sudo chmod a+w {path}")
|
||||
try:
|
||||
with open(path, 'w') as f:
|
||||
f.write(str(val))
|
||||
except PermissionError:
|
||||
# fallback for debugfs files
|
||||
os.system(f"sudo su -c 'echo {val} > {path}'")
|
||||
|
||||
def sudo_read(path: str) -> str:
|
||||
try:
|
||||
return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip()
|
||||
except Exception:
|
||||
return ""
|
||||
|
||||
def affine_irq(val, action):
|
||||
irqs = get_irqs_for_action(action)
|
||||
if len(irqs) == 0:
|
||||
print(f"No IRQs found for '{action}'")
|
||||
return
|
||||
|
||||
for i in irqs:
|
||||
sudo_write(str(val), f"/proc/irq/{i}/smp_affinity_list")
|
||||
|
||||
@lru_cache
|
||||
def get_device_type():
|
||||
# lru_cache and cache can cause memory leaks when used in classes
|
||||
with open("/sys/firmware/devicetree/base/model") as f:
|
||||
model = f.read().strip('\x00')
|
||||
return model.split('comma ')[-1]
|
||||
|
||||
class Tici(HardwareBase):
|
||||
@cached_property
|
||||
def bus(self):
|
||||
import dbus
|
||||
return dbus.SystemBus()
|
||||
|
||||
@cached_property
|
||||
def nm(self):
|
||||
return self.bus.get_object(NM, '/org/freedesktop/NetworkManager')
|
||||
|
||||
@property # this should not be cached, in case the modemmanager restarts
|
||||
def mm(self):
|
||||
return self.bus.get_object(MM, '/org/freedesktop/ModemManager1')
|
||||
|
||||
@cached_property
|
||||
def amplifier(self):
|
||||
if self.get_device_type() == "mici" or os.getenv("DISABLE_DRIVER"):
|
||||
return None
|
||||
return Amplifier()
|
||||
|
||||
def get_os_version(self):
|
||||
with open("/VERSION") as f:
|
||||
return f.read().strip()
|
||||
|
||||
def get_device_type(self):
|
||||
return get_device_type()
|
||||
|
||||
def reboot(self, reason=None):
|
||||
subprocess.check_output(["sudo", "reboot"])
|
||||
|
||||
def uninstall(self):
|
||||
Path("/data/__system_reset__").touch()
|
||||
os.sync()
|
||||
self.reboot()
|
||||
|
||||
def get_serial(self):
|
||||
return self.get_cmdline()['androidboot.serialno']
|
||||
|
||||
def get_network_type(self):
|
||||
try:
|
||||
primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
primary_connection = self.bus.get_object(NM, primary_connection)
|
||||
primary_type = primary_connection.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
|
||||
if primary_type == '802-3-ethernet':
|
||||
return NetworkType.ethernet
|
||||
elif primary_type == '802-11-wireless':
|
||||
return NetworkType.wifi
|
||||
else:
|
||||
active_connections = self.nm.Get(NM, 'ActiveConnections', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
for conn in active_connections:
|
||||
c = self.bus.get_object(NM, conn)
|
||||
tp = c.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
if tp == 'gsm':
|
||||
modem = self.get_modem()
|
||||
access_t = modem.Get(MM_MODEM, 'AccessTechnologies', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
if access_t >= MM_MODEM_ACCESS_TECHNOLOGY_LTE:
|
||||
return NetworkType.cell4G
|
||||
elif access_t >= MM_MODEM_ACCESS_TECHNOLOGY_UMTS:
|
||||
return NetworkType.cell3G
|
||||
else:
|
||||
return NetworkType.cell2G
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return NetworkType.none
|
||||
|
||||
def get_modem(self):
|
||||
objects = self.mm.GetManagedObjects(dbus_interface="org.freedesktop.DBus.ObjectManager", timeout=TIMEOUT)
|
||||
modem_path = list(objects.keys())[0]
|
||||
return self.bus.get_object(MM, modem_path)
|
||||
|
||||
def get_wlan(self):
|
||||
wlan_path = self.nm.GetDeviceByIpIface('wlan0', dbus_interface=NM, timeout=TIMEOUT)
|
||||
return self.bus.get_object(NM, wlan_path)
|
||||
|
||||
def get_wwan(self):
|
||||
wwan_path = self.nm.GetDeviceByIpIface('wwan0', dbus_interface=NM, timeout=TIMEOUT)
|
||||
return self.bus.get_object(NM, wwan_path)
|
||||
|
||||
def get_sim_info(self):
|
||||
modem = self.get_modem()
|
||||
sim_path = modem.Get(MM_MODEM, 'Sim', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
|
||||
if sim_path == "/":
|
||||
return {
|
||||
'sim_id': '',
|
||||
'mcc_mnc': None,
|
||||
'network_type': ["Unknown"],
|
||||
'sim_state': ["ABSENT"],
|
||||
'data_connected': False
|
||||
}
|
||||
else:
|
||||
sim = self.bus.get_object(MM, sim_path)
|
||||
return {
|
||||
'sim_id': str(sim.Get(MM_SIM, 'SimIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)),
|
||||
'mcc_mnc': str(sim.Get(MM_SIM, 'OperatorIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)),
|
||||
'network_type': ["Unknown"],
|
||||
'sim_state': ["READY"],
|
||||
'data_connected': modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) == MM_MODEM_STATE.CONNECTED,
|
||||
}
|
||||
|
||||
def get_imei(self, slot):
|
||||
if slot != 0:
|
||||
return ""
|
||||
|
||||
return str(self.get_modem().Get(MM_MODEM, 'EquipmentIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT))
|
||||
|
||||
def get_network_info(self):
|
||||
if self.get_device_type() == "mici" or os.getenv("DISABLE_DRIVER"):
|
||||
return None
|
||||
try:
|
||||
modem = self.get_modem()
|
||||
info = modem.Command("AT+QNWINFO", math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
|
||||
extra = modem.Command('AT+QENG="servingcell"', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
|
||||
state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
if info and info.startswith('+QNWINFO: '):
|
||||
info = info.replace('+QNWINFO: ', '').replace('"', '').split(',')
|
||||
extra = "" if extra is None else extra.replace('+QENG: "servingcell",', '').replace('"', '')
|
||||
state = "" if state is None else MM_MODEM_STATE(state).name
|
||||
|
||||
if len(info) != 4:
|
||||
return None
|
||||
|
||||
technology, operator, band, channel = info
|
||||
|
||||
return({
|
||||
'technology': technology,
|
||||
'operator': operator,
|
||||
'band': band,
|
||||
'channel': int(channel),
|
||||
'extra': extra,
|
||||
'state': state,
|
||||
})
|
||||
else:
|
||||
return None
|
||||
|
||||
def parse_strength(self, percentage):
|
||||
if percentage < 25:
|
||||
return NetworkStrength.poor
|
||||
elif percentage < 50:
|
||||
return NetworkStrength.moderate
|
||||
elif percentage < 75:
|
||||
return NetworkStrength.good
|
||||
else:
|
||||
return NetworkStrength.great
|
||||
|
||||
def get_network_strength(self, network_type):
|
||||
network_strength = NetworkStrength.unknown
|
||||
|
||||
try:
|
||||
if network_type == NetworkType.none:
|
||||
pass
|
||||
elif network_type == NetworkType.wifi:
|
||||
wlan = self.get_wlan()
|
||||
active_ap_path = wlan.Get(NM_DEV_WL, 'ActiveAccessPoint', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
if active_ap_path != "/":
|
||||
active_ap = self.bus.get_object(NM, active_ap_path)
|
||||
strength = int(active_ap.Get(NM_AP, 'Strength', dbus_interface=DBUS_PROPS, timeout=TIMEOUT))
|
||||
network_strength = self.parse_strength(strength)
|
||||
else: # Cellular
|
||||
modem = self.get_modem()
|
||||
strength = int(modem.Get(MM_MODEM, 'SignalQuality', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)[0])
|
||||
network_strength = self.parse_strength(strength)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return network_strength
|
||||
|
||||
def get_network_metered(self, network_type) -> bool:
|
||||
try:
|
||||
primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
primary_connection = self.bus.get_object(NM, primary_connection)
|
||||
primary_devices = primary_connection.Get(NM_CON_ACT, 'Devices', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
|
||||
for dev in primary_devices:
|
||||
dev_obj = self.bus.get_object(NM, str(dev))
|
||||
metered_prop = dev_obj.Get(NM_DEV, 'Metered', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
|
||||
if network_type == NetworkType.wifi:
|
||||
if metered_prop in [NMMetered.NM_METERED_YES, NMMetered.NM_METERED_GUESS_YES]:
|
||||
return True
|
||||
elif network_type in [NetworkType.cell2G, NetworkType.cell3G, NetworkType.cell4G, NetworkType.cell5G]:
|
||||
if metered_prop == NMMetered.NM_METERED_NO:
|
||||
return False
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return super().get_network_metered(network_type)
|
||||
|
||||
def get_modem_version(self):
|
||||
try:
|
||||
modem = self.get_modem()
|
||||
return modem.Get(MM_MODEM, 'Revision', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
def get_modem_temperatures(self):
|
||||
if self.get_device_type() == "mici" or os.getenv("DISABLE_DRIVER"):
|
||||
return []
|
||||
timeout = 0.2 # Default timeout is too short
|
||||
try:
|
||||
modem = self.get_modem()
|
||||
temps = modem.Command("AT+QTEMP", math.ceil(timeout), dbus_interface=MM_MODEM, timeout=timeout)
|
||||
return list(map(int, temps.split(' ')[1].split(',')))
|
||||
except Exception:
|
||||
return []
|
||||
|
||||
def get_nvme_temperatures(self):
|
||||
ret = []
|
||||
try:
|
||||
out = subprocess.check_output("sudo smartctl -aj /dev/nvme0", shell=True)
|
||||
dat = json.loads(out)
|
||||
ret = list(map(int, dat["nvme_smart_health_information_log"]["temperature_sensors"]))
|
||||
except Exception:
|
||||
pass
|
||||
return ret
|
||||
|
||||
def get_current_power_draw(self):
|
||||
return (self.read_param_file("/sys/class/hwmon/hwmon1/power1_input", int) / 1e6)
|
||||
|
||||
def get_som_power_draw(self):
|
||||
return (self.read_param_file("/sys/class/power_supply/bms/voltage_now", int) * self.read_param_file("/sys/class/power_supply/bms/current_now", int) / 1e12)
|
||||
|
||||
def shutdown(self):
|
||||
os.system("sudo poweroff")
|
||||
|
||||
def get_thermal_config(self):
|
||||
intake, exhaust, case = None, None, None
|
||||
if self.get_device_type() == "mici":
|
||||
case = ThermalZone("case")
|
||||
intake = ThermalZone("intake")
|
||||
exhaust = ThermalZone("exhaust")
|
||||
return ThermalConfig(cpu=[ThermalZone(f"cpu{i}-silver-usr") for i in range(4)] +
|
||||
[ThermalZone(f"cpu{i}-gold-usr") for i in range(4)],
|
||||
gpu=[ThermalZone("gpu0-usr"), ThermalZone("gpu1-usr")],
|
||||
memory=ThermalZone("ddr-usr"),
|
||||
pmic=[ThermalZone("pm8998_tz"), ThermalZone("pm8005_tz")],
|
||||
intake=intake,
|
||||
exhaust=exhaust,
|
||||
case=case)
|
||||
|
||||
def set_screen_brightness(self, percentage):
|
||||
try:
|
||||
with open("/sys/class/backlight/panel0-backlight/max_brightness") as f:
|
||||
max_brightness = float(f.read().strip())
|
||||
|
||||
val = int(percentage * (max_brightness / 100.))
|
||||
with open("/sys/class/backlight/panel0-backlight/brightness", "w") as f:
|
||||
f.write(str(val))
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def get_screen_brightness(self):
|
||||
try:
|
||||
with open("/sys/class/backlight/panel0-backlight/max_brightness") as f:
|
||||
max_brightness = float(f.read().strip())
|
||||
|
||||
with open("/sys/class/backlight/panel0-backlight/brightness") as f:
|
||||
return int(float(f.read()) / (max_brightness / 100.))
|
||||
except Exception:
|
||||
return 0
|
||||
|
||||
def set_power_save(self, powersave_enabled):
|
||||
# amplifier, 100mW at idle
|
||||
if self.amplifier is not None:
|
||||
self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled)
|
||||
if not powersave_enabled:
|
||||
self.amplifier.initialize_configuration(self.get_device_type())
|
||||
|
||||
# *** CPU config ***
|
||||
|
||||
# offline big cluster
|
||||
for i in range(4, 8):
|
||||
val = '0' if powersave_enabled else '1'
|
||||
sudo_write(val, f'/sys/devices/system/cpu/cpu{i}/online')
|
||||
|
||||
for n in ('0', '4'):
|
||||
if powersave_enabled and n == '4':
|
||||
continue
|
||||
gov = 'ondemand' if powersave_enabled else 'performance'
|
||||
sudo_write(gov, f'/sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor')
|
||||
|
||||
# *** IRQ config ***
|
||||
|
||||
# GPU, modeld core
|
||||
affine_irq(7, "kgsl-3d0")
|
||||
|
||||
# camerad core
|
||||
camera_irqs = ("a5", "cci", "cpas_camnoc", "cpas-cdm", "csid", "ife", "csid-lite", "ife-lite")
|
||||
for n in camera_irqs:
|
||||
affine_irq(6, n)
|
||||
|
||||
def get_gpu_usage_percent(self):
|
||||
try:
|
||||
with open('/sys/class/kgsl/kgsl-3d0/gpubusy') as f:
|
||||
used, total = f.read().strip().split()
|
||||
return 100.0 * int(used) / int(total)
|
||||
except Exception:
|
||||
return 0
|
||||
|
||||
def initialize_hardware(self):
|
||||
if self.amplifier is not None:
|
||||
self.amplifier.initialize_configuration(self.get_device_type())
|
||||
|
||||
# Allow hardwared to write engagement status to kmsg
|
||||
os.system("sudo chmod a+w /dev/kmsg")
|
||||
|
||||
# Ensure fan gpio is enabled so fan runs until shutdown, also turned on at boot by the ABL
|
||||
gpio_init(GPIO.SOM_ST_IO, True)
|
||||
gpio_set(GPIO.SOM_ST_IO, 1)
|
||||
|
||||
# *** IRQ config ***
|
||||
|
||||
# mask off big cluster from default affinity
|
||||
sudo_write("f", "/proc/irq/default_smp_affinity")
|
||||
|
||||
# move these off the default core
|
||||
affine_irq(1, "msm_drm") # display
|
||||
affine_irq(1, "msm_vidc") # encoders
|
||||
affine_irq(1, "i2c_geni") # sensors
|
||||
|
||||
# *** GPU config ***
|
||||
# https://github.com/commaai/agnos-kernel-sdm845/blob/master/arch/arm64/boot/dts/qcom/sdm845-gpu.dtsi#L216
|
||||
sudo_write("0", "/sys/class/kgsl/kgsl-3d0/min_pwrlevel")
|
||||
sudo_write("0", "/sys/class/kgsl/kgsl-3d0/max_pwrlevel")
|
||||
sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_bus_on")
|
||||
sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_clk_on")
|
||||
sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_rail_on")
|
||||
sudo_write("1000", "/sys/class/kgsl/kgsl-3d0/idle_timer")
|
||||
sudo_write("performance", "/sys/class/kgsl/kgsl-3d0/devfreq/governor")
|
||||
sudo_write("710", "/sys/class/kgsl/kgsl-3d0/max_clock_mhz")
|
||||
|
||||
# setup governors
|
||||
sudo_write("performance", "/sys/class/devfreq/soc:qcom,cpubw/governor")
|
||||
sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu0/governor")
|
||||
sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu4/governor")
|
||||
|
||||
# *** VIDC (encoder) config ***
|
||||
sudo_write("N", "/sys/kernel/debug/msm_vidc/clock_scaling")
|
||||
sudo_write("Y", "/sys/kernel/debug/msm_vidc/disable_thermal_mitigation")
|
||||
|
||||
# pandad core
|
||||
affine_irq(3, "spi_geni") # SPI
|
||||
if "tici" in self.get_device_type():
|
||||
affine_irq(3, "xhci-hcd:usb3") # aux panda USB (or potentially anything else on USB)
|
||||
affine_irq(3, "xhci-hcd:usb1") # internal panda USB (also modem)
|
||||
try:
|
||||
pid = subprocess.check_output(["pgrep", "-f", "spi0"], encoding='utf8').strip()
|
||||
subprocess.call(["sudo", "chrt", "-f", "-p", "1", pid])
|
||||
subprocess.call(["sudo", "taskset", "-pc", "3", pid])
|
||||
except subprocess.CalledProcessException as e:
|
||||
print(str(e))
|
||||
|
||||
def configure_modem(self):
|
||||
sim_id = self.get_sim_info().get('sim_id', '')
|
||||
|
||||
modem = self.get_modem()
|
||||
try:
|
||||
manufacturer = str(modem.Get(MM_MODEM, 'Manufacturer', dbus_interface=DBUS_PROPS, timeout=TIMEOUT))
|
||||
except Exception:
|
||||
manufacturer = None
|
||||
|
||||
cmds = []
|
||||
|
||||
if self.get_device_type() in ("tici", "tizi"):
|
||||
# clear out old blue prime initial APN
|
||||
os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn="')
|
||||
|
||||
cmds += [
|
||||
# configure modem as data-centric
|
||||
'AT+QNVW=5280,0,"0102000000000000"',
|
||||
'AT+QNVFW="/nv/item_files/ims/IMS_enable",00',
|
||||
'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01',
|
||||
]
|
||||
if self.get_device_type() == "tizi":
|
||||
# SIM hot swap, not routed on tici
|
||||
cmds += [
|
||||
'AT+QSIMDET=1,0',
|
||||
'AT+QSIMSTAT=1',
|
||||
]
|
||||
elif manufacturer == 'Cavli Inc.':
|
||||
cmds += [
|
||||
'AT^SIMSWAP=1', # use SIM slot, instead of internal eSIM
|
||||
'AT$QCSIMSLEEP=0', # disable SIM sleep
|
||||
'AT$QCSIMCFG=SimPowerSave,0', # more sleep disable
|
||||
|
||||
# ethernet config
|
||||
'AT$QCPCFG=usbNet,0',
|
||||
'AT$QCNETDEVCTL=3,1',
|
||||
]
|
||||
else:
|
||||
# this modem gets upset with too many AT commands
|
||||
if sim_id is None or len(sim_id) == 0:
|
||||
cmds += [
|
||||
# SIM sleep disable
|
||||
'AT$QCSIMSLEEP=0',
|
||||
'AT$QCSIMCFG=SimPowerSave,0',
|
||||
|
||||
# ethernet config
|
||||
'AT$QCPCFG=usbNet,1',
|
||||
]
|
||||
|
||||
for cmd in cmds:
|
||||
try:
|
||||
modem.Command(cmd, math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# eSIM prime
|
||||
dest = "/etc/NetworkManager/system-connections/esim.nmconnection"
|
||||
if sim_id.startswith('8985235') and not os.path.exists(dest):
|
||||
with open(Path(__file__).parent/'esim.nmconnection') as f, tempfile.NamedTemporaryFile(mode='w') as tf:
|
||||
dat = f.read()
|
||||
dat = dat.replace("sim-id=", f"sim-id={sim_id}")
|
||||
tf.write(dat)
|
||||
tf.flush()
|
||||
|
||||
# needs to be root
|
||||
os.system(f"sudo cp {tf.name} {dest}")
|
||||
os.system(f"sudo nmcli con load {dest}")
|
||||
|
||||
def get_networks(self):
|
||||
r = {}
|
||||
|
||||
wlan = iwlist.scan()
|
||||
if wlan is not None:
|
||||
r['wlan'] = wlan
|
||||
|
||||
lte_info = self.get_network_info()
|
||||
if lte_info is not None:
|
||||
extra = lte_info['extra']
|
||||
|
||||
# <state>,"LTE",<is_tdd>,<mcc>,<mnc>,<cellid>,<pcid>,<earfcn>,<freq_band_ind>,
|
||||
# <ul_bandwidth>,<dl_bandwidth>,<tac>,<rsrp>,<rsrq>,<rssi>,<sinr>,<srxlev>
|
||||
if 'LTE' in extra:
|
||||
extra = extra.split(',')
|
||||
try:
|
||||
r['lte'] = [{
|
||||
"mcc": int(extra[3]),
|
||||
"mnc": int(extra[4]),
|
||||
"cid": int(extra[5], 16),
|
||||
"nmr": [{"pci": int(extra[6]), "earfcn": int(extra[7])}],
|
||||
}]
|
||||
except (ValueError, IndexError):
|
||||
pass
|
||||
|
||||
return r
|
||||
|
||||
def get_modem_data_usage(self):
|
||||
try:
|
||||
wwan = self.get_wwan()
|
||||
|
||||
# Ensure refresh rate is set so values don't go stale
|
||||
refresh_rate = wwan.Get(NM_DEV_STATS, 'RefreshRateMs', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
if refresh_rate != REFRESH_RATE_MS:
|
||||
u = type(refresh_rate)
|
||||
wwan.Set(NM_DEV_STATS, 'RefreshRateMs', u(REFRESH_RATE_MS), dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
|
||||
tx = wwan.Get(NM_DEV_STATS, 'TxBytes', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
rx = wwan.Get(NM_DEV_STATS, 'RxBytes', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
return int(tx), int(rx)
|
||||
except Exception:
|
||||
return -1, -1
|
||||
|
||||
def has_internal_panda(self):
|
||||
return True
|
||||
|
||||
def reset_internal_panda(self):
|
||||
gpio_init(GPIO.STM_RST_N, True)
|
||||
gpio_init(GPIO.STM_BOOT0, True)
|
||||
|
||||
gpio_set(GPIO.STM_RST_N, 1)
|
||||
gpio_set(GPIO.STM_BOOT0, 0)
|
||||
time.sleep(1)
|
||||
gpio_set(GPIO.STM_RST_N, 0)
|
||||
|
||||
def recover_internal_panda(self):
|
||||
gpio_init(GPIO.STM_RST_N, True)
|
||||
gpio_init(GPIO.STM_BOOT0, True)
|
||||
|
||||
gpio_set(GPIO.STM_RST_N, 1)
|
||||
gpio_set(GPIO.STM_BOOT0, 1)
|
||||
time.sleep(0.5)
|
||||
gpio_set(GPIO.STM_RST_N, 0)
|
||||
time.sleep(0.5)
|
||||
gpio_set(GPIO.STM_BOOT0, 0)
|
||||
|
||||
def booted(self):
|
||||
# this normally boots within 8s, but on rare occasions takes 30+s
|
||||
encoder_state = sudo_read("/sys/kernel/debug/msm_vidc/core0/info")
|
||||
if "Core state: 0" in encoder_state and (time.monotonic() < 60*2):
|
||||
return False
|
||||
return True
|
||||
|
||||
if __name__ == "__main__":
|
||||
t = Tici()
|
||||
t.configure_modem()
|
||||
t.initialize_hardware()
|
||||
t.set_power_save(False)
|
||||
print(t.get_sim_info())
|
||||
@@ -0,0 +1,28 @@
|
||||
-----BEGIN RSA PRIVATE KEY-----
|
||||
MIIEvAIBADANBgkqhkiG9w0BAQEFAASCBKYwggSiAgEAAoIBAQC+iXXq30Tq+J5N
|
||||
Kat3KWHCzcmwZ55nGh6WggAqECa5CasBlM9VeROpVu3beA+5h0MibRgbD4DMtVXB
|
||||
t6gEvZ8nd04E7eLA9LTZyFDZ7SkSOVj4oXOQsT0GnJmKrASW5KslTWqVzTfo2XCt
|
||||
Z+004ikLxmyFeBO8NOcErW1pa8gFdQDToH9FrA7kgysic/XVESTOoe7XlzRoe/eZ
|
||||
acEQ+jtnmFd21A4aEADkk00Ahjr0uKaJiLUAPatxs2icIXWpgYtfqqtaKF23wSt6
|
||||
1OTu6cAwXbOWr3m+IUSRUO0IRzEIQS3z1jfd1svgzSgSSwZ1Lhj4AoKxIEAIc8qJ
|
||||
rO4uymCJAgMBAAECggEBAISFevxHGdoL3Z5xkw6oO5SQKO2GxEeVhRzNgmu/HA+q
|
||||
x8OryqD6O1CWY4037kft6iWxlwiLOdwna2P25ueVM3LxqdQH2KS4DmlCx+kq6FwC
|
||||
gv063fQPMhC9LpWimvaQSPEC7VUPjQlo4tPY6sTTYBUOh0A1ihRm/x7juKuQCWix
|
||||
Cq8C/DVnB1X4mGj+W3nJc5TwVJtgJbbiBrq6PWrhvB/3qmkxHRL7dU2SBb2iNRF1
|
||||
LLY30dJx/cD73UDKNHrlrsjk3UJc29Mp4/MladKvUkRqNwlYxSuAtJV0nZ3+iFkL
|
||||
s3adSTHdJpClQer45R51rFDlVsDz2ZBpb/hRNRoGDuECgYEA6A1EixLq7QYOh3cb
|
||||
Xhyh3W4kpVvA/FPfKH1OMy3ONOD/Y9Oa+M/wthW1wSoRL2n+uuIW5OAhTIvIEivj
|
||||
6bAZsTT3twrvOrvYu9rx9aln4p8BhyvdjeW4kS7T8FP5ol6LoOt2sTP3T1LOuJPO
|
||||
uQvOjlKPKIMh3c3RFNWTnGzMPa0CgYEA0jNiPLxP3A2nrX0keKDI+VHuvOY88gdh
|
||||
0W5BuLMLovOIDk9aQFIbBbMuW1OTjHKv9NK+Lrw+YbCFqOGf1dU/UN5gSyE8lX/Q
|
||||
FsUGUqUZx574nJZnOIcy3ONOnQLcvHAQToLFAGUd7PWgP3CtHkt9hEv2koUwL4vo
|
||||
ikTP1u9Gkc0CgYEA2apoWxPZrY963XLKBxNQecYxNbLFaWq67t3rFnKm9E8BAICi
|
||||
4zUaE5J1tMVi7Vi9iks9Ml9SnNyZRQJKfQ+kaebHXbkyAaPmfv+26rqHKboA0uxA
|
||||
nDOZVwXX45zBkp6g1sdHxJx8JLoGEnkC9eyvSi0C//tRLx86OhLErXwYcNkCf1it
|
||||
VMRKrWYoXJTUNo6tRhvodM88UnnIo3u3CALjhgU4uC1RTMHV4ZCGBwiAOb8GozSl
|
||||
s5YD1E1iKwEULloHnK6BIh6P5v8q7J6uf/xdqoKMjlWBHgq6/roxKvkSPA1DOZ3l
|
||||
jTadcgKFnRUmc+JT9p/ZbCxkA/ALFg8++G+0ghECgYA8vG3M/utweLvq4RI7l7U7
|
||||
b+i2BajfK2OmzNi/xugfeLjY6k2tfQGRuv6ppTjehtji2uvgDWkgjJUgPfZpir3I
|
||||
RsVMUiFgloWGHETOy0Qvc5AwtqTJFLTD1Wza2uBilSVIEsg6Y83Gickh+ejOmEsY
|
||||
6co17RFaAZHwGfCFFjO76Q==
|
||||
-----END RSA PRIVATE KEY-----
|
||||
@@ -0,0 +1,35 @@
|
||||
import subprocess
|
||||
|
||||
|
||||
def scan(interface="wlan0"):
|
||||
result = []
|
||||
try:
|
||||
r = subprocess.check_output(["iwlist", interface, "scan"], encoding='utf8')
|
||||
|
||||
mac = None
|
||||
for line in r.split('\n'):
|
||||
if "Address" in line:
|
||||
# Based on the adapter eithere a percentage or dBm is returned
|
||||
# Add previous network in case no dBm signal level was seen
|
||||
if mac is not None:
|
||||
result.append({"mac": mac})
|
||||
mac = None
|
||||
|
||||
mac = line.split(' ')[-1]
|
||||
elif "dBm" in line:
|
||||
try:
|
||||
level = line.split('Signal level=')[1]
|
||||
rss = int(level.split(' ')[0])
|
||||
result.append({"mac": mac, "rss": rss})
|
||||
mac = None
|
||||
except ValueError:
|
||||
continue
|
||||
|
||||
# Add last network if no dBm was found
|
||||
if mac is not None:
|
||||
result.append({"mac": mac})
|
||||
|
||||
return result
|
||||
|
||||
except Exception:
|
||||
return None
|
||||
@@ -0,0 +1,32 @@
|
||||
# TODO: these are also defined in a header
|
||||
|
||||
# GPIO pin definitions
|
||||
class GPIO:
|
||||
# both GPIO_STM_RST_N and GPIO_LTE_RST_N are misnamed, they are high to reset
|
||||
HUB_RST_N = 30
|
||||
UBLOX_RST_N = 32
|
||||
UBLOX_SAFEBOOT_N = 33
|
||||
GNSS_PWR_EN = 34 # SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN
|
||||
|
||||
STM_RST_N = 124
|
||||
STM_BOOT0 = 134
|
||||
STM_PWR_EN_N = 41 # because STM32H7 RST doesn't generate a full power-on-reset
|
||||
|
||||
SIREN = 42
|
||||
SOM_ST_IO = 49
|
||||
|
||||
LTE_RST_N = 50
|
||||
LTE_PWRKEY = 116
|
||||
LTE_BOOT = 52
|
||||
|
||||
# GPIO_CAM0_DVDD_EN = /sys/kernel/debug/regulator/camera_rear_ldo
|
||||
CAM0_AVDD_EN = 8
|
||||
CAM0_RSTN = 9
|
||||
CAM1_RSTN = 7
|
||||
CAM2_RSTN = 12
|
||||
|
||||
# Sensor interrupts
|
||||
BMX055_ACCEL_INT = 21
|
||||
BMX055_GYRO_INT = 23
|
||||
BMX055_MAGN_INT = 87
|
||||
LSM_INT = 84
|
||||
Executable
+66
@@ -0,0 +1,66 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import time
|
||||
import datetime
|
||||
import numpy as np
|
||||
from collections import deque
|
||||
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
|
||||
|
||||
def read_power():
|
||||
with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/power1_input") as f:
|
||||
return int(f.read()) / 1e6
|
||||
|
||||
def sample_power(seconds=5) -> list[float]:
|
||||
rate = 123
|
||||
rk = Ratekeeper(rate, print_delay_threshold=None)
|
||||
|
||||
pwrs = []
|
||||
for _ in range(rate*seconds):
|
||||
pwrs.append(read_power())
|
||||
rk.keep_time()
|
||||
return pwrs
|
||||
|
||||
def get_power(seconds=5):
|
||||
pwrs = sample_power(seconds)
|
||||
return np.mean(pwrs)
|
||||
|
||||
def wait_for_power(min_pwr, max_pwr, min_secs_in_range, timeout):
|
||||
start_time = time.monotonic()
|
||||
pwrs = deque([min_pwr - 1.]*min_secs_in_range, maxlen=min_secs_in_range)
|
||||
while (time.monotonic() - start_time < timeout):
|
||||
pwrs.append(get_power(1))
|
||||
if all(min_pwr <= p <= max_pwr for p in pwrs):
|
||||
break
|
||||
return np.mean(pwrs)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
duration = None
|
||||
if len(sys.argv) > 1:
|
||||
duration = int(sys.argv[1])
|
||||
|
||||
rate = 23
|
||||
rk = Ratekeeper(rate, print_delay_threshold=None)
|
||||
fltr = FirstOrderFilter(0, 5, 1. / rate, initialized=False)
|
||||
|
||||
measurements = []
|
||||
start_time = time.monotonic()
|
||||
|
||||
try:
|
||||
while duration is None or time.monotonic() - start_time < duration:
|
||||
fltr.update(read_power())
|
||||
if rk.frame % rate == 0:
|
||||
measurements.append(fltr.x)
|
||||
t = datetime.timedelta(seconds=time.monotonic() - start_time)
|
||||
avg = sum(measurements) / len(measurements)
|
||||
print(f"Now: {fltr.x:.2f} W, Avg: {avg:.2f} W over {t}")
|
||||
rk.keep_time()
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
|
||||
t = datetime.timedelta(seconds=time.monotonic() - start_time)
|
||||
avg = sum(measurements) / len(measurements)
|
||||
print(f"\nAverage power: {avg:.2f}W over {t}")
|
||||
+9
@@ -0,0 +1,9 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from openpilot.system.hardware.tici.power_monitor import sample_power
|
||||
|
||||
if __name__ == '__main__':
|
||||
print("measuring for 5 seconds")
|
||||
for _ in range(3):
|
||||
pwrs = sample_power()
|
||||
print(f"mean {np.mean(pwrs):.2f} std {np.std(pwrs):.2f}")
|
||||
Executable
+18
@@ -0,0 +1,18 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
#nmcli connection modify --temporary lte gsm.home-only yes
|
||||
#nmcli connection modify --temporary lte gsm.auto-config yes
|
||||
#nmcli connection modify --temporary lte connection.autoconnect-retries 20
|
||||
sudo nmcli connection reload
|
||||
|
||||
sudo systemctl stop ModemManager
|
||||
nmcli con down lte
|
||||
nmcli con down blue-prime
|
||||
|
||||
# power cycle modem
|
||||
/usr/comma/lte/lte.sh stop_blocking
|
||||
/usr/comma/lte/lte.sh start
|
||||
|
||||
sudo systemctl restart NetworkManager
|
||||
#sudo systemctl restart ModemManager
|
||||
sudo ModemManager --debug
|
||||
Executable
BIN
Binary file not shown.
@@ -0,0 +1 @@
|
||||
logcatd
|
||||
@@ -0,0 +1,3 @@
|
||||
Import('env', 'messaging', 'common')
|
||||
|
||||
env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[messaging, common, 'systemd'])
|
||||
Executable
BIN
Binary file not shown.
@@ -0,0 +1,4 @@
|
||||
loggerd
|
||||
encoderd
|
||||
bootlog
|
||||
tests/test_logger
|
||||
@@ -0,0 +1,26 @@
|
||||
Import('env', 'arch', 'messaging', 'common', 'visionipc')
|
||||
|
||||
libs = [common, messaging, visionipc,
|
||||
'avformat', 'avcodec', 'avutil',
|
||||
'yuv', 'OpenCL', 'pthread', 'zstd']
|
||||
|
||||
src = ['logger.cc', 'zstd_writer.cc', 'video_writer.cc', 'encoder/encoder.cc', 'encoder/v4l_encoder.cc', 'encoder/jpeg_encoder.cc']
|
||||
if arch != "larch64":
|
||||
src += ['encoder/ffmpeg_encoder.cc']
|
||||
|
||||
if arch == "Darwin":
|
||||
# fix OpenCL
|
||||
del libs[libs.index('OpenCL')]
|
||||
env['FRAMEWORKS'] = ['OpenCL']
|
||||
# exclude v4l
|
||||
del src[src.index('encoder/v4l_encoder.cc')]
|
||||
|
||||
logger_lib = env.Library('logger', src)
|
||||
libs.insert(0, logger_lib)
|
||||
|
||||
env.Program('loggerd', ['loggerd.cc'], LIBS=libs)
|
||||
env.Program('encoderd', ['encoderd.cc'], LIBS=libs + ["jpeg"])
|
||||
env.Program('bootlog.cc', LIBS=libs)
|
||||
|
||||
if GetOption('extras'):
|
||||
env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_logger.cc', 'tests/test_zstd_writer.cc'], LIBS=libs + ['curl', 'crypto'])
|
||||
Executable
BIN
Binary file not shown.
@@ -0,0 +1,29 @@
|
||||
import os
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
CAMERA_FPS = 20
|
||||
SEGMENT_LENGTH = 60
|
||||
|
||||
STATS_DIR_FILE_LIMIT = 10000
|
||||
STATS_SOCKET = "ipc:///tmp/stats"
|
||||
STATS_FLUSH_TIME_S = 60
|
||||
|
||||
def get_available_percent(default: float) -> float:
|
||||
try:
|
||||
statvfs = os.statvfs(Paths.log_root())
|
||||
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
|
||||
except OSError:
|
||||
available_percent = default
|
||||
|
||||
return available_percent
|
||||
|
||||
|
||||
def get_available_bytes(default: int) -> int:
|
||||
try:
|
||||
statvfs = os.statvfs(Paths.log_root())
|
||||
available_bytes = statvfs.f_bavail * statvfs.f_frsize
|
||||
except OSError:
|
||||
available_bytes = default
|
||||
|
||||
return available_bytes
|
||||
Executable
+80
@@ -0,0 +1,80 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import shutil
|
||||
import threading
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.loggerd.config import get_available_bytes, get_available_percent
|
||||
from openpilot.system.loggerd.uploader import listdir_by_creation
|
||||
from openpilot.system.loggerd.xattr_cache import getxattr
|
||||
|
||||
MIN_BYTES = 5 * 1024 * 1024 * 1024
|
||||
MIN_PERCENT = 10
|
||||
|
||||
DELETE_LAST = ['boot', 'crash']
|
||||
|
||||
PRESERVE_ATTR_NAME = 'user.preserve'
|
||||
PRESERVE_ATTR_VALUE = b'1'
|
||||
PRESERVE_COUNT = 5
|
||||
|
||||
|
||||
def has_preserve_xattr(d: str) -> bool:
|
||||
return getxattr(os.path.join(Paths.log_root(), d), PRESERVE_ATTR_NAME) == PRESERVE_ATTR_VALUE
|
||||
|
||||
|
||||
def get_preserved_segments(dirs_by_creation: list[str]) -> set[str]:
|
||||
# skip deleting most recent N preserved segments (and their prior segment)
|
||||
preserved = set()
|
||||
for n, d in enumerate(filter(has_preserve_xattr, reversed(dirs_by_creation))):
|
||||
if n == PRESERVE_COUNT:
|
||||
break
|
||||
date_str, _, seg_str = d.rpartition("--")
|
||||
|
||||
# ignore non-segment directories
|
||||
if not date_str:
|
||||
continue
|
||||
try:
|
||||
seg_num = int(seg_str)
|
||||
except ValueError:
|
||||
continue
|
||||
|
||||
# preserve segment and two prior
|
||||
for _seg_num in range(max(0, seg_num - 2), seg_num + 1):
|
||||
preserved.add(f"{date_str}--{_seg_num}")
|
||||
|
||||
return preserved
|
||||
|
||||
|
||||
def deleter_thread(exit_event: threading.Event):
|
||||
while not exit_event.is_set():
|
||||
out_of_bytes = get_available_bytes(default=MIN_BYTES + 1) < MIN_BYTES
|
||||
out_of_percent = get_available_percent(default=MIN_PERCENT + 1) < MIN_PERCENT
|
||||
|
||||
if out_of_percent or out_of_bytes:
|
||||
dirs = listdir_by_creation(Paths.log_root())
|
||||
preserved_dirs = get_preserved_segments(dirs)
|
||||
|
||||
# remove the earliest directory we can
|
||||
for delete_dir in sorted(dirs, key=lambda d: (d in DELETE_LAST, d in preserved_dirs)):
|
||||
delete_path = os.path.join(Paths.log_root(), delete_dir)
|
||||
|
||||
if any(name.endswith(".lock") for name in os.listdir(delete_path)):
|
||||
continue
|
||||
|
||||
try:
|
||||
cloudlog.info(f"deleting {delete_path}")
|
||||
shutil.rmtree(delete_path)
|
||||
break
|
||||
except OSError:
|
||||
cloudlog.exception(f"issue deleting {delete_path}")
|
||||
exit_event.wait(.1)
|
||||
else:
|
||||
exit_event.wait(30)
|
||||
|
||||
|
||||
def main():
|
||||
deleter_thread(threading.Event())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,42 @@
|
||||
#pragma once
|
||||
|
||||
// has to be in this order
|
||||
#ifdef __linux__
|
||||
#include "third_party/linux/include/v4l2-controls.h"
|
||||
#include <linux/videodev2.h>
|
||||
#else
|
||||
#define V4L2_BUF_FLAG_KEYFRAME 8
|
||||
#endif
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "msgq/visionipc/visionipc.h"
|
||||
#include "common/queue.h"
|
||||
#include "system/loggerd/loggerd.h"
|
||||
|
||||
class VideoEncoder {
|
||||
public:
|
||||
VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in_height);
|
||||
virtual ~VideoEncoder() {}
|
||||
virtual int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) = 0;
|
||||
virtual void encoder_open() = 0;
|
||||
virtual void encoder_close() = 0;
|
||||
|
||||
void publisher_publish(int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);
|
||||
|
||||
protected:
|
||||
int in_width, in_height;
|
||||
int out_width, out_height;
|
||||
const EncoderInfo encoder_info;
|
||||
|
||||
private:
|
||||
// total frames encoded
|
||||
int cnt = 0;
|
||||
std::unique_ptr<PubMaster> pm;
|
||||
std::vector<capnp::byte> msg_cache;
|
||||
};
|
||||
@@ -0,0 +1,34 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
extern "C" {
|
||||
#include <libavcodec/avcodec.h>
|
||||
#include <libavformat/avformat.h>
|
||||
#include <libavutil/imgutils.h>
|
||||
}
|
||||
|
||||
#include "system/loggerd/encoder/encoder.h"
|
||||
#include "system/loggerd/loggerd.h"
|
||||
|
||||
class FfmpegEncoder : public VideoEncoder {
|
||||
public:
|
||||
FfmpegEncoder(const EncoderInfo &encoder_info, int in_width, int in_height);
|
||||
~FfmpegEncoder();
|
||||
int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra);
|
||||
void encoder_open();
|
||||
void encoder_close();
|
||||
|
||||
private:
|
||||
int segment_num = -1;
|
||||
int counter = 0;
|
||||
bool is_open = false;
|
||||
|
||||
AVCodecContext *codec_ctx;
|
||||
AVFrame *frame = NULL;
|
||||
std::vector<uint8_t> convert_buf;
|
||||
std::vector<uint8_t> downscale_buf;
|
||||
};
|
||||
@@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <jpeglib.h>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "msgq/visionipc/visionbuf.h"
|
||||
|
||||
class JpegEncoder {
|
||||
public:
|
||||
JpegEncoder(const std::string &pusblish_name, int width, int height);
|
||||
~JpegEncoder();
|
||||
void pushThumbnail(VisionBuf *buf, const VisionIpcBufExtra &extra);
|
||||
|
||||
private:
|
||||
void generateThumbnail(const uint8_t *y, const uint8_t *uv, int width, int height, int stride);
|
||||
void compressToJpeg(uint8_t *y_plane, uint8_t *u_plane, uint8_t *v_plane);
|
||||
|
||||
int thumbnail_width;
|
||||
int thumbnail_height;
|
||||
std::string publish_name;
|
||||
std::vector<uint8_t> yuv_buffer;
|
||||
std::unique_ptr<PubMaster> pm;
|
||||
|
||||
// JPEG output buffer
|
||||
unsigned char* out_buffer = nullptr;
|
||||
unsigned long out_size = 0;
|
||||
};
|
||||
@@ -0,0 +1,31 @@
|
||||
#pragma once
|
||||
|
||||
#include "common/queue.h"
|
||||
#include "system/loggerd/encoder/encoder.h"
|
||||
|
||||
#define BUF_IN_COUNT 7
|
||||
#define BUF_OUT_COUNT 6
|
||||
|
||||
class V4LEncoder : public VideoEncoder {
|
||||
public:
|
||||
V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_height);
|
||||
~V4LEncoder();
|
||||
int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra);
|
||||
void encoder_open();
|
||||
void encoder_close();
|
||||
|
||||
private:
|
||||
int fd;
|
||||
|
||||
bool is_open = false;
|
||||
int segment_num = -1;
|
||||
int counter = 0;
|
||||
|
||||
SafeQueue<VisionIpcBufExtra> extras;
|
||||
|
||||
static void dequeue_handler(V4LEncoder *e);
|
||||
std::thread dequeue_handler_thread;
|
||||
|
||||
VisionBuf buf_out[BUF_OUT_COUNT];
|
||||
SafeQueue<unsigned int> free_buf_in;
|
||||
};
|
||||
Executable
BIN
Binary file not shown.
@@ -0,0 +1,37 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "common/util.h"
|
||||
#include "system/hardware/hw.h"
|
||||
#include "system/loggerd/zstd_writer.h"
|
||||
|
||||
constexpr int LOG_COMPRESSION_LEVEL = 10;
|
||||
|
||||
typedef cereal::Sentinel::SentinelType SentinelType;
|
||||
|
||||
class LoggerState {
|
||||
public:
|
||||
LoggerState(const std::string& log_root = Path::log_root());
|
||||
~LoggerState();
|
||||
bool next();
|
||||
void write(uint8_t* data, size_t size, bool in_qlog);
|
||||
inline int segment() const { return part; }
|
||||
inline const std::string& segmentPath() const { return segment_path; }
|
||||
inline const std::string& routeName() const { return route_name; }
|
||||
inline void write(kj::ArrayPtr<kj::byte> bytes, bool in_qlog) { write(bytes.begin(), bytes.size(), in_qlog); }
|
||||
inline void setExitSignal(int signal) { exit_signal = signal; }
|
||||
|
||||
protected:
|
||||
int part = -1, exit_signal = 0;
|
||||
std::string route_path, route_name, segment_path, lock_file;
|
||||
kj::Array<capnp::word> init_data;
|
||||
std::unique_ptr<ZstdFileWriter> rlog, qlog;
|
||||
};
|
||||
|
||||
kj::Array<capnp::word> logger_build_init_data();
|
||||
std::string logger_get_identifier(std::string key);
|
||||
std::string zstd_decompress(const std::string &in);
|
||||
Executable
BIN
Binary file not shown.
@@ -0,0 +1,149 @@
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "cereal/services.h"
|
||||
#include "msgq/visionipc/visionipc_client.h"
|
||||
#include "system/hardware/hw.h"
|
||||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/util.h"
|
||||
|
||||
#include "system/loggerd/logger.h"
|
||||
|
||||
constexpr int MAIN_FPS = 20;
|
||||
const int MAIN_BITRATE = 1e7;
|
||||
const int LIVESTREAM_BITRATE = 1e6;
|
||||
const int QCAM_BITRATE = 256000;
|
||||
|
||||
#define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead
|
||||
|
||||
#define INIT_ENCODE_FUNCTIONS(encode_type) \
|
||||
.get_encode_data_func = &cereal::Event::Reader::get##encode_type##Data, \
|
||||
.set_encode_idx_func = &cereal::Event::Builder::set##encode_type##Idx, \
|
||||
.init_encode_data_func = &cereal::Event::Builder::init##encode_type##Data
|
||||
|
||||
const bool LOGGERD_TEST = getenv("LOGGERD_TEST");
|
||||
const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) : 60;
|
||||
|
||||
constexpr char PRESERVE_ATTR_NAME[] = "user.preserve";
|
||||
constexpr char PRESERVE_ATTR_VALUE = '1';
|
||||
class EncoderInfo {
|
||||
public:
|
||||
const char *publish_name;
|
||||
const char *thumbnail_name = NULL;
|
||||
const char *filename = NULL;
|
||||
bool record = true;
|
||||
int frame_width = -1;
|
||||
int frame_height = -1;
|
||||
int fps = MAIN_FPS;
|
||||
int bitrate = MAIN_BITRATE;
|
||||
cereal::EncodeIndex::Type encode_type = Hardware::PC() ? cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS
|
||||
: cereal::EncodeIndex::Type::FULL_H_E_V_C;
|
||||
::cereal::EncodeData::Reader (cereal::Event::Reader::*get_encode_data_func)() const;
|
||||
void (cereal::Event::Builder::*set_encode_idx_func)(::cereal::EncodeIndex::Reader);
|
||||
cereal::EncodeData::Builder (cereal::Event::Builder::*init_encode_data_func)();
|
||||
};
|
||||
|
||||
class LogCameraInfo {
|
||||
public:
|
||||
const char *thread_name;
|
||||
int fps = MAIN_FPS;
|
||||
VisionStreamType stream_type;
|
||||
std::vector<EncoderInfo> encoder_infos;
|
||||
};
|
||||
|
||||
const EncoderInfo main_road_encoder_info = {
|
||||
.publish_name = "roadEncodeData",
|
||||
.thumbnail_name = "thumbnail",
|
||||
.filename = "fcamera.hevc",
|
||||
INIT_ENCODE_FUNCTIONS(RoadEncode),
|
||||
};
|
||||
|
||||
const EncoderInfo main_wide_road_encoder_info = {
|
||||
.publish_name = "wideRoadEncodeData",
|
||||
.filename = "ecamera.hevc",
|
||||
INIT_ENCODE_FUNCTIONS(WideRoadEncode),
|
||||
};
|
||||
|
||||
const EncoderInfo main_driver_encoder_info = {
|
||||
.publish_name = "driverEncodeData",
|
||||
.filename = "dcamera.hevc",
|
||||
.record = !getenv("DISABLE_DRIVER") && Params().getBool("RecordFront"),
|
||||
INIT_ENCODE_FUNCTIONS(DriverEncode),
|
||||
};
|
||||
|
||||
const EncoderInfo stream_road_encoder_info = {
|
||||
.publish_name = "livestreamRoadEncodeData",
|
||||
//.thumbnail_name = "thumbnail",
|
||||
.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
|
||||
.record = false,
|
||||
.bitrate = LIVESTREAM_BITRATE,
|
||||
INIT_ENCODE_FUNCTIONS(LivestreamRoadEncode),
|
||||
};
|
||||
|
||||
const EncoderInfo stream_wide_road_encoder_info = {
|
||||
.publish_name = "livestreamWideRoadEncodeData",
|
||||
.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
|
||||
.record = false,
|
||||
.bitrate = LIVESTREAM_BITRATE,
|
||||
INIT_ENCODE_FUNCTIONS(LivestreamWideRoadEncode),
|
||||
};
|
||||
|
||||
const EncoderInfo stream_driver_encoder_info = {
|
||||
.publish_name = "livestreamDriverEncodeData",
|
||||
.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
|
||||
.record = false,
|
||||
.bitrate = LIVESTREAM_BITRATE,
|
||||
INIT_ENCODE_FUNCTIONS(LivestreamDriverEncode),
|
||||
};
|
||||
|
||||
const EncoderInfo qcam_encoder_info = {
|
||||
.publish_name = "qRoadEncodeData",
|
||||
.filename = "qcamera.ts",
|
||||
.bitrate = QCAM_BITRATE,
|
||||
.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
|
||||
.frame_width = 526,
|
||||
.frame_height = 330,
|
||||
INIT_ENCODE_FUNCTIONS(QRoadEncode),
|
||||
};
|
||||
|
||||
const LogCameraInfo road_camera_info{
|
||||
.thread_name = "road_cam_encoder",
|
||||
.stream_type = VISION_STREAM_ROAD,
|
||||
.encoder_infos = {main_road_encoder_info, qcam_encoder_info}
|
||||
};
|
||||
|
||||
const LogCameraInfo wide_road_camera_info{
|
||||
.thread_name = "wide_road_cam_encoder",
|
||||
.stream_type = VISION_STREAM_WIDE_ROAD,
|
||||
.encoder_infos = {main_wide_road_encoder_info}
|
||||
};
|
||||
|
||||
const LogCameraInfo driver_camera_info{
|
||||
.thread_name = "driver_cam_encoder",
|
||||
.stream_type = VISION_STREAM_DRIVER,
|
||||
.encoder_infos = {main_driver_encoder_info}
|
||||
};
|
||||
|
||||
const LogCameraInfo stream_road_camera_info{
|
||||
.thread_name = "road_cam_encoder",
|
||||
.stream_type = VISION_STREAM_ROAD,
|
||||
.encoder_infos = {stream_road_encoder_info}
|
||||
};
|
||||
|
||||
const LogCameraInfo stream_wide_road_camera_info{
|
||||
.thread_name = "wide_road_cam_encoder",
|
||||
.stream_type = VISION_STREAM_WIDE_ROAD,
|
||||
.encoder_infos = {stream_wide_road_encoder_info}
|
||||
};
|
||||
|
||||
const LogCameraInfo stream_driver_camera_info{
|
||||
.thread_name = "driver_cam_encoder",
|
||||
.stream_type = VISION_STREAM_DRIVER,
|
||||
.encoder_infos = {stream_driver_encoder_info}
|
||||
};
|
||||
|
||||
const LogCameraInfo cameras_logged[] = {road_camera_info, wide_road_camera_info, driver_camera_info};
|
||||
const LogCameraInfo stream_cameras_logged[] = {stream_road_camera_info, stream_wide_road_camera_info, stream_driver_camera_info};
|
||||
Executable
+273
@@ -0,0 +1,273 @@
|
||||
#!/usr/bin/env python3
|
||||
import json
|
||||
import os
|
||||
import random
|
||||
import requests
|
||||
import threading
|
||||
import time
|
||||
import traceback
|
||||
import datetime
|
||||
from collections.abc import Iterator
|
||||
|
||||
from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.api import Api
|
||||
from openpilot.common.file_helpers import get_upload_stream
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import set_core_affinity
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from openpilot.system.loggerd.xattr_cache import getxattr, setxattr
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
UPLOAD_ATTR_NAME = 'user.upload'
|
||||
UPLOAD_ATTR_VALUE = b'1'
|
||||
|
||||
MAX_UPLOAD_SIZES = {
|
||||
"qlog": 25*1e6, # can't be too restrictive here since we use qlogs to find
|
||||
# bugs, including ones that can cause massive log sizes
|
||||
"qcam": 5*1e6,
|
||||
}
|
||||
|
||||
allow_sleep = bool(os.getenv("UPLOADER_SLEEP", "1"))
|
||||
force_wifi = os.getenv("FORCEWIFI") is not None
|
||||
fake_upload = os.getenv("FAKEUPLOAD") is not None
|
||||
|
||||
|
||||
class FakeRequest:
|
||||
def __init__(self):
|
||||
self.headers = {"Content-Length": "0"}
|
||||
|
||||
|
||||
class FakeResponse:
|
||||
def __init__(self):
|
||||
self.status_code = 200
|
||||
self.request = FakeRequest()
|
||||
|
||||
|
||||
def get_directory_sort(d: str) -> list[str]:
|
||||
# ensure old format is sorted sooner
|
||||
o = ["0", ] if d.startswith("2024-") else ["1", ]
|
||||
return o + [s.rjust(10, '0') for s in d.rsplit('--', 1)]
|
||||
|
||||
def listdir_by_creation(d: str) -> list[str]:
|
||||
if not os.path.isdir(d):
|
||||
return []
|
||||
|
||||
try:
|
||||
paths = [f for f in os.listdir(d) if os.path.isdir(os.path.join(d, f))]
|
||||
paths = sorted(paths, key=get_directory_sort)
|
||||
return paths
|
||||
except OSError:
|
||||
cloudlog.exception("listdir_by_creation failed")
|
||||
return []
|
||||
|
||||
def clear_locks(root: str) -> None:
|
||||
for logdir in os.listdir(root):
|
||||
path = os.path.join(root, logdir)
|
||||
try:
|
||||
for fname in os.listdir(path):
|
||||
if fname.endswith(".lock"):
|
||||
os.unlink(os.path.join(path, fname))
|
||||
except OSError:
|
||||
cloudlog.exception("clear_locks failed")
|
||||
|
||||
|
||||
class Uploader:
|
||||
def __init__(self, dongle_id: str, root: str):
|
||||
self.dongle_id = dongle_id
|
||||
self.api = Api(dongle_id)
|
||||
self.root = root
|
||||
|
||||
self.params = Params()
|
||||
|
||||
# stats for last successfully uploaded file
|
||||
self.last_filename = ""
|
||||
|
||||
self.immediate_folders = ["crash/", "boot/"]
|
||||
self.immediate_priority = {"qlog": 0, "qlog.zst": 0, "qcamera.ts": 1}
|
||||
|
||||
def list_upload_files(self, metered: bool) -> Iterator[tuple[str, str, str]]:
|
||||
r = self.params.get("AthenadRecentlyViewedRoutes", encoding="utf8")
|
||||
requested_routes = [] if r is None else r.split(",")
|
||||
|
||||
for logdir in listdir_by_creation(self.root):
|
||||
path = os.path.join(self.root, logdir)
|
||||
try:
|
||||
names = os.listdir(path)
|
||||
except OSError:
|
||||
continue
|
||||
|
||||
if any(name.endswith(".lock") for name in names):
|
||||
continue
|
||||
|
||||
for name in sorted(names, key=lambda n: self.immediate_priority.get(n, 1000)):
|
||||
key = os.path.join(logdir, name)
|
||||
fn = os.path.join(path, name)
|
||||
# skip files already uploaded
|
||||
try:
|
||||
ctime = os.path.getctime(fn)
|
||||
is_uploaded = getxattr(fn, UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE
|
||||
except OSError:
|
||||
cloudlog.event("uploader_getxattr_failed", key=key, fn=fn)
|
||||
# deleter could have deleted, so skip
|
||||
continue
|
||||
if is_uploaded:
|
||||
continue
|
||||
|
||||
# limit uploading on metered connections
|
||||
if metered:
|
||||
dt = datetime.timedelta(hours=12)
|
||||
if logdir in self.immediate_folders and (datetime.datetime.now() - datetime.datetime.fromtimestamp(ctime)) < dt:
|
||||
continue
|
||||
|
||||
if name == "qcamera.ts" and not any(logdir.startswith(r.split('|')[-1]) for r in requested_routes):
|
||||
continue
|
||||
|
||||
yield name, key, fn
|
||||
|
||||
def next_file_to_upload(self, metered: bool) -> tuple[str, str, str] | None:
|
||||
upload_files = list(self.list_upload_files(metered))
|
||||
|
||||
for name, key, fn in upload_files:
|
||||
if any(f in fn for f in self.immediate_folders):
|
||||
return name, key, fn
|
||||
|
||||
for name, key, fn in upload_files:
|
||||
if name in self.immediate_priority:
|
||||
return name, key, fn
|
||||
|
||||
return None
|
||||
|
||||
def do_upload(self, key: str, fn: str):
|
||||
url_resp = self.api.get("v1.4/" + self.dongle_id + "/upload_url/", timeout=10, path=key, access_token=self.api.get_token())
|
||||
if url_resp.status_code == 412:
|
||||
return url_resp
|
||||
|
||||
url_resp_json = json.loads(url_resp.text)
|
||||
url = url_resp_json['url']
|
||||
headers = url_resp_json['headers']
|
||||
cloudlog.debug("upload_url v1.4 %s %s", url, str(headers))
|
||||
|
||||
if fake_upload:
|
||||
return FakeResponse()
|
||||
|
||||
stream = None
|
||||
try:
|
||||
compress = key.endswith('.zst') and not fn.endswith('.zst')
|
||||
stream, _ = get_upload_stream(fn, compress)
|
||||
response = requests.put(url, data=stream, headers=headers, timeout=10)
|
||||
return response
|
||||
finally:
|
||||
if stream:
|
||||
stream.close()
|
||||
|
||||
def upload(self, name: str, key: str, fn: str, network_type: int, metered: bool) -> bool:
|
||||
try:
|
||||
sz = os.path.getsize(fn)
|
||||
except OSError:
|
||||
cloudlog.exception("upload: getsize failed")
|
||||
return False
|
||||
|
||||
cloudlog.event("upload_start", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
|
||||
if sz == 0:
|
||||
# tag files of 0 size as uploaded
|
||||
success = True
|
||||
elif name in MAX_UPLOAD_SIZES and sz > MAX_UPLOAD_SIZES[name]:
|
||||
cloudlog.event("uploader_too_large", key=key, fn=fn, sz=sz)
|
||||
success = True
|
||||
else:
|
||||
start_time = time.monotonic()
|
||||
|
||||
stat = None
|
||||
last_exc = None
|
||||
try:
|
||||
stat = self.do_upload(key, fn)
|
||||
except Exception as e:
|
||||
last_exc = (e, traceback.format_exc())
|
||||
|
||||
if stat is not None and stat.status_code in (200, 201, 401, 403, 412):
|
||||
self.last_filename = fn
|
||||
dt = time.monotonic() - start_time
|
||||
if stat.status_code == 412:
|
||||
cloudlog.event("upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
else:
|
||||
content_length = int(stat.request.headers.get("Content-Length", 0))
|
||||
speed = (content_length / 1e6) / dt
|
||||
cloudlog.event("upload_success", key=key, fn=fn, sz=sz, content_length=content_length,
|
||||
network_type=network_type, metered=metered, speed=speed)
|
||||
success = True
|
||||
else:
|
||||
success = False
|
||||
cloudlog.event("upload_failed", stat=stat, exc=last_exc, key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
|
||||
if success:
|
||||
# tag file as uploaded
|
||||
try:
|
||||
setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE)
|
||||
except OSError:
|
||||
cloudlog.event("uploader_setxattr_failed", exc=last_exc, key=key, fn=fn, sz=sz)
|
||||
|
||||
return success
|
||||
|
||||
|
||||
def step(self, network_type: int, metered: bool) -> bool | None:
|
||||
d = self.next_file_to_upload(metered)
|
||||
if d is None:
|
||||
return None
|
||||
|
||||
name, key, fn = d
|
||||
|
||||
# qlogs and bootlogs need to be compressed before uploading
|
||||
if key.endswith(('qlog', 'rlog')) or (key.startswith('boot/') and not key.endswith('.zst')):
|
||||
key += ".zst"
|
||||
|
||||
return self.upload(name, key, fn, network_type, metered)
|
||||
|
||||
|
||||
def main(exit_event: threading.Event = None) -> None:
|
||||
if exit_event is None:
|
||||
exit_event = threading.Event()
|
||||
|
||||
try:
|
||||
set_core_affinity([0, 1, 2, 3])
|
||||
except Exception:
|
||||
cloudlog.exception("failed to set core affinity")
|
||||
|
||||
clear_locks(Paths.log_root())
|
||||
|
||||
params = Params()
|
||||
dongle_id = params.get("DongleId", encoding='utf8')
|
||||
|
||||
if dongle_id is None:
|
||||
cloudlog.info("uploader missing dongle_id")
|
||||
raise Exception("uploader can't start without dongle id")
|
||||
|
||||
sm = messaging.SubMaster(['deviceState'])
|
||||
uploader = Uploader(dongle_id, Paths.log_root())
|
||||
|
||||
backoff = 0.1
|
||||
while not exit_event.is_set():
|
||||
sm.update(0)
|
||||
offroad = params.get_bool("IsOffroad")
|
||||
network_type = sm['deviceState'].networkType if not force_wifi else NetworkType.wifi
|
||||
if network_type == NetworkType.none:
|
||||
if allow_sleep:
|
||||
time.sleep(60 if offroad else 5)
|
||||
continue
|
||||
|
||||
success = uploader.step(sm['deviceState'].networkType.raw, sm['deviceState'].networkMetered)
|
||||
if success is None:
|
||||
backoff = 60 if offroad else 5
|
||||
elif success:
|
||||
backoff = 0.1
|
||||
else:
|
||||
cloudlog.info("upload backoff %r", backoff)
|
||||
backoff = min(backoff*2, 120)
|
||||
if allow_sleep:
|
||||
time.sleep(backoff + random.uniform(0, backoff))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,25 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
extern "C" {
|
||||
#include <libavformat/avformat.h>
|
||||
#include <libavcodec/avcodec.h>
|
||||
}
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
|
||||
class VideoWriter {
|
||||
public:
|
||||
VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec);
|
||||
void write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe);
|
||||
~VideoWriter();
|
||||
private:
|
||||
std::string vid_path, lock_path;
|
||||
FILE *of = nullptr;
|
||||
|
||||
AVCodecContext *codec_ctx;
|
||||
AVFormatContext *ofmt_ctx;
|
||||
AVStream *out_stream;
|
||||
bool remuxing;
|
||||
};
|
||||
@@ -0,0 +1,22 @@
|
||||
import os
|
||||
import errno
|
||||
|
||||
_cached_attributes: dict[tuple, bytes | None] = {}
|
||||
|
||||
def getxattr(path: str, attr_name: str) -> bytes | None:
|
||||
key = (path, attr_name)
|
||||
if key not in _cached_attributes:
|
||||
try:
|
||||
response = os.getxattr(path, attr_name)
|
||||
except OSError as e:
|
||||
# ENODATA means attribute hasn't been set
|
||||
if e.errno == errno.ENODATA:
|
||||
response = None
|
||||
else:
|
||||
raise
|
||||
_cached_attributes[key] = response
|
||||
return _cached_attributes[key]
|
||||
|
||||
def setxattr(path: str, attr_name: str, attr_value: bytes) -> None:
|
||||
_cached_attributes.pop((path, attr_name), None)
|
||||
return os.setxattr(path, attr_name, attr_value)
|
||||
@@ -0,0 +1,24 @@
|
||||
#pragma once
|
||||
|
||||
#include <zstd.h>
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <capnp/common.h>
|
||||
|
||||
class ZstdFileWriter {
|
||||
public:
|
||||
ZstdFileWriter(const std::string &filename, int compression_level);
|
||||
~ZstdFileWriter();
|
||||
void write(void* data, size_t size);
|
||||
inline void write(kj::ArrayPtr<capnp::byte> array) { write(array.begin(), array.size()); }
|
||||
|
||||
private:
|
||||
void flushCache(bool last_chunk);
|
||||
|
||||
size_t input_cache_capacity_ = 0;
|
||||
std::vector<char> input_cache_;
|
||||
std::vector<char> output_buffer_;
|
||||
ZSTD_CStream *cstream_;
|
||||
FILE* file_ = nullptr;
|
||||
};
|
||||
Executable
+55
@@ -0,0 +1,55 @@
|
||||
#!/usr/bin/env python3
|
||||
import zmq
|
||||
from typing import NoReturn
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.logging_extra import SwagLogFileFormatter
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from openpilot.common.swaglog import get_file_handler
|
||||
|
||||
|
||||
def main() -> NoReturn:
|
||||
log_handler = get_file_handler()
|
||||
log_handler.setFormatter(SwagLogFileFormatter(None))
|
||||
log_level = 20 # logging.INFO
|
||||
|
||||
ctx = zmq.Context.instance()
|
||||
sock = ctx.socket(zmq.PULL)
|
||||
sock.bind(Paths.swaglog_ipc())
|
||||
|
||||
# and we publish them
|
||||
log_message_sock = messaging.pub_sock('logMessage')
|
||||
error_log_message_sock = messaging.pub_sock('errorLogMessage')
|
||||
|
||||
try:
|
||||
while True:
|
||||
dat = b''.join(sock.recv_multipart())
|
||||
level = dat[0]
|
||||
record = dat[1:].decode("utf-8")
|
||||
if level >= log_level:
|
||||
log_handler.emit(record)
|
||||
|
||||
if len(record) > 2*1024*1024:
|
||||
print("WARNING: log too big to publish", len(record))
|
||||
print(record[:100])
|
||||
continue
|
||||
|
||||
# then we publish them
|
||||
msg = messaging.new_message(None, valid=True, logMessage=record)
|
||||
log_message_sock.send(msg.to_bytes())
|
||||
|
||||
if level >= 40: # logging.ERROR
|
||||
msg = messaging.new_message(None, valid=True, errorLogMessage=record)
|
||||
error_log_message_sock.send(msg.to_bytes())
|
||||
finally:
|
||||
sock.close()
|
||||
ctx.term()
|
||||
|
||||
# can hit this if interrupted during a rollover
|
||||
try:
|
||||
log_handler.close()
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Executable
+94
@@ -0,0 +1,94 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import subprocess
|
||||
from pathlib import Path
|
||||
|
||||
# NOTE: Do NOT import anything here that needs be built (e.g. params)
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.spinner import Spinner
|
||||
from openpilot.common.text_window import TextWindow
|
||||
from openpilot.system.hardware import HARDWARE, AGNOS
|
||||
from openpilot.common.swaglog import cloudlog, add_file_handler
|
||||
from openpilot.system.version import get_build_metadata
|
||||
|
||||
MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9
|
||||
CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache")
|
||||
|
||||
TOTAL_SCONS_NODES = 3130
|
||||
MAX_BUILD_PROGRESS = 100
|
||||
|
||||
def build(spinner: Spinner, dirty: bool = False, minimal: bool = False) -> None:
|
||||
env = os.environ.copy()
|
||||
env['SCONS_PROGRESS'] = "1"
|
||||
nproc = os.cpu_count()
|
||||
if nproc is None:
|
||||
nproc = 2
|
||||
|
||||
extra_args = ["--minimal"] if minimal else []
|
||||
|
||||
if AGNOS:
|
||||
HARDWARE.set_power_save(False)
|
||||
os.sched_setaffinity(0, range(8)) # ensure we can use the isolcpus cores
|
||||
|
||||
# building with all cores can result in using too
|
||||
# much memory, so retry with less parallelism
|
||||
compile_output: list[bytes] = []
|
||||
for n in (nproc, nproc/2, 1):
|
||||
compile_output.clear()
|
||||
scons: subprocess.Popen = subprocess.Popen(["scons", f"-j{int(n)}", "--cache-populate", *extra_args], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
|
||||
assert scons.stderr is not None
|
||||
|
||||
# Read progress from stderr and update spinner
|
||||
while scons.poll() is None:
|
||||
try:
|
||||
line = scons.stderr.readline()
|
||||
if line is None:
|
||||
continue
|
||||
line = line.rstrip()
|
||||
|
||||
prefix = b'progress: '
|
||||
if line.startswith(prefix):
|
||||
i = int(line[len(prefix):])
|
||||
spinner.update_progress(MAX_BUILD_PROGRESS * min(1., i / TOTAL_SCONS_NODES), 100.)
|
||||
elif len(line):
|
||||
compile_output.append(line)
|
||||
print(line.decode('utf8', 'replace'))
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
if scons.returncode == 0:
|
||||
break
|
||||
|
||||
if scons.returncode != 0:
|
||||
# Read remaining output
|
||||
if scons.stderr is not None:
|
||||
compile_output += scons.stderr.read().split(b'\n')
|
||||
|
||||
# Build failed log errors
|
||||
error_s = b"\n".join(compile_output).decode('utf8', 'replace')
|
||||
add_file_handler(cloudlog)
|
||||
cloudlog.error("scons build failed\n" + error_s)
|
||||
|
||||
# Show TextWindow
|
||||
spinner.close()
|
||||
if not os.getenv("CI"):
|
||||
with TextWindow("openpilot failed to build\n \n" + error_s) as t:
|
||||
t.wait_for_exit()
|
||||
exit(1)
|
||||
|
||||
# enforce max cache size
|
||||
cache_files = [f for f in CACHE_DIR.rglob('*') if f.is_file()]
|
||||
cache_files.sort(key=lambda f: f.stat().st_mtime)
|
||||
cache_size = sum(f.stat().st_size for f in cache_files)
|
||||
for f in cache_files:
|
||||
if cache_size < MAX_CACHE_SIZE:
|
||||
break
|
||||
cache_size -= f.stat().st_size
|
||||
f.unlink()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
spinner = Spinner()
|
||||
spinner.update_progress(0, 100)
|
||||
build_metadata = get_build_metadata()
|
||||
build(spinner, build_metadata.openpilot.is_dirty, minimal = AGNOS)
|
||||
@@ -0,0 +1,67 @@
|
||||
import errno
|
||||
import fcntl
|
||||
import os
|
||||
import sys
|
||||
import pathlib
|
||||
import shutil
|
||||
import signal
|
||||
import subprocess
|
||||
import tempfile
|
||||
import threading
|
||||
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.params import Params
|
||||
|
||||
def unblock_stdout() -> None:
|
||||
# get a non-blocking stdout
|
||||
child_pid, child_pty = os.forkpty()
|
||||
if child_pid != 0: # parent
|
||||
|
||||
# child is in its own process group, manually pass kill signals
|
||||
signal.signal(signal.SIGINT, lambda signum, frame: os.kill(child_pid, signal.SIGINT))
|
||||
signal.signal(signal.SIGTERM, lambda signum, frame: os.kill(child_pid, signal.SIGTERM))
|
||||
|
||||
fcntl.fcntl(sys.stdout, fcntl.F_SETFL, fcntl.fcntl(sys.stdout, fcntl.F_GETFL) | os.O_NONBLOCK)
|
||||
|
||||
while True:
|
||||
try:
|
||||
dat = os.read(child_pty, 4096)
|
||||
except OSError as e:
|
||||
if e.errno == errno.EIO:
|
||||
break
|
||||
continue
|
||||
|
||||
if not dat:
|
||||
break
|
||||
|
||||
try:
|
||||
sys.stdout.write(dat.decode('utf8'))
|
||||
except (OSError, UnicodeDecodeError):
|
||||
pass
|
||||
|
||||
# os.wait() returns a tuple with the pid and a 16 bit value
|
||||
# whose low byte is the signal number and whose high byte is the exit status
|
||||
exit_status = os.wait()[1] >> 8
|
||||
os._exit(exit_status)
|
||||
|
||||
|
||||
def write_onroad_params(started, params):
|
||||
params.put_bool("IsOnroad", started)
|
||||
params.put_bool("IsOffroad", not started)
|
||||
|
||||
|
||||
def save_bootlog():
|
||||
# copy current params
|
||||
tmp = tempfile.mkdtemp()
|
||||
params_dirname = pathlib.Path(Params().get_param_path()).name
|
||||
params_dir = os.path.join(tmp, params_dirname)
|
||||
shutil.copytree(Params().get_param_path(), params_dir, dirs_exist_ok=True)
|
||||
|
||||
def fn(tmpdir):
|
||||
env = os.environ.copy()
|
||||
env['PARAMS_COPY_PATH'] = tmpdir
|
||||
subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "system/loggerd"), env=env)
|
||||
shutil.rmtree(tmpdir)
|
||||
t = threading.Thread(target=fn, args=(tmp, ))
|
||||
t.daemon = True
|
||||
t.start()
|
||||
Executable
+262
@@ -0,0 +1,262 @@
|
||||
#!/usr/bin/env python3
|
||||
import datetime
|
||||
import os
|
||||
import signal
|
||||
import sys
|
||||
import traceback
|
||||
|
||||
from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
import openpilot.system.sentry as sentry
|
||||
from openpilot.common.params import Params, ParamKeyType
|
||||
from openpilot.common.text_window import TextWindow
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
from openpilot.system.manager.helpers import unblock_stdout, write_onroad_params, save_bootlog
|
||||
from openpilot.system.manager.process import ensure_running
|
||||
from openpilot.system.manager.process_config import managed_processes
|
||||
from openpilot.system.athena.registration import register, UNREGISTERED_DONGLE_ID
|
||||
from openpilot.common.swaglog import cloudlog, add_file_handler
|
||||
from openpilot.system.version import get_build_metadata, terms_version, training_version
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from openpilot.system.manager.vehicle_model_collector import VehicleModelCollector
|
||||
|
||||
|
||||
def manager_init() -> None:
|
||||
save_bootlog()
|
||||
|
||||
build_metadata = get_build_metadata()
|
||||
|
||||
params = Params()
|
||||
params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START)
|
||||
params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION)
|
||||
params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
|
||||
if build_metadata.release_channel:
|
||||
params.clear_all(ParamKeyType.DEVELOPMENT_ONLY)
|
||||
|
||||
default_params: list[tuple[str, str | bytes]] = [
|
||||
("CompletedTrainingVersion", "0"),
|
||||
("DisengageOnAccelerator", "0"),
|
||||
("GsmMetered", "1"),
|
||||
("HasAcceptedTerms", "0"),
|
||||
("LanguageSetting", "main_en"),
|
||||
("OpenpilotEnabledToggle", "1"),
|
||||
("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)),
|
||||
("DisableLogging", "0"),
|
||||
("dp_device_is_rhd", "0"),
|
||||
("dp_device_monitoring_disabled", "0"),
|
||||
("dp_device_beep", "0"),
|
||||
("dp_lat_alka", "0"),
|
||||
("dp_ui_display_mode", "0"),
|
||||
("dp_device_model_selected", ""),
|
||||
("dp_device_model_list", ""),
|
||||
("dp_lat_lca_speed", "20"),
|
||||
("dp_lat_lca_auto_sec", "0"),
|
||||
("dp_ui_hide_hud_speed_kph", "0"),
|
||||
("dp_lon_ext_radar", "0"),
|
||||
("dp_lat_road_edge_detection", "0"),
|
||||
("dp_ui_rainbow", "0"),
|
||||
("dp_lon_acm", "0"),
|
||||
("dp_lon_acm_downhill", "0"),
|
||||
("dp_lon_aem", "0"),
|
||||
("dp_device_audible_alert_mode", "0"),
|
||||
("dp_lon_no_gas_gating", "0"),
|
||||
("dp_device_auto_shutdown_in", "-5"),
|
||||
("dp_ui_radar_tracks", "0"),
|
||||
("dp_toyota_door_auto_lock_unlock", "0"),
|
||||
("dp_toyota_tss1_sng", "0"),
|
||||
("dp_toyota_stock_lon", "0"),
|
||||
("dp_vag_a0_sng", "0"),
|
||||
("dp_vag_pq_steering_patch", "0"),
|
||||
("dp_vag_avoid_eps_lockout", "0"),
|
||||
]
|
||||
|
||||
if params.get_bool("RecordFrontLock"):
|
||||
params.put_bool("RecordFront", True)
|
||||
|
||||
# set unset params
|
||||
for k, v in default_params:
|
||||
if params.get(k) is None:
|
||||
params.put(k, v)
|
||||
|
||||
params.put("dp_device_model_list", VehicleModelCollector().get())
|
||||
|
||||
# Create folders needed for msgq
|
||||
try:
|
||||
os.mkdir(Paths.shm_path())
|
||||
except FileExistsError:
|
||||
pass
|
||||
except PermissionError:
|
||||
print(f"WARNING: failed to make {Paths.shm_path()}")
|
||||
|
||||
# set params
|
||||
serial = HARDWARE.get_serial()
|
||||
params.put("Version", build_metadata.openpilot.version)
|
||||
params.put("TermsVersion", terms_version)
|
||||
params.put("TrainingVersion", training_version)
|
||||
params.put("GitCommit", build_metadata.openpilot.git_commit)
|
||||
params.put("GitCommitDate", build_metadata.openpilot.git_commit_date)
|
||||
params.put("GitBranch", build_metadata.channel)
|
||||
params.put("GitRemote", build_metadata.openpilot.git_origin)
|
||||
params.put_bool("IsTestedBranch", build_metadata.tested_channel)
|
||||
params.put_bool("IsReleaseBranch", build_metadata.release_channel)
|
||||
params.put("HardwareSerial", serial)
|
||||
|
||||
# set dongle id
|
||||
reg_res = register(show_spinner=True)
|
||||
if reg_res:
|
||||
dongle_id = reg_res
|
||||
else:
|
||||
raise Exception(f"Registration failed for device {serial}")
|
||||
os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog
|
||||
os.environ['GIT_ORIGIN'] = build_metadata.openpilot.git_normalized_origin # Needed for swaglog
|
||||
os.environ['GIT_BRANCH'] = build_metadata.channel # Needed for swaglog
|
||||
os.environ['GIT_COMMIT'] = build_metadata.openpilot.git_commit # Needed for swaglog
|
||||
|
||||
if not build_metadata.openpilot.is_dirty:
|
||||
os.environ['CLEAN'] = '1'
|
||||
|
||||
# init logging
|
||||
sentry.init(sentry.SentryProject.SELFDRIVE)
|
||||
cloudlog.bind_global(dongle_id=dongle_id,
|
||||
version=build_metadata.openpilot.version,
|
||||
origin=build_metadata.openpilot.git_normalized_origin,
|
||||
branch=build_metadata.channel,
|
||||
commit=build_metadata.openpilot.git_commit,
|
||||
dirty=build_metadata.openpilot.is_dirty,
|
||||
device=HARDWARE.get_device_type())
|
||||
|
||||
# preimport all processes
|
||||
for p in managed_processes.values():
|
||||
p.prepare()
|
||||
|
||||
|
||||
def manager_cleanup() -> None:
|
||||
# send signals to kill all procs
|
||||
for p in managed_processes.values():
|
||||
p.stop(block=False)
|
||||
|
||||
# ensure all are killed
|
||||
for p in managed_processes.values():
|
||||
p.stop(block=True)
|
||||
|
||||
cloudlog.info("everything is dead")
|
||||
|
||||
|
||||
def manager_thread() -> None:
|
||||
cloudlog.bind(daemon="manager")
|
||||
cloudlog.info("manager start")
|
||||
cloudlog.info({"environ": os.environ})
|
||||
|
||||
params = Params()
|
||||
|
||||
ignore: list[str] = []
|
||||
if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID):
|
||||
ignore += ["manage_athenad", "uploader"]
|
||||
if os.getenv("NOBOARD") is not None:
|
||||
ignore.append("pandad")
|
||||
ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0]
|
||||
|
||||
sm = messaging.SubMaster(['deviceState', 'carParams'], poll='deviceState')
|
||||
pm = messaging.PubMaster(['managerState'])
|
||||
|
||||
write_onroad_params(False, params)
|
||||
ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore)
|
||||
|
||||
started_prev = False
|
||||
|
||||
while True:
|
||||
sm.update(1000)
|
||||
|
||||
started = sm['deviceState'].started
|
||||
|
||||
if started and not started_prev:
|
||||
params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION)
|
||||
elif not started and started_prev:
|
||||
params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
|
||||
|
||||
# update onroad params, which drives pandad's safety setter thread
|
||||
if started != started_prev:
|
||||
write_onroad_params(started, params)
|
||||
|
||||
started_prev = started
|
||||
|
||||
ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore)
|
||||
|
||||
running = ' '.join("{}{}\u001b[0m".format("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
|
||||
for p in managed_processes.values() if p.proc)
|
||||
print(running)
|
||||
cloudlog.debug(running)
|
||||
|
||||
# send managerState
|
||||
msg = messaging.new_message('managerState', valid=True)
|
||||
msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()]
|
||||
pm.send('managerState', msg)
|
||||
|
||||
# Exit main loop when uninstall/shutdown/reboot is needed
|
||||
shutdown = False
|
||||
for param in ("DoUninstall", "DoShutdown", "DoReboot", "dp_device_reset_conf"):
|
||||
if params.get_bool(param):
|
||||
if param == "dp_device_reset_conf":
|
||||
os.system("rm -fr /data/params/d/dp_*")
|
||||
shutdown = True
|
||||
params.put("LastManagerExitReason", f"{param} {datetime.datetime.now()}")
|
||||
cloudlog.warning(f"Shutting down manager - {param} set")
|
||||
|
||||
if shutdown:
|
||||
break
|
||||
|
||||
|
||||
def main() -> None:
|
||||
manager_init()
|
||||
if os.getenv("PREPAREONLY") is not None:
|
||||
return
|
||||
|
||||
# SystemExit on sigterm
|
||||
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1))
|
||||
|
||||
try:
|
||||
manager_thread()
|
||||
except Exception:
|
||||
traceback.print_exc()
|
||||
sentry.capture_exception()
|
||||
finally:
|
||||
manager_cleanup()
|
||||
|
||||
params = Params()
|
||||
if params.get_bool("DoUninstall"):
|
||||
cloudlog.warning("uninstalling")
|
||||
HARDWARE.uninstall()
|
||||
elif params.get_bool("DoReboot"):
|
||||
cloudlog.warning("reboot")
|
||||
HARDWARE.reboot()
|
||||
elif params.get_bool("DoShutdown"):
|
||||
cloudlog.warning("shutdown")
|
||||
HARDWARE.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unblock_stdout()
|
||||
|
||||
try:
|
||||
main()
|
||||
except KeyboardInterrupt:
|
||||
print("got CTRL-C, exiting")
|
||||
except Exception:
|
||||
add_file_handler(cloudlog)
|
||||
cloudlog.exception("Manager failed to start")
|
||||
|
||||
try:
|
||||
managed_processes['ui'].stop()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# Show last 3 lines of traceback
|
||||
error = traceback.format_exc(-3)
|
||||
error = "Manager failed to start\n\n" + error
|
||||
with TextWindow(error) as t:
|
||||
t.wait_for_exit()
|
||||
|
||||
raise
|
||||
|
||||
# manual exit because we are forked
|
||||
sys.exit(0)
|
||||
@@ -0,0 +1,298 @@
|
||||
import importlib
|
||||
import os
|
||||
import signal
|
||||
import struct
|
||||
import time
|
||||
import subprocess
|
||||
from collections.abc import Callable, ValuesView
|
||||
from abc import ABC, abstractmethod
|
||||
from multiprocessing import Process
|
||||
|
||||
from setproctitle import setproctitle
|
||||
|
||||
from cereal import car, log
|
||||
import cereal.messaging as messaging
|
||||
import openpilot.system.sentry as sentry
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
|
||||
WATCHDOG_FN = f"{Paths.shm_path()}/wd_"
|
||||
ENABLE_WATCHDOG = os.getenv("NO_WATCHDOG") is None
|
||||
|
||||
|
||||
def launcher(proc: str, name: str) -> None:
|
||||
try:
|
||||
# import the process
|
||||
mod = importlib.import_module(proc)
|
||||
|
||||
# rename the process
|
||||
setproctitle(proc)
|
||||
|
||||
# create new context since we forked
|
||||
messaging.reset_context()
|
||||
|
||||
# add daemon name tag to logs
|
||||
cloudlog.bind(daemon=name)
|
||||
sentry.set_tag("daemon", name)
|
||||
|
||||
# exec the process
|
||||
mod.main()
|
||||
except KeyboardInterrupt:
|
||||
cloudlog.warning(f"child {proc} got SIGINT")
|
||||
except Exception:
|
||||
# can't install the crash handler because sys.excepthook doesn't play nice
|
||||
# with threads, so catch it here.
|
||||
sentry.capture_exception()
|
||||
raise
|
||||
|
||||
|
||||
def nativelauncher(pargs: list[str], cwd: str, name: str) -> None:
|
||||
os.environ['MANAGER_DAEMON'] = name
|
||||
|
||||
# exec the process
|
||||
os.chdir(cwd)
|
||||
os.execvp(pargs[0], pargs)
|
||||
|
||||
|
||||
def join_process(process: Process, timeout: float) -> None:
|
||||
# Process().join(timeout) will hang due to a python 3 bug: https://bugs.python.org/issue28382
|
||||
# We have to poll the exitcode instead
|
||||
t = time.monotonic()
|
||||
while time.monotonic() - t < timeout and process.exitcode is None:
|
||||
time.sleep(0.001)
|
||||
|
||||
|
||||
class ManagerProcess(ABC):
|
||||
daemon = False
|
||||
sigkill = False
|
||||
should_run: Callable[[bool, Params, car.CarParams], bool]
|
||||
proc: Process | None = None
|
||||
enabled = True
|
||||
name = ""
|
||||
|
||||
last_watchdog_time = 0
|
||||
watchdog_max_dt: int | None = None
|
||||
watchdog_seen = False
|
||||
shutting_down = False
|
||||
|
||||
@abstractmethod
|
||||
def prepare(self) -> None:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def start(self) -> None:
|
||||
pass
|
||||
|
||||
def restart(self) -> None:
|
||||
self.stop(sig=signal.SIGKILL)
|
||||
self.start()
|
||||
|
||||
def check_watchdog(self, started: bool) -> None:
|
||||
if self.watchdog_max_dt is None or self.proc is None:
|
||||
return
|
||||
|
||||
try:
|
||||
fn = WATCHDOG_FN + str(self.proc.pid)
|
||||
with open(fn, "rb") as f:
|
||||
# TODO: why can't pylint find struct.unpack?
|
||||
self.last_watchdog_time = struct.unpack('Q', f.read())[0]
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
dt = time.monotonic() - self.last_watchdog_time / 1e9
|
||||
|
||||
if dt > self.watchdog_max_dt:
|
||||
if self.watchdog_seen and ENABLE_WATCHDOG:
|
||||
cloudlog.error(f"Watchdog timeout for {self.name} (exitcode {self.proc.exitcode}) restarting ({started=})")
|
||||
self.restart()
|
||||
else:
|
||||
self.watchdog_seen = True
|
||||
|
||||
def stop(self, retry: bool = True, block: bool = True, sig: signal.Signals = None) -> int | None:
|
||||
if self.proc is None:
|
||||
return None
|
||||
|
||||
if self.proc.exitcode is None:
|
||||
if not self.shutting_down:
|
||||
cloudlog.info(f"killing {self.name}")
|
||||
if sig is None:
|
||||
sig = signal.SIGKILL if self.sigkill else signal.SIGINT
|
||||
self.signal(sig)
|
||||
self.shutting_down = True
|
||||
|
||||
if not block:
|
||||
return None
|
||||
|
||||
join_process(self.proc, 5)
|
||||
|
||||
# If process failed to die send SIGKILL
|
||||
if self.proc.exitcode is None and retry:
|
||||
cloudlog.info(f"killing {self.name} with SIGKILL")
|
||||
self.signal(signal.SIGKILL)
|
||||
self.proc.join()
|
||||
|
||||
ret = self.proc.exitcode
|
||||
cloudlog.info(f"{self.name} is dead with {ret}")
|
||||
|
||||
if self.proc.exitcode is not None:
|
||||
self.shutting_down = False
|
||||
self.proc = None
|
||||
|
||||
return ret
|
||||
|
||||
def signal(self, sig: int) -> None:
|
||||
if self.proc is None:
|
||||
return
|
||||
|
||||
# Don't signal if already exited
|
||||
if self.proc.exitcode is not None and self.proc.pid is not None:
|
||||
return
|
||||
|
||||
# Can't signal if we don't have a pid
|
||||
if self.proc.pid is None:
|
||||
return
|
||||
|
||||
cloudlog.info(f"sending signal {sig} to {self.name}")
|
||||
os.kill(self.proc.pid, sig)
|
||||
|
||||
def get_process_state_msg(self):
|
||||
state = log.ManagerState.ProcessState.new_message()
|
||||
state.name = self.name
|
||||
if self.proc:
|
||||
state.running = self.proc.is_alive()
|
||||
state.shouldBeRunning = self.proc is not None and not self.shutting_down
|
||||
state.pid = self.proc.pid or 0
|
||||
state.exitCode = self.proc.exitcode or 0
|
||||
return state
|
||||
|
||||
|
||||
class NativeProcess(ManagerProcess):
|
||||
def __init__(self, name, cwd, cmdline, should_run, enabled=True, sigkill=False, watchdog_max_dt=None):
|
||||
self.name = name
|
||||
self.cwd = cwd
|
||||
self.cmdline = cmdline
|
||||
self.should_run = should_run
|
||||
self.enabled = enabled
|
||||
self.sigkill = sigkill
|
||||
self.watchdog_max_dt = watchdog_max_dt
|
||||
self.launcher = nativelauncher
|
||||
|
||||
def prepare(self) -> None:
|
||||
pass
|
||||
|
||||
def start(self) -> None:
|
||||
# In case we only tried a non blocking stop we need to stop it before restarting
|
||||
if self.shutting_down:
|
||||
self.stop()
|
||||
|
||||
if self.proc is not None:
|
||||
return
|
||||
|
||||
cwd = os.path.join(BASEDIR, self.cwd)
|
||||
cloudlog.info(f"starting process {self.name}")
|
||||
self.proc = Process(name=self.name, target=self.launcher, args=(self.cmdline, cwd, self.name))
|
||||
self.proc.start()
|
||||
self.watchdog_seen = False
|
||||
self.shutting_down = False
|
||||
|
||||
|
||||
class PythonProcess(ManagerProcess):
|
||||
def __init__(self, name, module, should_run, enabled=True, sigkill=False, watchdog_max_dt=None):
|
||||
self.name = name
|
||||
self.module = module
|
||||
self.should_run = should_run
|
||||
self.enabled = enabled
|
||||
self.sigkill = sigkill
|
||||
self.watchdog_max_dt = watchdog_max_dt
|
||||
self.launcher = launcher
|
||||
|
||||
def prepare(self) -> None:
|
||||
if self.enabled:
|
||||
cloudlog.info(f"preimporting {self.module}")
|
||||
importlib.import_module(self.module)
|
||||
|
||||
def start(self) -> None:
|
||||
# In case we only tried a non blocking stop we need to stop it before restarting
|
||||
if self.shutting_down:
|
||||
self.stop()
|
||||
|
||||
if self.proc is not None:
|
||||
return
|
||||
|
||||
# TODO: this is just a workaround for this tinygrad check:
|
||||
# https://github.com/tinygrad/tinygrad/blob/ac9c96dae1656dc220ee4acc39cef4dd449aa850/tinygrad/device.py#L26
|
||||
name = self.name if "modeld" not in self.name else "MainProcess"
|
||||
|
||||
cloudlog.info(f"starting python {self.module}")
|
||||
self.proc = Process(name=name, target=self.launcher, args=(self.module, self.name))
|
||||
self.proc.start()
|
||||
self.watchdog_seen = False
|
||||
self.shutting_down = False
|
||||
|
||||
|
||||
class DaemonProcess(ManagerProcess):
|
||||
"""Python process that has to stay running across manager restart.
|
||||
This is used for athena so you don't lose SSH access when restarting manager."""
|
||||
def __init__(self, name, module, param_name, enabled=True):
|
||||
self.name = name
|
||||
self.module = module
|
||||
self.param_name = param_name
|
||||
self.enabled = enabled
|
||||
self.params = None
|
||||
|
||||
@staticmethod
|
||||
def should_run(started, params, CP):
|
||||
return True
|
||||
|
||||
def prepare(self) -> None:
|
||||
pass
|
||||
|
||||
def start(self) -> None:
|
||||
if self.params is None:
|
||||
self.params = Params()
|
||||
|
||||
pid = self.params.get(self.param_name, encoding='utf-8')
|
||||
if pid is not None:
|
||||
try:
|
||||
os.kill(int(pid), 0)
|
||||
with open(f'/proc/{pid}/cmdline') as f:
|
||||
if self.module in f.read():
|
||||
# daemon is running
|
||||
return
|
||||
except (OSError, FileNotFoundError):
|
||||
# process is dead
|
||||
pass
|
||||
|
||||
cloudlog.info(f"starting daemon {self.name}")
|
||||
proc = subprocess.Popen(['python', '-m', self.module],
|
||||
stdin=open('/dev/null'),
|
||||
stdout=open('/dev/null', 'w'),
|
||||
stderr=open('/dev/null', 'w'),
|
||||
preexec_fn=os.setpgrp)
|
||||
|
||||
self.params.put(self.param_name, str(proc.pid))
|
||||
|
||||
def stop(self, retry=True, block=True, sig=None) -> None:
|
||||
pass
|
||||
|
||||
|
||||
def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None, CP: car.CarParams=None,
|
||||
not_run: list[str] | None=None) -> list[ManagerProcess]:
|
||||
if not_run is None:
|
||||
not_run = []
|
||||
|
||||
running = []
|
||||
for p in procs:
|
||||
if p.enabled and p.name not in not_run and p.should_run(started, params, CP):
|
||||
running.append(p)
|
||||
else:
|
||||
p.stop(block=False)
|
||||
|
||||
p.check_watchdog(started)
|
||||
|
||||
for p in running:
|
||||
p.start()
|
||||
|
||||
return running
|
||||
@@ -0,0 +1,118 @@
|
||||
import os
|
||||
import operator
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.system.hardware import PC, TICI
|
||||
from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess
|
||||
|
||||
WEBCAM = os.getenv("USE_WEBCAM") is not None
|
||||
|
||||
def driverview(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started or params.get_bool("IsDriverViewEnabled")
|
||||
|
||||
def notcar(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started and CP.notCar
|
||||
|
||||
def iscar(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started and not CP.notCar
|
||||
|
||||
def logging(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
run = not params.get_bool("DisableLogging")
|
||||
return started and run
|
||||
|
||||
def ublox_available() -> bool:
|
||||
return os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps')
|
||||
|
||||
def ublox(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
use_ublox = ublox_available()
|
||||
if use_ublox != params.get_bool("UbloxAvailable"):
|
||||
params.put_bool("UbloxAvailable", use_ublox)
|
||||
return started and use_ublox
|
||||
|
||||
def joystick(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started and params.get_bool("JoystickDebugMode")
|
||||
|
||||
def not_joystick(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started and not params.get_bool("JoystickDebugMode")
|
||||
|
||||
def long_maneuver(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started and params.get_bool("LongitudinalManeuverMode")
|
||||
|
||||
def not_long_maneuver(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started and not params.get_bool("LongitudinalManeuverMode")
|
||||
|
||||
def qcomgps(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started and not ublox_available()
|
||||
|
||||
def always_run(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return True
|
||||
|
||||
def only_onroad(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return started
|
||||
|
||||
def only_offroad(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return not started
|
||||
|
||||
def or_(*fns):
|
||||
return lambda *args: operator.or_(*(fn(*args) for fn in fns))
|
||||
|
||||
def and_(*fns):
|
||||
return lambda *args: operator.and_(*(fn(*args) for fn in fns))
|
||||
|
||||
procs = [
|
||||
DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"),
|
||||
|
||||
NativeProcess("loggerd", "system/loggerd", ["./loggerd"], logging),
|
||||
NativeProcess("encoderd", "system/loggerd", ["./encoderd"], only_onroad),
|
||||
NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], notcar),
|
||||
PythonProcess("logmessaged", "system.logmessaged", always_run),
|
||||
|
||||
NativeProcess("camerad", "system/camerad", ["./camerad"], driverview, enabled=not WEBCAM),
|
||||
PythonProcess("webcamerad", "tools.webcam.camerad", driverview, enabled=WEBCAM),
|
||||
NativeProcess("logcatd", "system/logcatd", ["./logcatd"], only_onroad),
|
||||
NativeProcess("proclogd", "system/proclogd", ["./proclogd"], only_onroad),
|
||||
PythonProcess("micd", "system.micd", iscar, enabled=not os.getenv("DISABLE_DRIVER")),
|
||||
PythonProcess("timed", "system.timed", always_run, enabled=not PC),
|
||||
|
||||
PythonProcess("modeld", "selfdrive.modeld.modeld", only_onroad),
|
||||
PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(WEBCAM or not PC) and not os.getenv("DISABLE_DRIVER")),
|
||||
|
||||
NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC),
|
||||
NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
|
||||
PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad, enabled=not os.getenv("DISABLE_DRIVER")),
|
||||
PythonProcess("beepd", "dragonpilot.selfdrive.ui.beepd", only_onroad, enabled=(Params().get_bool("dp_device_beep") and os.getenv("DISABLE_DRIVER"))),
|
||||
PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
|
||||
NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),
|
||||
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
|
||||
PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),
|
||||
PythonProcess("controlsd", "selfdrive.controls.controlsd", and_(not_joystick, iscar)),
|
||||
PythonProcess("joystickd", "tools.joystick.joystickd", or_(joystick, notcar)),
|
||||
PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad),
|
||||
PythonProcess("card", "selfdrive.car.card", only_onroad),
|
||||
PythonProcess("deleter", "system.loggerd.deleter", always_run),
|
||||
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(WEBCAM or not PC) and not os.getenv("DISABLE_DRIVER")),
|
||||
PythonProcess("dpmonitoringd", "selfdrive.monitoring.dpmonitoringd", only_onroad, enabled=os.getenv("DISABLE_DRIVER")),
|
||||
PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI),
|
||||
PythonProcess("pandad", "selfdrive.pandad.pandad", always_run),
|
||||
PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad),
|
||||
NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI),
|
||||
PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI),
|
||||
PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver),
|
||||
PythonProcess("maneuversd", "tools.longitudinal_maneuvers.maneuversd", long_maneuver),
|
||||
PythonProcess("radard", "selfdrive.controls.radard", only_onroad),
|
||||
PythonProcess("hardwared", "system.hardware.hardwared", always_run),
|
||||
PythonProcess("tombstoned", "system.tombstoned", always_run, enabled=not PC),
|
||||
PythonProcess("updated", "system.updated.updated", only_offroad, enabled=not PC),
|
||||
PythonProcess("uploader", "system.loggerd.uploader", always_run),
|
||||
PythonProcess("statsd", "system.statsd", always_run),
|
||||
PythonProcess("fileserv", "dragonpilot.selfdrive.fileserv", only_offroad),
|
||||
|
||||
# debug procs
|
||||
NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar),
|
||||
PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar),
|
||||
PythonProcess("webjoystick", "tools.bodyteleop.web", notcar),
|
||||
PythonProcess("joystick", "tools.joystick.joystick_control", and_(joystick, iscar)),
|
||||
]
|
||||
|
||||
managed_processes = {p.name: p for p in procs}
|
||||
@@ -0,0 +1,68 @@
|
||||
import os
|
||||
import pytest
|
||||
import signal
|
||||
import time
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.params import Params
|
||||
import openpilot.system.manager.manager as manager
|
||||
from openpilot.system.manager.process import ensure_running
|
||||
from openpilot.system.manager.process_config import managed_processes, procs
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
|
||||
os.environ['FAKEUPLOAD'] = "1"
|
||||
|
||||
MAX_STARTUP_TIME = 3
|
||||
BLACKLIST_PROCS = ['manage_athenad', 'pandad', 'pigeond']
|
||||
|
||||
|
||||
class TestManager:
|
||||
def setup_method(self):
|
||||
HARDWARE.set_power_save(False)
|
||||
|
||||
# ensure clean CarParams
|
||||
params = Params()
|
||||
params.clear_all()
|
||||
|
||||
def teardown_method(self):
|
||||
manager.manager_cleanup()
|
||||
|
||||
def test_manager_prepare(self):
|
||||
os.environ['PREPAREONLY'] = '1'
|
||||
manager.main()
|
||||
|
||||
def test_duplicate_procs(self):
|
||||
assert len(procs) == len(managed_processes), "Duplicate process names"
|
||||
|
||||
def test_blacklisted_procs(self):
|
||||
# TODO: ensure there are blacklisted procs until we have a dedicated test
|
||||
assert len(BLACKLIST_PROCS), "No blacklisted procs to test not_run"
|
||||
|
||||
@pytest.mark.skip("this test is flaky the way it's currently written, should be moved to test_onroad")
|
||||
def test_clean_exit(self, subtests):
|
||||
"""
|
||||
Ensure all processes exit cleanly when stopped.
|
||||
"""
|
||||
HARDWARE.set_power_save(False)
|
||||
manager.manager_init()
|
||||
|
||||
CP = car.CarParams.new_message()
|
||||
procs = ensure_running(managed_processes.values(), True, Params(), CP, not_run=BLACKLIST_PROCS)
|
||||
|
||||
time.sleep(10)
|
||||
|
||||
for p in procs:
|
||||
with subtests.test(proc=p.name):
|
||||
state = p.get_process_state_msg()
|
||||
assert state.running, f"{p.name} not running"
|
||||
exit_code = p.stop(retry=False)
|
||||
|
||||
assert p.name not in BLACKLIST_PROCS, f"{p.name} was started"
|
||||
|
||||
assert exit_code is not None, f"{p.name} failed to exit"
|
||||
|
||||
# TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code
|
||||
exit_codes = [0, 1]
|
||||
if p.sigkill:
|
||||
exit_codes = [-signal.SIGKILL]
|
||||
assert exit_code in exit_codes, f"{p.name} died with {exit_code}"
|
||||
Executable
+161
@@ -0,0 +1,161 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# MIT Non-Commercial License
|
||||
#
|
||||
# Copyright (c) 2019, dragonpilot
|
||||
#
|
||||
# Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, for non-commercial purposes only, subject to the following conditions:
|
||||
#
|
||||
# - The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
|
||||
# - Commercial use (e.g., use in a product, service, or activity intended to generate revenue) is prohibited without explicit written permission from dragonpilot. Contact ricklan@gmail.com for inquiries.
|
||||
# - Any project that uses the Software must visibly mention the following acknowledgment: "This project uses software from dragonpilot and is licensed under a custom license requiring permission for use."
|
||||
#
|
||||
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
import os
|
||||
import importlib
|
||||
import json
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.params import Params
|
||||
|
||||
|
||||
class VehicleModelCollector:
|
||||
def __init__(self):
|
||||
self.base_package = "opendbc.car"
|
||||
self.base_path = f"{BASEDIR}/opendbc/car"
|
||||
self.exclude_brands = ['body', 'mock']
|
||||
|
||||
# Define the lookup dictionary for brand-to-group mappings
|
||||
self.brand_to_group_map = {
|
||||
"chrysler": [
|
||||
{"prefix": "DODGE_", "group": "Dodge"},
|
||||
{"prefix": "RAM_", "group": "Ram"},
|
||||
{"prefix": "JEEP_", "group": "Jeep"},
|
||||
],
|
||||
"gm": [
|
||||
{"prefix": "BUICK_", "group": "Buick"},
|
||||
{"prefix": "CADILLAC_", "group": "Cadillac"},
|
||||
{"prefix": "CHEVROLET_", "group": "Chevrolet"},
|
||||
{"prefix": "HOLDEN_", "group": "Holden"},
|
||||
],
|
||||
"honda": {"prefix": "ACURA_", "group": "Acura"},
|
||||
"toyota": {"prefix": "LEXUS_", "group": "Lexus"},
|
||||
"hyundai": [
|
||||
{"prefix": "KIA_", "group": "Kia"},
|
||||
{"prefix": "GENESIS_", "group": "Genesis"}
|
||||
],
|
||||
"volkswagen": [
|
||||
{"prefix": "AUDI_", "group": "Audi"},
|
||||
{"prefix": "SKODA_", "group": "Skoda"},
|
||||
{"prefix": "SEAT_", "group": "Seat"}
|
||||
]
|
||||
}
|
||||
|
||||
# Define exceptions for group names
|
||||
self.group_name_exceptions = {
|
||||
"gm": "GM",
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def is_car_model(car_class, attr):
|
||||
"""Check if the attribute is a car model (not callable and not a dunder attribute)"""
|
||||
return not callable(getattr(car_class, attr)) and not attr.startswith("__")
|
||||
|
||||
@staticmethod
|
||||
def move_to_proper_group(models, prefix):
|
||||
"""
|
||||
Moves models with a certain prefix to their respective group.
|
||||
Example: Models starting with 'LEXUS_' should go to 'Lexus' group.
|
||||
"""
|
||||
moved_models = []
|
||||
for model in models[:]: # Iterate over a copy to avoid modifying during iteration
|
||||
if model.startswith(prefix):
|
||||
moved_models.append(model)
|
||||
models.remove(model) # Remove from the original group
|
||||
return moved_models
|
||||
|
||||
def format_group_name(self, group_name):
|
||||
"""
|
||||
Formats group names according to the exceptions dictionary.
|
||||
Groups in the exceptions dictionary are returned in all caps, others are title cased.
|
||||
"""
|
||||
return self.group_name_exceptions.get(group_name, group_name.title())
|
||||
|
||||
def collect_models(self):
|
||||
"""Collect all car models and organize them by brand/group"""
|
||||
# List all subdirectories (car brands)
|
||||
car_brands = sorted([
|
||||
name for name in os.listdir(self.base_path)
|
||||
if os.path.isdir(os.path.join(self.base_path, name)) and not name.startswith("__")
|
||||
])
|
||||
|
||||
grouped_models = {}
|
||||
|
||||
# Import CAR from each subdirectory and group models by brand
|
||||
for brand in car_brands:
|
||||
if brand in self.exclude_brands:
|
||||
continue
|
||||
|
||||
module_name = f"{self.base_package}.{brand}.values"
|
||||
try:
|
||||
module = importlib.import_module(module_name)
|
||||
if hasattr(module, "CAR"):
|
||||
car_class = getattr(module, "CAR")
|
||||
models = sorted([attr for attr in dir(car_class) if self.is_car_model(car_class, attr)])
|
||||
|
||||
# Check if the brand has a special group in the lookup map
|
||||
if brand in self.brand_to_group_map:
|
||||
group_info = self.brand_to_group_map[brand]
|
||||
|
||||
if isinstance(group_info, list): # If multiple prefixes for the brand
|
||||
for prefix_info in group_info:
|
||||
moved_models = self.move_to_proper_group(models, prefix_info["prefix"])
|
||||
if moved_models:
|
||||
if prefix_info["group"] not in grouped_models:
|
||||
grouped_models[prefix_info["group"]] = []
|
||||
grouped_models[prefix_info["group"]].extend(moved_models)
|
||||
else: # Single prefix for the brand
|
||||
moved_models = self.move_to_proper_group(models, group_info["prefix"])
|
||||
if moved_models:
|
||||
if group_info["group"] not in grouped_models:
|
||||
grouped_models[group_info["group"]] = []
|
||||
grouped_models[group_info["group"]].extend(moved_models)
|
||||
|
||||
# Add remaining models to the respective brand
|
||||
if models:
|
||||
grouped_models[brand] = models
|
||||
except ModuleNotFoundError:
|
||||
pass
|
||||
|
||||
# Sort the groups alphabetically
|
||||
sorted_grouped_models = sorted(grouped_models.items(), key=lambda x: x[0])
|
||||
|
||||
# Convert to the desired output structure, ensuring models are sorted within each group
|
||||
output = [{"group": self.format_group_name(group), "models": sorted(models)}
|
||||
for group, models in sorted_grouped_models]
|
||||
|
||||
return output
|
||||
|
||||
# def save_to_params(self, output=None):
|
||||
# """Save the collected model list to Params"""
|
||||
# if output is None:
|
||||
# output = self.collect_models()
|
||||
# Params().put("dp_device_model_list", json.dumps(output))
|
||||
# return output
|
||||
#
|
||||
# def run(self):
|
||||
# """Collect models and save to params"""
|
||||
# models = self.collect_models()
|
||||
# self.save_to_params(models)
|
||||
# return models
|
||||
|
||||
def get_json(self):
|
||||
return self.collect_models()
|
||||
|
||||
def get(self):
|
||||
return json.dumps(self.collect_models())
|
||||
|
||||
# Allow running as a script
|
||||
if __name__ == "__main__":
|
||||
collector = VehicleModelCollector()
|
||||
print(collector.get())
|
||||
Executable
+107
@@ -0,0 +1,107 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from functools import cache
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.common.retry import retry
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
RATE = 10
|
||||
FFT_SAMPLES = 4096
|
||||
REFERENCE_SPL = 2e-5 # newtons/m^2
|
||||
SAMPLE_RATE = 44100
|
||||
SAMPLE_BUFFER = 4096 # approx 100ms
|
||||
|
||||
|
||||
@cache
|
||||
def get_a_weighting_filter():
|
||||
# Calculate the A-weighting filter
|
||||
# https://en.wikipedia.org/wiki/A-weighting
|
||||
freqs = np.fft.fftfreq(FFT_SAMPLES, d=1 / SAMPLE_RATE)
|
||||
A = 12194 ** 2 * freqs ** 4 / ((freqs ** 2 + 20.6 ** 2) * (freqs ** 2 + 12194 ** 2) * np.sqrt((freqs ** 2 + 107.7 ** 2) * (freqs ** 2 + 737.9 ** 2)))
|
||||
return A / np.max(A)
|
||||
|
||||
|
||||
def calculate_spl(measurements):
|
||||
# https://www.engineeringtoolbox.com/sound-pressure-d_711.html
|
||||
sound_pressure = np.sqrt(np.mean(measurements ** 2)) # RMS of amplitudes
|
||||
if sound_pressure > 0:
|
||||
sound_pressure_level = 20 * np.log10(sound_pressure / REFERENCE_SPL) # dB
|
||||
else:
|
||||
sound_pressure_level = 0
|
||||
return sound_pressure, sound_pressure_level
|
||||
|
||||
|
||||
def apply_a_weighting(measurements: np.ndarray) -> np.ndarray:
|
||||
# Generate a Hanning window of the same length as the audio measurements
|
||||
measurements_windowed = measurements * np.hanning(len(measurements))
|
||||
|
||||
# Apply the A-weighting filter to the signal
|
||||
return np.abs(np.fft.ifft(np.fft.fft(measurements_windowed) * get_a_weighting_filter()))
|
||||
|
||||
|
||||
class Mic:
|
||||
def __init__(self):
|
||||
self.rk = Ratekeeper(RATE)
|
||||
self.pm = messaging.PubMaster(['microphone'])
|
||||
|
||||
self.measurements = np.empty(0)
|
||||
|
||||
self.sound_pressure = 0
|
||||
self.sound_pressure_weighted = 0
|
||||
self.sound_pressure_level_weighted = 0
|
||||
|
||||
def update(self):
|
||||
msg = messaging.new_message('microphone', valid=True)
|
||||
msg.microphone.soundPressure = float(self.sound_pressure)
|
||||
msg.microphone.soundPressureWeighted = float(self.sound_pressure_weighted)
|
||||
|
||||
msg.microphone.soundPressureWeightedDb = float(self.sound_pressure_level_weighted)
|
||||
|
||||
self.pm.send('microphone', msg)
|
||||
self.rk.keep_time()
|
||||
|
||||
def callback(self, indata, frames, time, status):
|
||||
"""
|
||||
Using amplitude measurements, calculate an uncalibrated sound pressure and sound pressure level.
|
||||
Then apply A-weighting to the raw amplitudes and run the same calculations again.
|
||||
|
||||
Logged A-weighted equivalents are rough approximations of the human-perceived loudness.
|
||||
"""
|
||||
|
||||
self.measurements = np.concatenate((self.measurements, indata[:, 0]))
|
||||
|
||||
while self.measurements.size >= FFT_SAMPLES:
|
||||
measurements = self.measurements[:FFT_SAMPLES]
|
||||
|
||||
self.sound_pressure, _ = calculate_spl(measurements)
|
||||
measurements_weighted = apply_a_weighting(measurements)
|
||||
self.sound_pressure_weighted, self.sound_pressure_level_weighted = calculate_spl(measurements_weighted)
|
||||
|
||||
self.measurements = self.measurements[FFT_SAMPLES:]
|
||||
|
||||
@retry(attempts=7, delay=3)
|
||||
def get_stream(self, sd):
|
||||
# reload sounddevice to reinitialize portaudio
|
||||
sd._terminate()
|
||||
sd._initialize()
|
||||
return sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback, blocksize=SAMPLE_BUFFER)
|
||||
|
||||
def micd_thread(self):
|
||||
# sounddevice must be imported after forking processes
|
||||
import sounddevice as sd
|
||||
|
||||
with self.get_stream(sd) as stream:
|
||||
cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}, {stream.blocksize=}")
|
||||
while True:
|
||||
self.update()
|
||||
|
||||
|
||||
def main():
|
||||
mic = Mic()
|
||||
mic.micd_thread()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,6 @@
|
||||
Import('env', 'messaging', 'common')
|
||||
libs = [messaging, 'pthread', common]
|
||||
env.Program('proclogd', ['main.cc', 'proclog.cc'], LIBS=libs)
|
||||
|
||||
if GetOption('extras'):
|
||||
env.Program('tests/test_proclog', ['tests/test_proclog.cc', 'proclog.cc'], LIBS=libs)
|
||||
@@ -0,0 +1,40 @@
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
|
||||
struct CPUTime {
|
||||
int id;
|
||||
unsigned long utime, ntime, stime, itime;
|
||||
unsigned long iowtime, irqtime, sirqtime;
|
||||
};
|
||||
|
||||
struct ProcCache {
|
||||
int pid;
|
||||
std::string name, exe;
|
||||
std::vector<std::string> cmdline;
|
||||
};
|
||||
|
||||
struct ProcStat {
|
||||
int pid, ppid, processor;
|
||||
char state;
|
||||
long cutime, cstime, priority, nice, num_threads, rss;
|
||||
unsigned long utime, stime, vms;
|
||||
unsigned long long starttime;
|
||||
std::string name;
|
||||
};
|
||||
|
||||
namespace Parser {
|
||||
|
||||
std::vector<int> pids();
|
||||
std::optional<ProcStat> procStat(std::string stat);
|
||||
std::vector<std::string> cmdline(std::istream &stream);
|
||||
std::vector<CPUTime> cpuTimes(std::istream &stream);
|
||||
std::unordered_map<std::string, uint64_t> memInfo(std::istream &stream);
|
||||
const ProcCache &getProcExtraInfo(int pid, const std::string &name);
|
||||
|
||||
}; // namespace Parser
|
||||
|
||||
void buildProcLogMessage(MessageBuilder &msg);
|
||||
Executable
BIN
Binary file not shown.
@@ -0,0 +1,94 @@
|
||||
import select
|
||||
from serial import Serial
|
||||
from crcmod import mkCrcFun
|
||||
from struct import pack, unpack_from, calcsize
|
||||
|
||||
class ModemDiag:
|
||||
def __init__(self):
|
||||
self.serial = self.open_serial()
|
||||
self.pend = b''
|
||||
|
||||
def open_serial(self):
|
||||
serial = Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0, exclusive=True)
|
||||
serial.flush()
|
||||
serial.reset_input_buffer()
|
||||
serial.reset_output_buffer()
|
||||
return serial
|
||||
|
||||
ccitt_crc16 = mkCrcFun(0x11021, initCrc=0, xorOut=0xffff)
|
||||
ESCAPE_CHAR = b'\x7d'
|
||||
TRAILER_CHAR = b'\x7e'
|
||||
|
||||
def hdlc_encapsulate(self, payload):
|
||||
payload += pack('<H', ModemDiag.ccitt_crc16(payload))
|
||||
payload = payload.replace(self.ESCAPE_CHAR, bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]))
|
||||
payload = payload.replace(self.TRAILER_CHAR, bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]))
|
||||
payload += self.TRAILER_CHAR
|
||||
return payload
|
||||
|
||||
def hdlc_decapsulate(self, payload):
|
||||
assert len(payload) >= 3
|
||||
assert payload[-1:] == self.TRAILER_CHAR
|
||||
payload = payload[:-1]
|
||||
payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]), self.TRAILER_CHAR)
|
||||
payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]), self.ESCAPE_CHAR)
|
||||
assert payload[-2:] == pack('<H', ModemDiag.ccitt_crc16(payload[:-2]))
|
||||
return payload[:-2]
|
||||
|
||||
def recv(self):
|
||||
# self.serial.read_until makes tons of syscalls!
|
||||
raw_payload = [self.pend]
|
||||
while self.TRAILER_CHAR not in raw_payload[-1]:
|
||||
select.select([self.serial.fd], [], [])
|
||||
raw = self.serial.read(0x10000)
|
||||
raw_payload.append(raw)
|
||||
raw_payload = b''.join(raw_payload)
|
||||
raw_payload, self.pend = raw_payload.split(self.TRAILER_CHAR, 1)
|
||||
raw_payload += self.TRAILER_CHAR
|
||||
unframed_message = self.hdlc_decapsulate(raw_payload)
|
||||
return unframed_message[0], unframed_message[1:]
|
||||
|
||||
def send(self, packet_type, packet_payload):
|
||||
self.serial.write(self.hdlc_encapsulate(bytes([packet_type]) + packet_payload))
|
||||
|
||||
# *** end class ***
|
||||
|
||||
DIAG_LOG_F = 16
|
||||
DIAG_LOG_CONFIG_F = 115
|
||||
LOG_CONFIG_RETRIEVE_ID_RANGES_OP = 1
|
||||
LOG_CONFIG_SET_MASK_OP = 3
|
||||
LOG_CONFIG_SUCCESS_S = 0
|
||||
|
||||
def send_recv(diag, packet_type, packet_payload):
|
||||
diag.send(packet_type, packet_payload)
|
||||
while 1:
|
||||
opcode, payload = diag.recv()
|
||||
if opcode != DIAG_LOG_F:
|
||||
break
|
||||
return opcode, payload
|
||||
|
||||
def setup_logs(diag, types_to_log):
|
||||
opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xI', LOG_CONFIG_RETRIEVE_ID_RANGES_OP))
|
||||
|
||||
header_spec = '<3xII'
|
||||
operation, status = unpack_from(header_spec, payload)
|
||||
assert operation == LOG_CONFIG_RETRIEVE_ID_RANGES_OP
|
||||
assert status == LOG_CONFIG_SUCCESS_S
|
||||
|
||||
log_masks = unpack_from('<16I', payload, calcsize(header_spec))
|
||||
|
||||
for log_type, log_mask_bitsize in enumerate(log_masks):
|
||||
if log_mask_bitsize:
|
||||
log_mask = [0] * ((log_mask_bitsize+7)//8)
|
||||
for i in range(log_mask_bitsize):
|
||||
if ((log_type<<12)|i) in types_to_log:
|
||||
log_mask[i//8] |= 1 << (i%8)
|
||||
opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xIII',
|
||||
LOG_CONFIG_SET_MASK_OP,
|
||||
log_type,
|
||||
log_mask_bitsize
|
||||
) + bytes(log_mask))
|
||||
assert opcode == DIAG_LOG_CONFIG_F
|
||||
operation, status = unpack_from(header_spec, payload)
|
||||
assert operation == LOG_CONFIG_SET_MASK_OP
|
||||
assert status == LOG_CONFIG_SUCCESS_S
|
||||
@@ -0,0 +1,169 @@
|
||||
import os
|
||||
import sys
|
||||
from dataclasses import dataclass, fields
|
||||
from subprocess import check_output, CalledProcessError
|
||||
from time import sleep
|
||||
from typing import NoReturn
|
||||
|
||||
DEBUG = int(os.environ.get("DEBUG", "0"))
|
||||
|
||||
@dataclass
|
||||
class GnssClockNmeaPort:
|
||||
# flags bit mask:
|
||||
# 0x01 = leap_seconds valid
|
||||
# 0x02 = time_uncertainty_ns valid
|
||||
# 0x04 = full_bias_ns valid
|
||||
# 0x08 = bias_ns valid
|
||||
# 0x10 = bias_uncertainty_ns valid
|
||||
# 0x20 = drift_nsps valid
|
||||
# 0x40 = drift_uncertainty_nsps valid
|
||||
flags: int
|
||||
leap_seconds: int
|
||||
time_ns: int
|
||||
time_uncertainty_ns: int # 1-sigma
|
||||
full_bias_ns: int
|
||||
bias_ns: float
|
||||
bias_uncertainty_ns: float # 1-sigma
|
||||
drift_nsps: float
|
||||
drift_uncertainty_nsps: float # 1-sigma
|
||||
|
||||
def __post_init__(self):
|
||||
for field in fields(self):
|
||||
val = getattr(self, field.name)
|
||||
setattr(self, field.name, field.type(val) if val else None)
|
||||
|
||||
@dataclass
|
||||
class GnssMeasNmeaPort:
|
||||
messageCount: int
|
||||
messageNum: int
|
||||
svCount: int
|
||||
# constellation enum:
|
||||
# 1 = GPS
|
||||
# 2 = SBAS
|
||||
# 3 = GLONASS
|
||||
# 4 = QZSS
|
||||
# 5 = BEIDOU
|
||||
# 6 = GALILEO
|
||||
constellation: int
|
||||
svId: int
|
||||
flags: int # always zero
|
||||
time_offset_ns: int
|
||||
# state bit mask:
|
||||
# 0x0001 = CODE LOCK
|
||||
# 0x0002 = BIT SYNC
|
||||
# 0x0004 = SUBFRAME SYNC
|
||||
# 0x0008 = TIME OF WEEK DECODED
|
||||
# 0x0010 = MSEC AMBIGUOUS
|
||||
# 0x0020 = SYMBOL SYNC
|
||||
# 0x0040 = GLONASS STRING SYNC
|
||||
# 0x0080 = GLONASS TIME OF DAY DECODED
|
||||
# 0x0100 = BEIDOU D2 BIT SYNC
|
||||
# 0x0200 = BEIDOU D2 SUBFRAME SYNC
|
||||
# 0x0400 = GALILEO E1BC CODE LOCK
|
||||
# 0x0800 = GALILEO E1C 2ND CODE LOCK
|
||||
# 0x1000 = GALILEO E1B PAGE SYNC
|
||||
# 0x2000 = GALILEO E1B PAGE SYNC
|
||||
state: int
|
||||
time_of_week_ns: int
|
||||
time_of_week_uncertainty_ns: int # 1-sigma
|
||||
carrier_to_noise_ratio: float
|
||||
pseudorange_rate: float
|
||||
pseudorange_rate_uncertainty: float # 1-sigma
|
||||
|
||||
def __post_init__(self):
|
||||
for field in fields(self):
|
||||
val = getattr(self, field.name)
|
||||
setattr(self, field.name, field.type(val) if val else None)
|
||||
|
||||
def nmea_checksum_ok(s):
|
||||
checksum = 0
|
||||
for i, c in enumerate(s[1:]):
|
||||
if c == "*":
|
||||
if i != len(s) - 4: # should be 3rd to last character
|
||||
print("ERROR: NMEA string does not have checksum delimiter in correct location:", s)
|
||||
return False
|
||||
break
|
||||
checksum ^= ord(c)
|
||||
else:
|
||||
print("ERROR: NMEA string does not have checksum delimiter:", s)
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def process_nmea_port_messages(device:str="/dev/ttyUSB1") -> NoReturn:
|
||||
while True:
|
||||
try:
|
||||
with open(device) as nmeaport:
|
||||
for line in nmeaport:
|
||||
line = line.strip()
|
||||
if DEBUG:
|
||||
print(line)
|
||||
if not line.startswith("$"): # all NMEA messages start with $
|
||||
continue
|
||||
if not nmea_checksum_ok(line):
|
||||
continue
|
||||
|
||||
fields = line.split(",")
|
||||
match fields[0]:
|
||||
case "$GNCLK":
|
||||
# fields at end are reserved (not used)
|
||||
gnss_clock = GnssClockNmeaPort(*fields[1:10]) # type: ignore[arg-type]
|
||||
print(gnss_clock)
|
||||
case "$GNMEAS":
|
||||
# fields at end are reserved (not used)
|
||||
gnss_meas = GnssMeasNmeaPort(*fields[1:14]) # type: ignore[arg-type]
|
||||
print(gnss_meas)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
sleep(1)
|
||||
|
||||
def main() -> NoReturn:
|
||||
from openpilot.common.gpio import gpio_init, gpio_set
|
||||
from openpilot.system.hardware.tici.pins import GPIO
|
||||
from openpilot.system.qcomgpsd.qcomgpsd import at_cmd
|
||||
|
||||
try:
|
||||
check_output(["pidof", "qcomgpsd"])
|
||||
print("qcomgpsd is running, please kill openpilot before running this script! (aborted)")
|
||||
sys.exit(1)
|
||||
except CalledProcessError as e:
|
||||
if e.returncode != 1: # 1 == no process found (pandad not running)
|
||||
raise e
|
||||
|
||||
print("power up antenna ...")
|
||||
gpio_init(GPIO.GNSS_PWR_EN, True)
|
||||
gpio_set(GPIO.GNSS_PWR_EN, True)
|
||||
|
||||
if b"+QGPS: 0" not in (at_cmd("AT+QGPS?") or b""):
|
||||
print("stop location tracking ...")
|
||||
at_cmd("AT+QGPSEND")
|
||||
|
||||
if b'+QGPSCFG: "outport",usbnmea' not in (at_cmd('AT+QGPSCFG="outport"') or b""):
|
||||
print("configure outport ...")
|
||||
at_cmd('AT+QGPSCFG="outport","usbnmea"') # usbnmea = /dev/ttyUSB1
|
||||
|
||||
if b'+QGPSCFG: "gnssrawdata",3,0' not in (at_cmd('AT+QGPSCFG="gnssrawdata"') or b""):
|
||||
print("configure gnssrawdata ...")
|
||||
# AT+QGPSCFG="gnssrawdata",<constellation-mask>,<port>'
|
||||
# <constellation-mask> values:
|
||||
# 0x01 = GPS
|
||||
# 0x02 = GLONASS
|
||||
# 0x04 = BEIDOU
|
||||
# 0x08 = GALILEO
|
||||
# 0x10 = QZSS
|
||||
# <port> values:
|
||||
# 0 = NMEA port
|
||||
# 1 = AT port
|
||||
at_cmd('AT+QGPSCFG="gnssrawdata",3,0') # enable all constellations, output data to NMEA port
|
||||
print("rebooting ...")
|
||||
at_cmd('AT+CFUN=1,1')
|
||||
print("re-run this script when it is back up")
|
||||
sys.exit(2)
|
||||
|
||||
print("starting location tracking ...")
|
||||
at_cmd("AT+QGPS=1")
|
||||
|
||||
process_nmea_port_messages()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Executable
+463
@@ -0,0 +1,463 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
import signal
|
||||
import itertools
|
||||
import math
|
||||
import time
|
||||
import requests
|
||||
import shutil
|
||||
import subprocess
|
||||
import datetime
|
||||
from multiprocessing import Process, Event
|
||||
from typing import NoReturn
|
||||
from struct import unpack_from, calcsize, pack
|
||||
|
||||
from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.gpio import gpio_init, gpio_set
|
||||
from openpilot.common.retry import retry
|
||||
from openpilot.common.time_helpers import system_time_valid
|
||||
from openpilot.system.hardware.tici.pins import GPIO
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.qcomgpsd.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
|
||||
from openpilot.system.qcomgpsd.structs import (dict_unpacker, position_report, relist,
|
||||
gps_measurement_report, gps_measurement_report_sv,
|
||||
glonass_measurement_report, glonass_measurement_report_sv,
|
||||
oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report,
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT)
|
||||
|
||||
DEBUG = int(os.getenv("DEBUG", "0"))==1
|
||||
ASSIST_DATA_FILE = '/tmp/xtra3grc.bin'
|
||||
ASSIST_DATA_FILE_DOWNLOAD = ASSIST_DATA_FILE + '.download'
|
||||
ASSISTANCE_URL = 'http://xtrapath3.izatcloud.net/xtra3grc.bin'
|
||||
|
||||
LOG_TYPES = [
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_POSITION_REPORT,
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT,
|
||||
]
|
||||
|
||||
|
||||
miscStatusFields = {
|
||||
"multipathEstimateIsValid": 0,
|
||||
"directionIsValid": 1,
|
||||
}
|
||||
|
||||
measurementStatusFields = {
|
||||
"subMillisecondIsValid": 0,
|
||||
"subBitTimeIsKnown": 1,
|
||||
"satelliteTimeIsKnown": 2,
|
||||
"bitEdgeConfirmedFromSignal": 3,
|
||||
"measuredVelocity": 4,
|
||||
"fineOrCoarseVelocity": 5,
|
||||
"lockPointValid": 6,
|
||||
"lockPointPositive": 7,
|
||||
|
||||
"lastUpdateFromDifference": 9,
|
||||
"lastUpdateFromVelocityDifference": 10,
|
||||
"strongIndicationOfCrossCorelation": 11,
|
||||
"tentativeMeasurement": 12,
|
||||
"measurementNotUsable": 13,
|
||||
"sirCheckIsNeeded": 14,
|
||||
"probationMode": 15,
|
||||
|
||||
"multipathIndicator": 24,
|
||||
"imdJammingIndicator": 25,
|
||||
"lteB13TxJammingIndicator": 26,
|
||||
"freshMeasurementIndicator": 27,
|
||||
}
|
||||
|
||||
measurementStatusGPSFields = {
|
||||
"gpsRoundRobinRxDiversity": 18,
|
||||
"gpsRxDiversity": 19,
|
||||
"gpsLowBandwidthRxDiversityCombined": 20,
|
||||
"gpsHighBandwidthNu4": 21,
|
||||
"gpsHighBandwidthNu8": 22,
|
||||
"gpsHighBandwidthUniform": 23,
|
||||
}
|
||||
|
||||
measurementStatusGlonassFields = {
|
||||
"glonassMeanderBitEdgeValid": 16,
|
||||
"glonassTimeMarkValid": 17
|
||||
}
|
||||
|
||||
@retry(attempts=10, delay=1.0)
|
||||
def try_setup_logs(diag, logs):
|
||||
return setup_logs(diag, logs)
|
||||
|
||||
@retry(attempts=3, delay=1.0)
|
||||
def at_cmd(cmd: str) -> str | None:
|
||||
return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8')
|
||||
|
||||
def gps_enabled() -> bool:
|
||||
return "QGPS: 1" in at_cmd("AT+QGPS?")
|
||||
|
||||
def download_assistance():
|
||||
try:
|
||||
response = requests.get(ASSISTANCE_URL, timeout=5, stream=True)
|
||||
|
||||
with open(ASSIST_DATA_FILE_DOWNLOAD, 'wb') as fp:
|
||||
for chunk in response.iter_content(chunk_size=8192):
|
||||
fp.write(chunk)
|
||||
if fp.tell() > 1e5:
|
||||
cloudlog.error("Qcom assistance data larger than expected")
|
||||
return
|
||||
|
||||
os.rename(ASSIST_DATA_FILE_DOWNLOAD, ASSIST_DATA_FILE)
|
||||
|
||||
except requests.exceptions.RequestException:
|
||||
cloudlog.exception("Failed to download assistance file")
|
||||
return
|
||||
|
||||
def downloader_loop(event):
|
||||
if os.path.exists(ASSIST_DATA_FILE):
|
||||
os.remove(ASSIST_DATA_FILE)
|
||||
|
||||
alt_path = os.getenv("QCOM_ALT_ASSISTANCE_PATH", None)
|
||||
if alt_path is not None and os.path.exists(alt_path):
|
||||
shutil.copyfile(alt_path, ASSIST_DATA_FILE)
|
||||
|
||||
try:
|
||||
while not os.path.exists(ASSIST_DATA_FILE) and not event.is_set():
|
||||
download_assistance()
|
||||
event.wait(timeout=10)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
|
||||
@retry(attempts=5, delay=0.2, ignore_failure=True)
|
||||
def inject_assistance():
|
||||
cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}"
|
||||
subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True)
|
||||
cloudlog.info("successfully loaded assistance data")
|
||||
|
||||
@retry(attempts=5, delay=1.0)
|
||||
def setup_quectel(diag: ModemDiag) -> bool:
|
||||
ret = False
|
||||
|
||||
# enable OEMDRE in the NV
|
||||
# TODO: it has to reboot for this to take effect
|
||||
DIAG_NV_READ_F = 38
|
||||
DIAG_NV_WRITE_F = 39
|
||||
NV_GNSS_OEM_FEATURE_MASK = 7165
|
||||
send_recv(diag, DIAG_NV_WRITE_F, pack('<HI', NV_GNSS_OEM_FEATURE_MASK, 1))
|
||||
send_recv(diag, DIAG_NV_READ_F, pack('<H', NV_GNSS_OEM_FEATURE_MASK))
|
||||
|
||||
try_setup_logs(diag, LOG_TYPES)
|
||||
|
||||
if gps_enabled():
|
||||
at_cmd("AT+QGPSEND")
|
||||
|
||||
if "GPS_COLD_START" in os.environ:
|
||||
# deletes all assistance
|
||||
at_cmd("AT+QGPSDEL=0")
|
||||
else:
|
||||
# allow module to perform hot start
|
||||
at_cmd("AT+QGPSDEL=1")
|
||||
|
||||
# disable DPO power savings for more accuracy
|
||||
at_cmd("AT+QGPSCFG=\"dpoenable\",0")
|
||||
# don't automatically turn on GNSS on powerup
|
||||
at_cmd("AT+QGPSCFG=\"autogps\",0")
|
||||
|
||||
# Do internet assistance
|
||||
at_cmd("AT+QGPSXTRA=1")
|
||||
at_cmd("AT+QGPSSUPLURL=\"NULL\"")
|
||||
if os.path.exists(ASSIST_DATA_FILE):
|
||||
ret = True
|
||||
inject_assistance()
|
||||
os.remove(ASSIST_DATA_FILE)
|
||||
#at_cmd("AT+QGPSXTRADATA?")
|
||||
if system_time_valid():
|
||||
time_str = datetime.datetime.now(datetime.UTC).replace(tzinfo=None).strftime("%Y/%m/%d,%H:%M:%S")
|
||||
at_cmd(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000")
|
||||
|
||||
at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"")
|
||||
at_cmd("AT+QGPS=1")
|
||||
|
||||
# enable OEMDRE mode
|
||||
DIAG_SUBSYS_CMD_F = 75
|
||||
DIAG_SUBSYS_GPS = 13
|
||||
CGPS_DIAG_PDAPI_CMD = 0x64
|
||||
CGPS_OEM_CONTROL = 202
|
||||
GPSDIAG_OEMFEATURE_DRE = 1
|
||||
GPSDIAG_OEM_DRE_ON = 1
|
||||
|
||||
# gpsdiag_OemControlReqType
|
||||
send_recv(diag, DIAG_SUBSYS_CMD_F, pack('<BHBBIIII',
|
||||
DIAG_SUBSYS_GPS, # Subsystem Id
|
||||
CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code
|
||||
CGPS_OEM_CONTROL, # CGPS Command Code
|
||||
0, # Version
|
||||
GPSDIAG_OEMFEATURE_DRE,
|
||||
GPSDIAG_OEM_DRE_ON,
|
||||
0,0
|
||||
))
|
||||
|
||||
return ret
|
||||
|
||||
def teardown_quectel(diag):
|
||||
at_cmd("AT+QGPSCFG=\"outport\",\"none\"")
|
||||
if gps_enabled():
|
||||
at_cmd("AT+QGPSEND")
|
||||
try_setup_logs(diag, [])
|
||||
|
||||
|
||||
def wait_for_modem(cmd="AT+QGPS?"):
|
||||
cloudlog.warning("waiting for modem to come up")
|
||||
while True:
|
||||
ret = subprocess.call(f"mmcli -m any --timeout 10 --command=\"{cmd}\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True)
|
||||
if ret == 0:
|
||||
return
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
def main() -> NoReturn:
|
||||
unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True)
|
||||
unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True)
|
||||
|
||||
unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True)
|
||||
unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True)
|
||||
|
||||
unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True)
|
||||
unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True)
|
||||
|
||||
unpack_svpoly, _ = dict_unpacker(oemdre_svpoly_report, True)
|
||||
unpack_position, _ = dict_unpacker(position_report)
|
||||
|
||||
unpack_position, _ = dict_unpacker(position_report)
|
||||
|
||||
wait_for_modem()
|
||||
|
||||
stop_download_event = Event()
|
||||
assist_fetch_proc = Process(target=downloader_loop, args=(stop_download_event,))
|
||||
assist_fetch_proc.start()
|
||||
def cleanup(sig, frame):
|
||||
cloudlog.warning("caught sig disabling quectel gps")
|
||||
|
||||
gpio_set(GPIO.GNSS_PWR_EN, False)
|
||||
try:
|
||||
teardown_quectel(diag)
|
||||
cloudlog.warning("quectel cleanup done")
|
||||
except NameError:
|
||||
cloudlog.warning('quectel not yet setup')
|
||||
|
||||
stop_download_event.set()
|
||||
assist_fetch_proc.kill()
|
||||
assist_fetch_proc.join()
|
||||
|
||||
sys.exit(0)
|
||||
signal.signal(signal.SIGINT, cleanup)
|
||||
signal.signal(signal.SIGTERM, cleanup)
|
||||
|
||||
# connect to modem
|
||||
diag = ModemDiag()
|
||||
r = setup_quectel(diag)
|
||||
want_assistance = not r
|
||||
cloudlog.warning("quectel setup done")
|
||||
gpio_init(GPIO.GNSS_PWR_EN, True)
|
||||
gpio_set(GPIO.GNSS_PWR_EN, True)
|
||||
|
||||
pm = messaging.PubMaster(['qcomGnss', 'gpsLocation'])
|
||||
|
||||
while 1:
|
||||
if os.path.exists(ASSIST_DATA_FILE) and want_assistance:
|
||||
setup_quectel(diag)
|
||||
want_assistance = False
|
||||
|
||||
opcode, payload = diag.recv()
|
||||
if opcode != DIAG_LOG_F:
|
||||
cloudlog.error(f"Unhandled opcode: {opcode}")
|
||||
continue
|
||||
|
||||
(pending_msgs, log_outer_length), inner_log_packet = unpack_from('<BH', payload), payload[calcsize('<BH'):]
|
||||
if pending_msgs > 0:
|
||||
cloudlog.debug(f"have {pending_msgs} pending messages")
|
||||
assert log_outer_length == len(inner_log_packet)
|
||||
|
||||
(log_inner_length, log_type, log_time), log_payload = unpack_from('<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):]
|
||||
assert log_inner_length == len(inner_log_packet)
|
||||
|
||||
if log_type not in LOG_TYPES:
|
||||
continue
|
||||
|
||||
if DEBUG:
|
||||
print(f"{time.time():.4f}: got log: {log_type} len {len(log_payload)}")
|
||||
|
||||
if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
|
||||
msg = messaging.new_message('qcomGnss', valid=True)
|
||||
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('drMeasurementReport')
|
||||
report = gnss.drMeasurementReport
|
||||
|
||||
dat = unpack_oemdre_meas(log_payload)
|
||||
for k,v in dat.items():
|
||||
if k in ["gpsTimeBias", "gpsClockTimeUncertainty"]:
|
||||
k += "Ms"
|
||||
if k == "version":
|
||||
assert v == 2
|
||||
elif k == "svCount" or k.startswith("cdmaClockInfo["):
|
||||
# TODO: should we save cdmaClockInfo?
|
||||
pass
|
||||
elif k == "systemRtcValid":
|
||||
setattr(report, k, bool(v))
|
||||
else:
|
||||
setattr(report, k, v)
|
||||
|
||||
report.init('sv', dat['svCount'])
|
||||
sats = log_payload[size_oemdre_meas:]
|
||||
for i in range(dat['svCount']):
|
||||
sat = unpack_oemdre_meas_sv(sats[size_oemdre_meas_sv*i:size_oemdre_meas_sv*(i+1)])
|
||||
sv = report.sv[i]
|
||||
sv.init('measurementStatus')
|
||||
for k,v in sat.items():
|
||||
if k in ["unkn", "measurementStatus2"]:
|
||||
pass
|
||||
elif k == "multipathEstimateValid":
|
||||
sv.measurementStatus.multipathEstimateIsValid = bool(v)
|
||||
elif k == "directionValid":
|
||||
sv.measurementStatus.directionIsValid = bool(v)
|
||||
elif k == "goodParity":
|
||||
setattr(sv, k, bool(v))
|
||||
elif k == "measurementStatus":
|
||||
for kk,vv in measurementStatusFields.items():
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
else:
|
||||
setattr(sv, k, v)
|
||||
pm.send('qcomGnss', msg)
|
||||
elif log_type == LOG_GNSS_POSITION_REPORT:
|
||||
report = unpack_position(log_payload)
|
||||
if report["u_PosSource"] != 2:
|
||||
continue
|
||||
vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]]
|
||||
vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]]
|
||||
|
||||
msg = messaging.new_message('gpsLocation', valid=True)
|
||||
gps = msg.gpsLocation
|
||||
gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi
|
||||
gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi
|
||||
gps.altitude = report["q_FltFinalPosAlt"]
|
||||
gps.speed = math.sqrt(sum([x**2 for x in vNED]))
|
||||
gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
|
||||
|
||||
# TODO needs update if there is another leap second, after june 2024?
|
||||
dt_timestamp = (datetime.datetime(1980, 1, 6, 0, 0, 0, 0, datetime.UTC) +
|
||||
datetime.timedelta(weeks=report['w_GpsWeekNumber']) +
|
||||
datetime.timedelta(seconds=(1e-3*report['q_GpsFixTimeMs'] - 18)))
|
||||
gps.unixTimestampMillis = dt_timestamp.timestamp()*1e3
|
||||
gps.source = log.GpsLocationData.SensorSource.qcomdiag
|
||||
gps.vNED = vNED
|
||||
gps.verticalAccuracy = report["q_FltVdop"]
|
||||
gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi if (report["q_FltHeadingUncRad"] != 0) else 180
|
||||
gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma]))
|
||||
# quectel gps verticalAccuracy is clipped to 500, set invalid if so
|
||||
gps.hasFix = gps.verticalAccuracy != 500
|
||||
if gps.hasFix:
|
||||
want_assistance = False
|
||||
stop_download_event.set()
|
||||
pm.send('gpsLocation', msg)
|
||||
|
||||
elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
|
||||
msg = messaging.new_message('qcomGnss', valid=True)
|
||||
dat = unpack_svpoly(log_payload)
|
||||
dat = relist(dat)
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('drSvPoly')
|
||||
poly = gnss.drSvPoly
|
||||
for k,v in dat.items():
|
||||
if k == "version":
|
||||
assert v == 2
|
||||
elif k == "flags":
|
||||
pass
|
||||
else:
|
||||
setattr(poly, k, v)
|
||||
|
||||
'''
|
||||
# Timestamp glonass polys with GPSTime
|
||||
from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
|
||||
from laika.helpers import get_prn_from_nmea_id
|
||||
prn = get_prn_from_nmea_id(poly.svId)
|
||||
if prn[0] == 'R':
|
||||
epoch = GPSTime(current_gps_time.week, (poly.t0 - 3*SECS_IN_HR + SECS_IN_DAY) % (SECS_IN_WEEK) + get_leap_seconds(current_gps_time))
|
||||
else:
|
||||
epoch = GPSTime(current_gps_time.week, poly.t0)
|
||||
|
||||
# handle week rollover
|
||||
if epoch.tow < SECS_IN_DAY and current_gps_time.tow > 6*SECS_IN_DAY:
|
||||
epoch.week += 1
|
||||
elif epoch.tow > 6*SECS_IN_DAY and current_gps_time.tow < SECS_IN_DAY:
|
||||
epoch.week -= 1
|
||||
|
||||
poly.gpsWeek = epoch.week
|
||||
poly.gpsTow = epoch.tow
|
||||
'''
|
||||
pm.send('qcomGnss', msg)
|
||||
|
||||
elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
|
||||
msg = messaging.new_message('qcomGnss', valid=True)
|
||||
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('measurementReport')
|
||||
report = gnss.measurementReport
|
||||
|
||||
if log_type == LOG_GNSS_GPS_MEASUREMENT_REPORT:
|
||||
dat = unpack_gps_meas(log_payload)
|
||||
sats = log_payload[size_gps_meas:]
|
||||
unpack_meas_sv, size_meas_sv = unpack_gps_meas_sv, size_gps_meas_sv
|
||||
report.source = 0 # gps
|
||||
measurement_status_fields = (measurementStatusFields.items(), measurementStatusGPSFields.items())
|
||||
elif log_type == LOG_GNSS_GLONASS_MEASUREMENT_REPORT:
|
||||
dat = unpack_glonass_meas(log_payload)
|
||||
sats = log_payload[size_glonass_meas:]
|
||||
unpack_meas_sv, size_meas_sv = unpack_glonass_meas_sv, size_glonass_meas_sv
|
||||
report.source = 1 # glonass
|
||||
measurement_status_fields = (measurementStatusFields.items(), measurementStatusGlonassFields.items())
|
||||
else:
|
||||
raise RuntimeError(f"invalid log_type: {log_type}")
|
||||
|
||||
for k,v in dat.items():
|
||||
if k == "version":
|
||||
assert v == 0
|
||||
elif k == "week":
|
||||
report.gpsWeek = v
|
||||
elif k == "svCount":
|
||||
pass
|
||||
else:
|
||||
setattr(report, k, v)
|
||||
report.init('sv', dat['svCount'])
|
||||
if dat['svCount'] > 0:
|
||||
assert len(sats)//dat['svCount'] == size_meas_sv
|
||||
for i in range(dat['svCount']):
|
||||
sv = report.sv[i]
|
||||
sv.init('measurementStatus')
|
||||
sat = unpack_meas_sv(sats[size_meas_sv*i:size_meas_sv*(i+1)])
|
||||
for k,v in sat.items():
|
||||
if k == "parityErrorCount":
|
||||
sv.gpsParityErrorCount = v
|
||||
elif k == "frequencyIndex":
|
||||
sv.glonassFrequencyIndex = v
|
||||
elif k == "hemmingErrorCount":
|
||||
sv.glonassHemmingErrorCount = v
|
||||
elif k == "measurementStatus":
|
||||
for kk,vv in itertools.chain(*measurement_status_fields):
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
elif k == "miscStatus":
|
||||
for kk,vv in miscStatusFields.items():
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
elif k == "pad":
|
||||
pass
|
||||
else:
|
||||
setattr(sv, k, v)
|
||||
|
||||
pm.send('qcomGnss', msg)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,353 @@
|
||||
from struct import unpack_from, calcsize
|
||||
|
||||
LOG_GNSS_POSITION_REPORT = 0x1476
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT = 0x1477
|
||||
LOG_GNSS_CLOCK_REPORT = 0x1478
|
||||
LOG_GNSS_GLONASS_MEASUREMENT_REPORT = 0x1480
|
||||
LOG_GNSS_BDS_MEASUREMENT_REPORT = 0x1756
|
||||
LOG_GNSS_GAL_MEASUREMENT_REPORT = 0x1886
|
||||
|
||||
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT = 0x14DE
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT = 0x14E1
|
||||
|
||||
LOG_GNSS_ME_DPO_STATUS = 0x1838
|
||||
LOG_GNSS_CD_DB_REPORT = 0x147B
|
||||
LOG_GNSS_PRX_RF_HW_STATUS_REPORT = 0x147E
|
||||
LOG_CGPS_SLOW_CLOCK_CLIB_REPORT = 0x1488
|
||||
LOG_GNSS_CONFIGURATION_STATE = 0x1516
|
||||
|
||||
oemdre_measurement_report = """
|
||||
uint8_t version;
|
||||
uint8_t reason;
|
||||
uint8_t sv_count;
|
||||
uint8_t seq_num;
|
||||
uint8_t seq_max;
|
||||
uint16_t rf_loss;
|
||||
|
||||
uint8_t system_rtc_valid;
|
||||
uint32_t f_count;
|
||||
uint32_t clock_resets;
|
||||
uint64_t system_rtc_time;
|
||||
|
||||
uint8_t gps_leap_seconds;
|
||||
uint8_t gps_leap_seconds_uncertainty;
|
||||
float gps_to_glonass_time_bias_milliseconds;
|
||||
float gps_to_glonass_time_bias_milliseconds_uncertainty;
|
||||
|
||||
uint16_t gps_week;
|
||||
uint32_t gps_milliseconds;
|
||||
uint32_t gps_time_bias;
|
||||
uint32_t gps_clock_time_uncertainty;
|
||||
uint8_t gps_clock_source;
|
||||
|
||||
uint8_t glonass_clock_source;
|
||||
uint8_t glonass_year;
|
||||
uint16_t glonass_day;
|
||||
uint32_t glonass_milliseconds;
|
||||
float glonass_time_bias;
|
||||
float glonass_clock_time_uncertainty;
|
||||
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t frequency_source;
|
||||
|
||||
uint32_t cdma_clock_info[5];
|
||||
|
||||
uint8_t source;
|
||||
"""
|
||||
|
||||
oemdre_svpoly_report = """
|
||||
uint8_t version;
|
||||
uint16_t sv_id;
|
||||
int8_t frequency_index;
|
||||
uint8_t flags;
|
||||
uint16_t iode;
|
||||
double t0;
|
||||
double xyz0[3];
|
||||
double xyzN[9];
|
||||
float other[4];
|
||||
float position_uncertainty;
|
||||
float iono_delay;
|
||||
float iono_dot;
|
||||
float sbas_iono_delay;
|
||||
float sbas_iono_dot;
|
||||
float tropo_delay;
|
||||
float elevation;
|
||||
float elevation_dot;
|
||||
float elevation_uncertainty;
|
||||
double velocity_coeff[12];
|
||||
"""
|
||||
|
||||
|
||||
oemdre_measurement_report_sv = """
|
||||
uint8_t sv_id;
|
||||
uint8_t unkn;
|
||||
int8_t glonass_frequency_index;
|
||||
uint32_t observation_state;
|
||||
uint8_t observations;
|
||||
uint8_t good_observations;
|
||||
uint8_t filter_stages;
|
||||
uint8_t predetect_interval;
|
||||
uint8_t cycle_slip_count;
|
||||
uint16_t postdetections;
|
||||
|
||||
uint32_t measurement_status;
|
||||
uint32_t measurement_status2;
|
||||
|
||||
uint16_t carrier_noise;
|
||||
uint16_t rf_loss;
|
||||
int16_t latency;
|
||||
|
||||
float filtered_measurement_fraction;
|
||||
uint32_t filtered_measurement_integral;
|
||||
float filtered_time_uncertainty;
|
||||
float filtered_speed;
|
||||
float filtered_speed_uncertainty;
|
||||
|
||||
float unfiltered_measurement_fraction;
|
||||
uint32_t unfiltered_measurement_integral;
|
||||
float unfiltered_time_uncertainty;
|
||||
float unfiltered_speed;
|
||||
float unfiltered_speed_uncertainty;
|
||||
|
||||
uint8_t multipath_estimate_valid;
|
||||
uint32_t multipath_estimate;
|
||||
uint8_t direction_valid;
|
||||
float azimuth;
|
||||
float elevation;
|
||||
float doppler_acceleration;
|
||||
float fine_speed;
|
||||
float fine_speed_uncertainty;
|
||||
|
||||
uint64_t carrier_phase;
|
||||
uint32_t f_count;
|
||||
|
||||
uint16_t parity_error_count;
|
||||
uint8_t good_parity;
|
||||
"""
|
||||
|
||||
glonass_measurement_report = """
|
||||
uint8_t version;
|
||||
uint32_t f_count;
|
||||
uint8_t glonass_cycle_number;
|
||||
uint16_t glonass_number_of_days;
|
||||
uint32_t milliseconds;
|
||||
float time_bias;
|
||||
float clock_time_uncertainty;
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t sv_count;
|
||||
"""
|
||||
|
||||
glonass_measurement_report_sv = """
|
||||
uint8_t sv_id;
|
||||
int8_t frequency_index;
|
||||
uint8_t observation_state; // SVObservationStates
|
||||
uint8_t observations;
|
||||
uint8_t good_observations;
|
||||
uint8_t hemming_error_count;
|
||||
uint8_t filter_stages;
|
||||
uint16_t carrier_noise;
|
||||
int16_t latency;
|
||||
uint8_t predetect_interval;
|
||||
uint16_t postdetections;
|
||||
uint32_t unfiltered_measurement_integral;
|
||||
float unfiltered_measurement_fraction;
|
||||
float unfiltered_time_uncertainty;
|
||||
float unfiltered_speed;
|
||||
float unfiltered_speed_uncertainty;
|
||||
uint32_t measurement_status;
|
||||
uint8_t misc_status;
|
||||
uint32_t multipath_estimate;
|
||||
float azimuth;
|
||||
float elevation;
|
||||
int32_t carrier_phase_cycles_integral;
|
||||
uint16_t carrier_phase_cycles_fraction;
|
||||
float fine_speed;
|
||||
float fine_speed_uncertainty;
|
||||
uint8_t cycle_slip_count;
|
||||
uint32_t pad;
|
||||
"""
|
||||
|
||||
gps_measurement_report = """
|
||||
uint8_t version;
|
||||
uint32_t f_count;
|
||||
uint16_t week;
|
||||
uint32_t milliseconds;
|
||||
float time_bias;
|
||||
float clock_time_uncertainty;
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t sv_count;
|
||||
"""
|
||||
|
||||
gps_measurement_report_sv = """
|
||||
uint8_t sv_id; // SV PRN
|
||||
uint8_t observation_state; // SV Observation state
|
||||
uint8_t observations; // Count of all observation (both success and failure)
|
||||
uint8_t good_observations; // Count of Good observations
|
||||
uint16_t parity_error_count; // Carrier to Code filtering N count
|
||||
uint8_t filter_stages; // Pre-Detection (Coherent) Interval (msecs)
|
||||
uint16_t carrier_noise; // CNo. Units of 0.1 dB
|
||||
int16_t latency; // Age of the measurement in msecs (+ve meas Meas precedes ref time)
|
||||
uint8_t predetect_interval; // Pre-Detection (Coherent) Interval (msecs)
|
||||
uint16_t postdetections; // Num Post-Detections (uints of PreInts
|
||||
uint32_t unfiltered_measurement_integral; // Range of 0 through (WEEK_MSECS-1) [msecs]
|
||||
float unfiltered_measurement_fraction; // Range of 0 through 0.99999 [msecs]
|
||||
float unfiltered_time_uncertainty; // Time uncertainty (msec)
|
||||
float unfiltered_speed; // Speed estimate (meters/sec)
|
||||
float unfiltered_speed_uncertainty; // Speed uncertainty estimate (meters/sec)
|
||||
uint32_t measurement_status;
|
||||
uint8_t misc_status;
|
||||
uint32_t multipath_estimate;
|
||||
float azimuth; // Azimuth (radians)
|
||||
float elevation; // Elevation (radians)
|
||||
int32_t carrier_phase_cycles_integral;
|
||||
uint16_t carrier_phase_cycles_fraction;
|
||||
float fine_speed; // Carrier phase derived speed
|
||||
float fine_speed_uncertainty; // Carrier phase derived speed UNC
|
||||
uint8_t cycle_slip_count; // Increments when a CSlip is detected
|
||||
uint32_t pad;
|
||||
"""
|
||||
|
||||
position_report = """
|
||||
uint8 u_Version; /* Version number of DM log */
|
||||
uint32 q_Fcount; /* Local millisecond counter */
|
||||
uint8 u_PosSource; /* Source of position information */ /* 0: None 1: Weighted least-squares 2: Kalman filter 3: Externally injected 4: Internal database */
|
||||
uint32 q_Reserved1; /* Reserved memory field */
|
||||
uint16 w_PosVelFlag; /* Position velocity bit field: (see DM log 0x1476 documentation) */
|
||||
uint32 q_PosVelFlag2; /* Position velocity 2 bit field: (see DM log 0x1476 documentation) */
|
||||
uint8 u_FailureCode; /* Failure code: (see DM log 0x1476 documentation) */
|
||||
uint16 w_FixEvents; /* Fix events bit field: (see DM log 0x1476 documentation) */
|
||||
uint32 _fake_align_week_number;
|
||||
uint16 w_GpsWeekNumber; /* GPS week number of position */
|
||||
uint32 q_GpsFixTimeMs; /* GPS fix time of week of in milliseconds */
|
||||
uint8 u_GloNumFourYear; /* Number of Glonass four year cycles */
|
||||
uint16 w_GloNumDaysInFourYear; /* Glonass calendar day in four year cycle */
|
||||
uint32 q_GloFixTimeMs; /* Glonass fix time of day in milliseconds */
|
||||
uint32 q_PosCount; /* Integer count of the number of unique positions reported */
|
||||
uint64 t_DblFinalPosLatLon[2]; /* Final latitude and longitude of position in radians */
|
||||
uint32 q_FltFinalPosAlt; /* Final height-above-ellipsoid altitude of position */
|
||||
uint32 q_FltHeadingRad; /* User heading in radians */
|
||||
uint32 q_FltHeadingUncRad; /* User heading uncertainty in radians */
|
||||
uint32 q_FltVelEnuMps[3]; /* User velocity in east, north, up coordinate frame. In meters per second. */
|
||||
uint32 q_FltVelSigmaMps[3]; /* Gaussian 1-sigma value for east, north, up components of user velocity */
|
||||
uint32 q_FltClockBiasMeters; /* Receiver clock bias in meters */
|
||||
uint32 q_FltClockBiasSigmaMeters; /* Gaussian 1-sigma value for receiver clock bias in meters */
|
||||
uint32 q_FltGGTBMeters; /* GPS to Glonass time bias in meters */
|
||||
uint32 q_FltGGTBSigmaMeters; /* Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltGBTBMeters; /* GPS to BeiDou time bias in meters */
|
||||
uint32 q_FltGBTBSigmaMeters; /* Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
|
||||
uint32 q_FltBGTBMeters; /* BeiDou to Glonass time bias in meters */
|
||||
uint32 q_FltBGTBSigmaMeters; /* Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltFiltGGTBMeters; /* Filtered GPS to Glonass time bias in meters */
|
||||
uint32 q_FltFiltGGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltFiltGBTBMeters; /* Filtered GPS to BeiDou time bias in meters */
|
||||
uint32 q_FltFiltGBTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
|
||||
uint32 q_FltFiltBGTBMeters; /* Filtered BeiDou to Glonass time bias in meters */
|
||||
uint32 q_FltFiltBGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltSftOffsetSec; /* SFT offset as computed by WLS in seconds */
|
||||
uint32 q_FltSftOffsetSigmaSec; /* Gaussian 1-sigma value for SFT offset in seconds */
|
||||
uint32 q_FltClockDriftMps; /* Clock drift (clock frequency bias) in meters per second */
|
||||
uint32 q_FltClockDriftSigmaMps; /* Gaussian 1-sigma value for clock drift in meters per second */
|
||||
uint32 q_FltFilteredAlt; /* Filtered height-above-ellipsoid altitude in meters as computed by WLS */
|
||||
uint32 q_FltFilteredAltSigma; /* Gaussian 1-sigma value for filtered height-above-ellipsoid altitude in meters */
|
||||
uint32 q_FltRawAlt; /* Raw height-above-ellipsoid altitude in meters as computed by WLS */
|
||||
uint32 q_FltRawAltSigma; /* Gaussian 1-sigma value for raw height-above-ellipsoid altitude in meters */
|
||||
uint32 align_Flt[14];
|
||||
uint32 q_FltPdop; /* 3D position dilution of precision as computed from the unweighted
|
||||
uint32 q_FltHdop; /* Horizontal position dilution of precision as computed from the unweighted least-squares covariance matrix */
|
||||
uint32 q_FltVdop; /* Vertical position dilution of precision as computed from the unweighted least-squares covariance matrix */
|
||||
uint8 u_EllipseConfidence; /* Statistical measure of the confidence (percentage) associated with the uncertainty ellipse values */
|
||||
uint32 q_FltEllipseAngle; /* Angle of semimajor axis with respect to true North, with increasing angles moving clockwise from North. In units of degrees. */
|
||||
uint32 q_FltEllipseSemimajorAxis; /* Semimajor axis of final horizontal position uncertainty error ellipse. In units of meters. */
|
||||
uint32 q_FltEllipseSemiminorAxis; /* Semiminor axis of final horizontal position uncertainty error ellipse. In units of meters. */
|
||||
uint32 q_FltPosSigmaVertical; /* Gaussian 1-sigma value for final position height-above-ellipsoid altitude in meters */
|
||||
uint8 u_HorizontalReliability; /* Horizontal position reliability 0: Not set 1: Very Low 2: Low 3: Medium 4: High */
|
||||
uint8 u_VerticalReliability; /* Vertical position reliability */
|
||||
uint16 w_Reserved2; /* Reserved memory field */
|
||||
uint32 q_FltGnssHeadingRad; /* User heading in radians derived from GNSS only solution */
|
||||
uint32 q_FltGnssHeadingUncRad; /* User heading uncertainty in radians derived from GNSS only solution */
|
||||
uint32 q_SensorDataUsageMask; /* Denotes which additional sensor data were used to compute this position fix. BIT[0] 0x00000001 <96> Accelerometer BIT[1] 0x00000002 <96> Gyro 0x0000FFFC - Reserved A bit set to 1 indicates that certain fields as defined by the SENSOR_AIDING_MASK were aided with sensor data*/
|
||||
uint32 q_SensorAidMask; /* Denotes which component of the position report was assisted with additional sensors defined in SENSOR_DATA_USAGE_MASK BIT[0] 0x00000001 <96> Heading aided with sensor data BIT[1] 0x00000002 <96> Speed aided with sensor data BIT[2] 0x00000004 <96> Position aided with sensor data BIT[3] 0x00000008 <96> Velocity aided with sensor data 0xFFFFFFF0 <96> Reserved */
|
||||
uint8 u_NumGpsSvsUsed; /* The number of GPS SVs used in the fix */
|
||||
uint8 u_TotalGpsSvs; /* Total number of GPS SVs detected by searcher, including ones not used in position calculation */
|
||||
uint8 u_NumGloSvsUsed; /* The number of Glonass SVs used in the fix */
|
||||
uint8 u_TotalGloSvs; /* Total number of Glonass SVs detected by searcher, including ones not used in position calculation */
|
||||
uint8 u_NumBdsSvsUsed; /* The number of BeiDou SVs used in the fix */
|
||||
uint8 u_TotalBdsSvs; /* Total number of BeiDou SVs detected by searcher, including ones not used in position calculation */
|
||||
""" # noqa: E501
|
||||
|
||||
def name_to_camelcase(nam):
|
||||
ret = []
|
||||
i = 0
|
||||
while i < len(nam):
|
||||
if nam[i] == "_":
|
||||
ret.append(nam[i+1].upper())
|
||||
i += 2
|
||||
else:
|
||||
ret.append(nam[i])
|
||||
i += 1
|
||||
return ''.join(ret)
|
||||
|
||||
def parse_struct(ss):
|
||||
st = "<"
|
||||
nams = []
|
||||
for l in ss.strip().split("\n"):
|
||||
if len(l.strip()) == 0:
|
||||
continue
|
||||
typ, nam = l.split(";")[0].split()
|
||||
#print(typ, nam)
|
||||
if typ == "float" or '_Flt' in nam:
|
||||
st += "f"
|
||||
elif typ == "double" or '_Dbl' in nam:
|
||||
st += "d"
|
||||
elif typ in ["uint8", "uint8_t"]:
|
||||
st += "B"
|
||||
elif typ in ["int8", "int8_t"]:
|
||||
st += "b"
|
||||
elif typ in ["uint32", "uint32_t"]:
|
||||
st += "I"
|
||||
elif typ in ["int32", "int32_t"]:
|
||||
st += "i"
|
||||
elif typ in ["uint16", "uint16_t"]:
|
||||
st += "H"
|
||||
elif typ in ["int16", "int16_t"]:
|
||||
st += "h"
|
||||
elif typ in ["uint64", "uint64_t"]:
|
||||
st += "Q"
|
||||
else:
|
||||
raise RuntimeError(f"unknown type {typ}")
|
||||
if '[' in nam:
|
||||
cnt = int(nam.split("[")[1].split("]")[0])
|
||||
st += st[-1]*(cnt-1)
|
||||
for i in range(cnt):
|
||||
nams.append(f'{nam.split("[")[0]}[{i}]')
|
||||
else:
|
||||
nams.append(nam)
|
||||
return st, nams
|
||||
|
||||
def dict_unpacker(ss, camelcase = False):
|
||||
st, nams = parse_struct(ss)
|
||||
if camelcase:
|
||||
nams = [name_to_camelcase(x) for x in nams]
|
||||
sz = calcsize(st)
|
||||
return lambda x: dict(zip(nams, unpack_from(st, x), strict=True)), sz
|
||||
|
||||
def relist(dat):
|
||||
list_keys = set()
|
||||
for key in dat.keys():
|
||||
if '[' in key:
|
||||
list_keys.add(key.split('[')[0])
|
||||
list_dict = {}
|
||||
for list_key in list_keys:
|
||||
list_dict[list_key] = []
|
||||
i = 0
|
||||
while True:
|
||||
key = list_key + f'[{i}]'
|
||||
if key not in dat:
|
||||
break
|
||||
list_dict[list_key].append(dat[key])
|
||||
del dat[key]
|
||||
i += 1
|
||||
return {**dat, **list_dict}
|
||||
@@ -0,0 +1 @@
|
||||
sensord
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user