Merge branch 'upstream/openpilot/master' into sync-20250710

# Conflicts:
#	.github/workflows/selfdrive_tests.yaml
#	README.md
#	common/params_keys.h
#	opendbc_repo
#	panda
#	selfdrive/controls/lib/longitudinal_planner.py
#	selfdrive/controls/lib/tests/test_latcontrol.py
#	selfdrive/ui/soundd.py
#	selfdrive/ui/translations/main_ar.ts
#	selfdrive/ui/translations/main_de.ts
#	selfdrive/ui/translations/main_es.ts
#	selfdrive/ui/translations/main_fr.ts
#	selfdrive/ui/translations/main_ja.ts
#	selfdrive/ui/translations/main_ko.ts
#	selfdrive/ui/translations/main_pt-BR.ts
#	selfdrive/ui/translations/main_th.ts
#	selfdrive/ui/translations/main_tr.ts
#	selfdrive/ui/translations/main_zh-CHS.ts
#	selfdrive/ui/translations/main_zh-CHT.ts
#	tinygrad_repo
This commit is contained in:
Jason Wen
2025-07-19 00:19:12 -04:00
64 changed files with 1457 additions and 1095 deletions
+22 -6
View File
@@ -23,6 +23,7 @@ from typing import cast
from collections.abc import Callable
import requests
from requests.adapters import HTTPAdapter, DEFAULT_POOLBLOCK
from jsonrpc import JSONRPCResponseManager, dispatcher
from websocket import (ABNF, WebSocket, WebSocketException, WebSocketTimeoutException,
create_connection)
@@ -56,6 +57,11 @@ WS_FRAME_SIZE = 4096
DEVICE_STATE_UPDATE_INTERVAL = 1.0 # in seconds
DEFAULT_UPLOAD_PRIORITY = 99 # higher number = lower priority
# https://bytesolutions.com/dscp-tos-cos-precedence-conversion-chart,
# https://en.wikipedia.org/wiki/Differentiated_services
UPLOAD_TOS = 0x20 # CS1, low priority background traffic
SSH_TOS = 0x90 # AF42, DSCP of 36/HDD_LINUX_AC_VI with the minimum delay flag
NetworkType = log.DeviceState.NetworkType
UploadFileDict = dict[str, str | int | float | bool]
@@ -64,6 +70,17 @@ UploadItemDict = dict[str, str | bool | int | float | dict[str, str]]
UploadFilesToUrlResponse = dict[str, int | list[UploadItemDict] | list[str]]
class UploadTOSAdapter(HTTPAdapter):
def init_poolmanager(self, connections, maxsize, block=DEFAULT_POOLBLOCK, **pool_kwargs):
pool_kwargs["socket_options"] = [(socket.IPPROTO_IP, socket.IP_TOS, UPLOAD_TOS)]
super().init_poolmanager(connections, maxsize, block, **pool_kwargs)
UPLOAD_SESS = requests.Session()
UPLOAD_SESS.mount("http://", UploadTOSAdapter())
UPLOAD_SESS.mount("https://", UploadTOSAdapter())
@dataclass
class UploadFile:
fn: str
@@ -311,10 +328,10 @@ def _do_upload(upload_item: UploadItem, callback: Callable = None) -> requests.R
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)
response = UPLOAD_SESS.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:
@@ -501,8 +518,7 @@ def start_local_proxy_shim(global_end_event: threading.Event, local_port: int, w
raise Exception("Requested local port not whitelisted")
# 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)
ws.sock.setsockopt(socket.IPPROTO_IP, socket.IP_TOS, SSH_TOS)
ssock, csock = socket.socketpair()
local_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+3 -3
View File
@@ -19,7 +19,7 @@ from cereal import messaging
from openpilot.common.params import Params
from openpilot.common.timeout import Timeout
from openpilot.system.athena import athenad
from openpilot.system.athena.athenad import MAX_RETRY_COUNT, dispatcher
from openpilot.system.athena.athenad import MAX_RETRY_COUNT, UPLOAD_SESS, dispatcher
from openpilot.system.athena.tests.helpers import HTTPRequestHandler, MockWebsocket, MockApi, EchoSocket
from openpilot.selfdrive.test.helpers import http_server_context
from openpilot.system.hardware.hw import Paths
@@ -29,7 +29,7 @@ def seed_athena_server(host, port):
with Timeout(2, 'HTTP Server seeding failed'):
while True:
try:
requests.put(f'http://{host}:{port}/qlog.zst', data='', timeout=10)
UPLOAD_SESS.put(f'http://{host}:{port}/qlog.zst', data='', timeout=10)
break
except requests.exceptions.ConnectionError:
time.sleep(0.1)
@@ -239,7 +239,7 @@ class TestAthenadMethods:
@pytest.mark.parametrize("status,retry", [(500,True), (412,False)])
@with_upload_handler
def test_upload_handler_retry(self, mocker, host, status, retry):
mock_put = mocker.patch('requests.put')
mock_put = mocker.patch('openpilot.system.athena.athenad.UPLOAD_SESS.put')
mock_put.return_value.__enter__.return_value.status_code = status
fn = self._create_file('qlog.zst')
item = athenad.UploadItem(path=fn, url=f"{host}/qlog.zst", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True)
+2 -2
View File
@@ -415,8 +415,8 @@ class Tici(HardwareBase):
# *** 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/min_pwrlevel")
sudo_write("1", "/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")
+55 -22
View File
@@ -62,6 +62,7 @@ struct RemoteEncoder {
bool recording = false;
bool marked_ready_to_rotate = false;
bool seen_first_packet = false;
bool audio_initialized = false;
};
size_t write_encode_data(LoggerdState *s, cereal::Event::Reader event, RemoteEncoder &re, const EncoderInfo &encoder_info) {
@@ -78,12 +79,7 @@ size_t write_encode_data(LoggerdState *s, cereal::Event::Reader event, RemoteEnc
LOGW("%s: dropped %d non iframe packets before init", encoder_info.publish_name, re.dropped_frames);
re.dropped_frames = 0;
}
// if we aren't actually recording, don't create the writer
if (encoder_info.record) {
assert(encoder_info.filename != NULL);
re.writer.reset(new VideoWriter(s->logger.segmentPath().c_str(),
encoder_info.filename, idx.getType() != cereal::EncodeIndex::Type::FULL_H_E_V_C,
edata.getWidth(), edata.getHeight(), encoder_info.fps, idx.getType()));
// write the header
auto header = edata.getHeader();
re.writer->write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof() / 1000, true, false);
@@ -138,12 +134,19 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
// if this is a new segment, we close any possible old segments, move to the new, and process any queued packets
if (re.current_segment != s->logger.segment()) {
if (re.recording) {
re.writer.reset();
// if we aren't actually recording, don't create the writer
if (encoder_info.record) {
assert(encoder_info.filename != NULL);
re.writer.reset(new VideoWriter(s->logger.segmentPath().c_str(),
encoder_info.filename, idx.getType() != cereal::EncodeIndex::Type::FULL_H_E_V_C,
edata.getWidth(), edata.getHeight(), encoder_info.fps, idx.getType()));
re.recording = false;
re.audio_initialized = false;
}
re.current_segment = s->logger.segment();
re.marked_ready_to_rotate = false;
}
if (re.audio_initialized || !encoder_info.include_audio) {
// we are in this segment now, process any queued messages before this one
if (!re.q.empty()) {
for (auto qmsg : re.q) {
@@ -153,9 +156,14 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
}
re.q.clear();
}
bytes_count += write_encode_data(s, event, re, encoder_info);
delete msg;
} else if (re.q.size() > MAIN_FPS*10) {
LOGE_100("%s: dropping frame waiting for audio initialization, queue is too large", name.c_str());
delete msg;
} else {
re.q.push_back(msg); // queue up all the new segment messages, they go in after audio is initialized
}
bytes_count += write_encode_data(s, event, re, encoder_info);
delete msg;
} else if (offset_segment_num > s->logger.segment()) {
// encoderd packet has a newer segment, this means encoderd has rolled over
if (!re.marked_ready_to_rotate) {
@@ -214,7 +222,7 @@ void loggerd_thread() {
typedef struct ServiceState {
std::string name;
int counter, freq;
bool encoder, user_flag;
bool encoder, user_flag, record_audio;
} ServiceState;
std::unordered_map<SubSocket*, ServiceState> service_state;
std::unordered_map<SubSocket*, struct RemoteEncoder> remote_encoders;
@@ -226,19 +234,22 @@ void loggerd_thread() {
for (const auto& [_, it] : services) {
const bool encoder = util::ends_with(it.name, "EncodeData");
const bool livestream_encoder = util::starts_with(it.name, "livestream");
if (!it.should_log && (!encoder || livestream_encoder)) continue;
LOGD("logging %s", it.name.c_str());
const bool record_audio = (it.name == "rawAudioData") && Params().getBool("RecordAudio");
if (it.should_log || (encoder && !livestream_encoder) || record_audio) {
LOGD("logging %s", it.name.c_str());
SubSocket * sock = SubSocket::create(ctx.get(), it.name);
assert(sock != NULL);
poller->registerSocket(sock);
service_state[sock] = {
.name = it.name,
.counter = 0,
.freq = it.decimation,
.encoder = encoder,
.user_flag = it.name == "userFlag",
};
SubSocket * sock = SubSocket::create(ctx.get(), it.name);
assert(sock != NULL);
poller->registerSocket(sock);
service_state[sock] = {
.name = it.name,
.counter = 0,
.freq = it.decimation,
.encoder = encoder,
.user_flag = it.name == "userFlag",
.record_audio = record_audio,
};
}
}
LoggerdState s;
@@ -247,6 +258,7 @@ void loggerd_thread() {
Params().put("CurrentRoute", s.logger.routeName());
std::map<std::string, EncoderInfo> encoder_infos_dict;
std::vector<RemoteEncoder*> encoders_with_audio;
for (const auto &cam : cameras_logged) {
for (const auto &encoder_info : cam.encoder_infos) {
encoder_infos_dict[encoder_info.publish_name] = encoder_info;
@@ -254,6 +266,13 @@ void loggerd_thread() {
}
}
for (auto &[sock, service] : service_state) {
auto it = encoder_infos_dict.find(service.name);
if (it != encoder_infos_dict.end() && it->second.include_audio) {
encoders_with_audio.push_back(&remote_encoders[sock]);
}
}
uint64_t msg_count = 0, bytes_count = 0;
double start_ts = millis_since_boot();
while (!do_exit) {
@@ -271,6 +290,20 @@ void loggerd_thread() {
Message *msg = nullptr;
while (!do_exit && (msg = sock->receive(true))) {
const bool in_qlog = service.freq != -1 && (service.counter++ % service.freq == 0);
if (service.record_audio) {
capnp::FlatArrayMessageReader cmsg(kj::ArrayPtr<capnp::word>((capnp::word *)msg->getData(), msg->getSize() / sizeof(capnp::word)));
auto event = cmsg.getRoot<cereal::Event>();
auto audio_data = event.getRawAudioData().getData();
auto sample_rate = event.getRawAudioData().getSampleRate();
for (auto* encoder : encoders_with_audio) {
if (encoder && encoder->writer) {
encoder->writer->write_audio((uint8_t*)audio_data.begin(), audio_data.size(), event.getLogMonoTime() / 1000, sample_rate);
encoder->audio_initialized = true;
}
}
}
if (service.encoder) {
s.last_camera_seen_tms = millis_since_boot();
bytes_count += handle_encoder_msg(&s, msg, service.name, remote_encoders[sock], encoder_infos_dict[service.name]);
+2
View File
@@ -35,6 +35,7 @@ public:
const char *thumbnail_name = NULL;
const char *filename = NULL;
bool record = true;
bool include_audio = false;
int frame_width = -1;
int frame_height = -1;
int fps = MAIN_FPS;
@@ -106,6 +107,7 @@ const EncoderInfo qcam_encoder_info = {
.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
.frame_width = 526,
.frame_height = 330,
.include_audio = Params().getBool("RecordAudio"),
INIT_ENCODE_FUNCTIONS(QRoadEncode),
};
+76 -35
View File
@@ -97,6 +97,50 @@ class TestLoggerd:
return sent_msgs
def _publish_camera_and_audio_messages(self, num_segs=1, segment_length=5):
d = DEVICE_CAMERAS[("tici", "ar0231")]
streams = [
(VisionStreamType.VISION_STREAM_ROAD, (d.fcam.width, d.fcam.height, 2048 * 2346, 2048, 2048 * 1216), "roadCameraState"),
(VisionStreamType.VISION_STREAM_DRIVER, (d.dcam.width, d.dcam.height, 2048 * 2346, 2048, 2048 * 1216), "driverCameraState"),
(VisionStreamType.VISION_STREAM_WIDE_ROAD, (d.ecam.width, d.ecam.height, 2048 * 2346, 2048, 2048 * 1216), "wideRoadCameraState"),
]
pm = messaging.PubMaster([s for _, _, s in streams] + ["rawAudioData"])
vipc_server = VisionIpcServer("camerad")
for stream_type, frame_spec, _ in streams:
vipc_server.create_buffers_with_sizes(stream_type, 40, *(frame_spec))
vipc_server.start_listener()
os.environ["LOGGERD_TEST"] = "1"
os.environ["LOGGERD_SEGMENT_LENGTH"] = str(segment_length)
managed_processes["loggerd"].start()
managed_processes["encoderd"].start()
assert pm.wait_for_readers_to_update("roadCameraState", timeout=5)
fps = 20
for n in range(1, int(num_segs * segment_length * fps) + 1):
# send video
for stream_type, frame_spec, state in streams:
dat = np.empty(frame_spec[2], dtype=np.uint8)
vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n / fps, n / fps)
camera_state = messaging.new_message(state)
frame = getattr(camera_state, state)
frame.frameId = n
pm.send(state, camera_state)
# send audio
msg = messaging.new_message('rawAudioData')
msg.rawAudioData.data = bytes(800 * 2) # 800 samples of int16
msg.rawAudioData.sampleRate = 16000
pm.send('rawAudioData', msg)
for _, _, state in streams:
assert pm.wait_for_readers_to_update(state, timeout=5, dt=0.001)
managed_processes["loggerd"].stop()
managed_processes["encoderd"].stop()
def test_init_data_values(self):
os.environ["CLEAN"] = random.choice(["0", "1"])
@@ -136,53 +180,23 @@ class TestLoggerd:
assert getattr(initData, initData_key) == v
assert logged_params[param_key].decode() == v
@pytest.mark.skip("FIXME: encoderd sometimes crashes in CI when running with pytest-xdist")
@pytest.mark.xdist_group("camera_encoder_tests") # setting xdist group ensures tests are run in same worker, prevents encoderd from crashing
def test_rotation(self):
os.environ["LOGGERD_TEST"] = "1"
Params().put("RecordFront", "1")
d = DEVICE_CAMERAS[("tici", "ar0231")]
expected_files = {"rlog.zst", "qlog.zst", "qcamera.ts", "fcamera.hevc", "dcamera.hevc", "ecamera.hevc"}
streams = [(VisionStreamType.VISION_STREAM_ROAD, (d.fcam.width, d.fcam.height, 2048*2346, 2048, 2048*1216), "roadCameraState"),
(VisionStreamType.VISION_STREAM_DRIVER, (d.dcam.width, d.dcam.height, 2048*2346, 2048, 2048*1216), "driverCameraState"),
(VisionStreamType.VISION_STREAM_WIDE_ROAD, (d.ecam.width, d.ecam.height, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")]
pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"])
vipc_server = VisionIpcServer("camerad")
for stream_type, frame_spec, _ in streams:
vipc_server.create_buffers_with_sizes(stream_type, 40, *(frame_spec))
vipc_server.start_listener()
num_segs = random.randint(2, 3)
length = random.randint(4, 5) # H264 encoder uses 40 lookahead frames and does B-frame reordering, so minimum 3 seconds before qcam output
num_segs = random.randint(2, 5)
length = random.randint(1, 3)
os.environ["LOGGERD_SEGMENT_LENGTH"] = str(length)
managed_processes["loggerd"].start()
managed_processes["encoderd"].start()
assert pm.wait_for_readers_to_update("roadCameraState", timeout=5)
fps = 20.0
for n in range(1, int(num_segs*length*fps)+1):
for stream_type, frame_spec, state in streams:
dat = np.empty(frame_spec[2], dtype=np.uint8)
vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n/fps, n/fps)
camera_state = messaging.new_message(state)
frame = getattr(camera_state, state)
frame.frameId = n
pm.send(state, camera_state)
for _, _, state in streams:
assert pm.wait_for_readers_to_update(state, timeout=5, dt=0.001)
managed_processes["loggerd"].stop()
managed_processes["encoderd"].stop()
self._publish_camera_and_audio_messages(num_segs=num_segs, segment_length=length)
route_path = str(self._get_latest_log_dir()).rsplit("--", 1)[0]
for n in range(num_segs):
p = Path(f"{route_path}--{n}")
logged = {f.name for f in p.iterdir() if f.is_file()}
diff = logged ^ expected_files
assert len(diff) == 0, f"didn't get all expected files. run={_} seg={n} {route_path=}, {diff=}\n{logged=} {expected_files=}"
assert len(diff) == 0, f"didn't get all expected files. seg={n} {route_path=}, {diff=}\n{logged=} {expected_files=}"
def test_bootlog(self):
# generate bootlog with fake launch log
@@ -281,3 +295,30 @@ class TestLoggerd:
segment_dir = self._get_latest_log_dir()
assert getxattr(segment_dir, PRESERVE_ATTR_NAME) is None
@pytest.mark.xdist_group("camera_encoder_tests") # setting xdist group ensures tests are run in same worker, prevents encoderd from crashing
@pytest.mark.parametrize("record_front", [True, False])
def test_record_front(self, record_front):
params = Params()
params.put_bool("RecordFront", record_front)
self._publish_camera_and_audio_messages()
dcamera_hevc_exists = os.path.exists(os.path.join(self._get_latest_log_dir(), 'dcamera.hevc'))
assert dcamera_hevc_exists == record_front
@pytest.mark.xdist_group("camera_encoder_tests") # setting xdist group ensures tests are run in same worker, prevents encoderd from crashing
@pytest.mark.parametrize("record_audio", [True, False])
def test_record_audio(self, record_audio):
params = Params()
params.put_bool("RecordAudio", record_audio)
self._publish_camera_and_audio_messages()
qcamera_ts_path = os.path.join(self._get_latest_log_dir(), 'qcamera.ts')
ffprobe_cmd = f"ffprobe -i {qcamera_ts_path} -show_streams -select_streams a -loglevel error"
has_audio_stream = subprocess.run(ffprobe_cmd, shell=True, capture_output=True).stdout.strip() != b''
assert has_audio_stream == record_audio
raw_audio_in_rlog = any(m.which() == 'rawAudioData' for m in LogReader(os.path.join(self._get_latest_log_dir(), 'rlog.zst')))
assert raw_audio_in_rlog == record_audio
+111
View File
@@ -50,6 +50,45 @@ VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing,
}
}
void VideoWriter::initialize_audio(int sample_rate) {
assert(this->ofmt_ctx->oformat->audio_codec != AV_CODEC_ID_NONE); // check output format supports audio streams
const AVCodec *audio_avcodec = avcodec_find_encoder(AV_CODEC_ID_AAC);
assert(audio_avcodec);
this->audio_codec_ctx = avcodec_alloc_context3(audio_avcodec);
assert(this->audio_codec_ctx);
this->audio_codec_ctx->sample_fmt = AV_SAMPLE_FMT_FLTP;
this->audio_codec_ctx->sample_rate = sample_rate;
#if LIBAVUTIL_VERSION_INT >= AV_VERSION_INT(57, 28, 100) // FFmpeg 5.1+
av_channel_layout_default(&this->audio_codec_ctx->ch_layout, 1);
#else
this->audio_codec_ctx->channel_layout = AV_CH_LAYOUT_MONO;
#endif
this->audio_codec_ctx->bit_rate = 32000;
this->audio_codec_ctx->flags |= AV_CODEC_FLAG_GLOBAL_HEADER;
this->audio_codec_ctx->time_base = (AVRational){1, audio_codec_ctx->sample_rate};
int err = avcodec_open2(this->audio_codec_ctx, audio_avcodec, NULL);
assert(err >= 0);
av_log_set_level(AV_LOG_WARNING); // hide "QAvg" info msgs at the end of every segment
this->audio_stream = avformat_new_stream(this->ofmt_ctx, NULL);
assert(this->audio_stream);
err = avcodec_parameters_from_context(this->audio_stream->codecpar, this->audio_codec_ctx);
assert(err >= 0);
this->audio_frame = av_frame_alloc();
assert(this->audio_frame);
this->audio_frame->format = this->audio_codec_ctx->sample_fmt;
#if LIBAVUTIL_VERSION_INT >= AV_VERSION_INT(57, 28, 100) // FFmpeg 5.1+
av_channel_layout_copy(&this->audio_frame->ch_layout, &this->audio_codec_ctx->ch_layout);
#else
this->audio_frame->channel_layout = this->audio_codec_ctx->channel_layout;
#endif
this->audio_frame->sample_rate = this->audio_codec_ctx->sample_rate;
this->audio_frame->nb_samples = this->audio_codec_ctx->frame_size;
err = av_frame_get_buffer(this->audio_frame, 0);
assert(err >= 0);
}
void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe) {
if (of && data) {
size_t written = util::safe_fwrite(data, 1, len, of);
@@ -67,8 +106,10 @@ void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecc
}
int err = avcodec_parameters_from_context(out_stream->codecpar, codec_ctx);
assert(err >= 0);
// if there is an audio stream, it must be initialized before this point
err = avformat_write_header(ofmt_ctx, NULL);
assert(err >= 0);
header_written = true;
} else {
// input timestamps are in microseconds
AVRational in_timebase = {1, 1000000};
@@ -77,6 +118,7 @@ void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecc
av_init_packet(&pkt);
pkt.data = data;
pkt.size = len;
pkt.stream_index = this->out_stream->index;
enum AVRounding rnd = static_cast<enum AVRounding>(AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX);
pkt.pts = pkt.dts = av_rescale_q_rnd(timestamp, in_timebase, ofmt_ctx->streams[0]->time_base, rnd);
@@ -95,11 +137,80 @@ void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecc
}
}
void VideoWriter::write_audio(uint8_t *data, int len, long long timestamp, int sample_rate) {
if (!remuxing) return;
if (!audio_initialized) {
initialize_audio(sample_rate);
audio_initialized = true;
}
if (!audio_codec_ctx) return;
// sync logMonoTime of first audio packet with the timestampEof of first video packet
if (audio_pts == 0) {
audio_pts = (timestamp * audio_codec_ctx->sample_rate) / 1000000ULL;
}
// convert s16le samples to fltp and add to buffer
const int16_t *raw_samples = reinterpret_cast<const int16_t*>(data);
int sample_count = len / sizeof(int16_t);
constexpr float normalizer = 1.0f / 32768.0f;
const size_t max_buffer_size = sample_rate * 10; // 10 seconds
if (audio_buffer.size() + sample_count > max_buffer_size) {
size_t samples_to_drop = (audio_buffer.size() + sample_count) - max_buffer_size;
LOGE("Audio buffer overflow, dropping %zu oldest samples", samples_to_drop);
audio_buffer.erase(audio_buffer.begin(), audio_buffer.begin() + samples_to_drop);
audio_pts += samples_to_drop;
}
// Add new samples to the buffer
const size_t original_size = audio_buffer.size();
audio_buffer.resize(original_size + sample_count);
std::transform(raw_samples, raw_samples + sample_count, audio_buffer.begin() + original_size,
[](int16_t sample) { return sample * normalizer; });
if (!header_written) return; // header not written yet, process audio frame after header is written
while (audio_buffer.size() >= audio_codec_ctx->frame_size) {
audio_frame->pts = audio_pts;
float *f_samples = reinterpret_cast<float*>(audio_frame->data[0]);
std::copy(audio_buffer.begin(), audio_buffer.begin() + audio_codec_ctx->frame_size, f_samples);
audio_buffer.erase(audio_buffer.begin(), audio_buffer.begin() + audio_codec_ctx->frame_size);
encode_and_write_audio_frame(audio_frame);
}
}
void VideoWriter::encode_and_write_audio_frame(AVFrame* frame) {
if (!remuxing || !audio_codec_ctx) return;
int send_result = avcodec_send_frame(audio_codec_ctx, frame); // encode frame
if (send_result >= 0) {
AVPacket *pkt = av_packet_alloc();
while (avcodec_receive_packet(audio_codec_ctx, pkt) == 0) {
av_packet_rescale_ts(pkt, audio_codec_ctx->time_base, audio_stream->time_base);
pkt->stream_index = audio_stream->index;
int err = av_interleaved_write_frame(ofmt_ctx, pkt); // write encoded frame
if (err < 0) {
LOGW("AUDIO: Write frame failed - error: %d", err);
}
av_packet_unref(pkt);
}
av_packet_free(&pkt);
} else {
LOGW("AUDIO: Failed to send audio frame to encoder: %d", send_result);
}
audio_pts += audio_codec_ctx->frame_size;
}
VideoWriter::~VideoWriter() {
if (this->remuxing) {
if (this->audio_codec_ctx) {
encode_and_write_audio_frame(NULL); // flush encoder
avcodec_free_context(&this->audio_codec_ctx);
}
int err = av_write_trailer(this->ofmt_ctx);
if (err != 0) LOGE("av_write_trailer failed %d", err);
avcodec_free_context(&this->codec_ctx);
if (this->audio_frame) av_frame_free(&this->audio_frame);
err = avio_closep(&this->ofmt_ctx->pb);
if (err != 0) LOGE("avio_closep failed %d", err);
avformat_free_context(this->ofmt_ctx);
+16
View File
@@ -1,6 +1,7 @@
#pragma once
#include <string>
#include <deque>
extern "C" {
#include <libavformat/avformat.h>
@@ -13,13 +14,28 @@ 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);
void write_audio(uint8_t *data, int len, long long timestamp, int sample_rate);
~VideoWriter();
private:
void initialize_audio(int sample_rate);
void encode_and_write_audio_frame(AVFrame* frame);
std::string vid_path, lock_path;
FILE *of = nullptr;
AVCodecContext *codec_ctx;
AVFormatContext *ofmt_ctx;
AVStream *out_stream;
bool audio_initialized = false;
bool header_written = false;
AVStream *audio_stream = nullptr;
AVCodecContext *audio_codec_ctx = nullptr;
AVFrame *audio_frame = nullptr;
uint64_t audio_pts = 0;
std::deque<float> audio_buffer;
bool remuxing;
};
+15 -9
View File
@@ -9,10 +9,10 @@ from openpilot.common.retry import retry
from openpilot.common.swaglog import cloudlog
RATE = 10
FFT_SAMPLES = 4096
FFT_SAMPLES = 1600 # 100ms
REFERENCE_SPL = 2e-5 # newtons/m^2
SAMPLE_RATE = 44100
SAMPLE_BUFFER = 4096 # approx 100ms
SAMPLE_RATE = 16000
SAMPLE_BUFFER = 800 # 50ms
@cache
@@ -45,7 +45,7 @@ def apply_a_weighting(measurements: np.ndarray) -> np.ndarray:
class Mic:
def __init__(self):
self.rk = Ratekeeper(RATE)
self.pm = messaging.PubMaster(['microphone'])
self.pm = messaging.PubMaster(['soundPressure', 'rawAudioData'])
self.measurements = np.empty(0)
@@ -61,12 +61,12 @@ class Mic:
sound_pressure_weighted = self.sound_pressure_weighted
sound_pressure_level_weighted = self.sound_pressure_level_weighted
msg = messaging.new_message('microphone', valid=True)
msg.microphone.soundPressure = float(sound_pressure)
msg.microphone.soundPressureWeighted = float(sound_pressure_weighted)
msg.microphone.soundPressureWeightedDb = float(sound_pressure_level_weighted)
msg = messaging.new_message('soundPressure', valid=True)
msg.soundPressure.soundPressure = float(sound_pressure)
msg.soundPressure.soundPressureWeighted = float(sound_pressure_weighted)
msg.soundPressure.soundPressureWeightedDb = float(sound_pressure_level_weighted)
self.pm.send('microphone', msg)
self.pm.send('soundPressure', msg)
self.rk.keep_time()
def callback(self, indata, frames, time, status):
@@ -76,6 +76,12 @@ class Mic:
Logged A-weighted equivalents are rough approximations of the human-perceived loudness.
"""
msg = messaging.new_message('rawAudioData', valid=True)
audio_data_int_16 = (indata[:, 0] * 32767).astype(np.int16)
msg.rawAudioData.data = audio_data_int_16.tobytes()
msg.rawAudioData.sampleRate = SAMPLE_RATE
self.pm.send('rawAudioData', msg)
with self.lock:
self.measurements = np.concatenate((self.measurements, indata[:, 0]))
+2 -2
View File
@@ -7,10 +7,10 @@ from openpilot.system.sensord.sensors.i2c_sensor import Sensor
class LSM6DS3_Temp(Sensor):
@property
def device_address(self) -> int:
return 0x6A # Default I2C address for LSM6DS3
return 0x6A
def _read_temperature(self) -> float:
scale = 16.0 if log.SensorEventData.SensorSource.lsm6ds3 else 256.0
scale = 16.0 if self.source == log.SensorEventData.SensorSource.lsm6ds3 else 256.0
data = self.read(0x20, 2)
return 25 + (self.parse_16bit(data[0], data[1]) / scale)
+17 -10
View File
@@ -136,6 +136,17 @@ class TTYPigeon:
return True
return False
def save_almanac(pigeon: TTYPigeon) -> None:
# store almanac in flash
pigeon.send(b"\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC")
try:
if pigeon.wait_for_ack(ack=UBLOX_SOS_ACK, nack=UBLOX_SOS_NACK):
cloudlog.info("Done storing almanac")
else:
cloudlog.error("Error storing almanac")
except TimeoutError:
pass
def init_baudrate(pigeon: TTYPigeon):
# ublox default setting on startup is 9600 baudrate
pigeon.set_baud(9600)
@@ -245,16 +256,6 @@ def deinitialize_and_exit(pigeon: TTYPigeon | None):
# controlled GNSS stop
pigeon.send(b"\xB5\x62\x06\x04\x04\x00\x00\x00\x08\x00\x16\x74")
# store almanac in flash
pigeon.send(b"\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC")
try:
if pigeon.wait_for_ack(ack=UBLOX_SOS_ACK, nack=UBLOX_SOS_NACK):
cloudlog.warning("Done storing almanac")
else:
cloudlog.error("Error storing almanac")
except TimeoutError:
pass
# turn off power and exit cleanly
set_power(False)
sys.exit(0)
@@ -281,6 +282,7 @@ def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0)
def end_condition():
return True if duration == 0 else time.monotonic() - start_time < duration
last_almanac_save = time.monotonic()
while end_condition():
dat = pigeon.receive()
if len(dat) > 0:
@@ -294,6 +296,11 @@ def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0)
msg = messaging.new_message('ubloxRaw', len(dat), valid=True)
msg.ubloxRaw = dat[:]
pm.send('ubloxRaw', msg)
# save almanac every 5 minutes
if (time.monotonic() - last_almanac_save) > 60*5:
save_almanac(pigeon)
last_almanac_save = time.monotonic()
else:
# prevent locking up a CPU core if ublox disconnects
time.sleep(0.001)
+110 -5
View File
@@ -3,21 +3,27 @@ import cffi
import os
import time
import pyray as rl
import threading
from collections.abc import Callable
from collections import deque
from dataclasses import dataclass
from enum import IntEnum
from typing import NamedTuple
from importlib.resources import as_file, files
from openpilot.common.swaglog import cloudlog
from openpilot.system.hardware import HARDWARE
from openpilot.system.hardware import HARDWARE, PC
from openpilot.common.realtime import Ratekeeper
DEFAULT_FPS = 60
DEFAULT_FPS = int(os.getenv("FPS", "60"))
FPS_LOG_INTERVAL = 5 # Seconds between logging FPS drops
FPS_DROP_THRESHOLD = 0.9 # FPS drop threshold for triggering a warning
FPS_CRITICAL_THRESHOLD = 0.5 # Critical threshold for triggering strict actions
MOUSE_THREAD_RATE = 140 # touch controller runs at 140Hz
ENABLE_VSYNC = os.getenv("ENABLE_VSYNC", "1") == "1"
SHOW_FPS = os.getenv("SHOW_FPS") == '1'
STRICT_MODE = os.getenv("STRICT_MODE") == '1'
ENABLE_VSYNC = os.getenv("ENABLE_VSYNC", "0") == "1"
SHOW_FPS = os.getenv("SHOW_FPS") == "1"
SHOW_TOUCHES = os.getenv("SHOW_TOUCHES") == "1"
STRICT_MODE = os.getenv("STRICT_MODE") == "1"
SCALE = float(os.getenv("SCALE", "1.0"))
DEFAULT_TEXT_SIZE = 60
@@ -45,6 +51,68 @@ class ModalOverlay:
callback: Callable | None = None
class MousePos(NamedTuple):
x: float
y: float
class MouseEvent(NamedTuple):
pos: MousePos
left_pressed: bool
left_released: bool
left_down: bool
t: float
class MouseState:
def __init__(self):
self._events: deque[MouseEvent] = deque(maxlen=MOUSE_THREAD_RATE) # bound event list
self._prev_mouse_event: MouseEvent | None = None
self._rk = Ratekeeper(MOUSE_THREAD_RATE)
self._lock = threading.Lock()
self._exit_event = threading.Event()
self._thread = None
def get_events(self) -> list[MouseEvent]:
with self._lock:
events = list(self._events)
self._events.clear()
return events
def start(self):
self._exit_event.clear()
if self._thread is None or not self._thread.is_alive():
self._thread = threading.Thread(target=self._run_thread, daemon=True)
self._thread.start()
def stop(self):
self._exit_event.set()
if self._thread is not None and self._thread.is_alive():
self._thread.join()
def _run_thread(self):
while not self._exit_event.is_set():
rl.poll_input_events()
self._handle_mouse_event()
self._rk.keep_time()
def _handle_mouse_event(self):
mouse_pos = rl.get_mouse_position()
ev = MouseEvent(
MousePos(mouse_pos.x, mouse_pos.y),
rl.is_mouse_button_pressed(rl.MouseButton.MOUSE_BUTTON_LEFT),
rl.is_mouse_button_released(rl.MouseButton.MOUSE_BUTTON_LEFT),
rl.is_mouse_button_down(rl.MouseButton.MOUSE_BUTTON_LEFT),
time.monotonic(),
)
# Only add changes
if self._prev_mouse_event is None or ev[:-1] != self._prev_mouse_event[:-1]:
with self._lock:
self._events.append(ev)
self._prev_mouse_event = ev
class GuiApplication:
def __init__(self, width: int, height: int):
self._fonts: dict[FontWeight, rl.Font] = {}
@@ -61,6 +129,12 @@ class GuiApplication:
self._trace_log_callback = None
self._modal_overlay = ModalOverlay()
self._mouse = MouseState()
self._mouse_events: list[MouseEvent] = []
# Debug variables
self._mouse_history: deque[MousePos] = deque(maxlen=MOUSE_THREAD_RATE)
def request_close(self):
self._window_close_requested = True
@@ -89,6 +163,9 @@ class GuiApplication:
self._set_styles()
self._load_fonts()
if not PC:
self._mouse.start()
def set_modal_overlay(self, overlay, callback: Callable | None = None):
self._modal_overlay = ModalOverlay(overlay=overlay, callback=callback)
@@ -149,11 +226,25 @@ class GuiApplication:
rl.unload_render_texture(self._render_texture)
self._render_texture = None
if not PC:
self._mouse.stop()
rl.close_window()
@property
def mouse_events(self) -> list[MouseEvent]:
return self._mouse_events
def render(self):
try:
while not (self._window_close_requested or rl.window_should_close()):
if PC:
# Thread is not used on PC, need to manually add mouse events
self._mouse._handle_mouse_event()
# Store all mouse events for the current frame
self._mouse_events = self._mouse.get_events()
if self._render_texture:
rl.begin_texture_mode(self._render_texture)
rl.clear_background(rl.BLACK)
@@ -190,6 +281,20 @@ class GuiApplication:
if SHOW_FPS:
rl.draw_fps(10, 10)
if SHOW_TOUCHES:
for mouse_event in self._mouse_events:
if mouse_event.left_pressed:
self._mouse_history.clear()
self._mouse_history.append(mouse_event.pos)
if self._mouse_history:
mouse_pos = self._mouse_history[-1]
rl.draw_circle(int(mouse_pos.x), int(mouse_pos.y), 15, rl.RED)
for idx, mouse_pos in enumerate(self._mouse_history):
perc = idx / len(self._mouse_history)
color = rl.Color(min(int(255 * (1.5 - perc)), 255), int(min(255 * (perc + 0.5), 255)), 50, 255)
rl.draw_circle(int(mouse_pos.x), int(mouse_pos.y), 5, color)
rl.end_drawing()
self._monitor_fps()
except KeyboardInterrupt:
+2 -2
View File
@@ -2,7 +2,7 @@ import os
import pyray as rl
from collections.abc import Callable
from abc import ABC
from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
from openpilot.system.ui.lib.text_measure import measure_text_cached
from openpilot.system.ui.lib.wrap_text import wrap_text
from openpilot.system.ui.lib.button import gui_button, ButtonStyle
@@ -229,7 +229,7 @@ class ListItem(Widget):
super().set_parent_rect(parent_rect)
self._rect.width = parent_rect.width
def _handle_mouse_release(self, mouse_pos: rl.Vector2):
def _handle_mouse_release(self, mouse_pos: MousePos):
if not self.is_visible:
return
+22 -19
View File
@@ -1,6 +1,8 @@
import time
import pyray as rl
from collections import deque
from enum import IntEnum
from openpilot.system.ui.lib.application import gui_app, MouseEvent, MousePos
# Scroll constants for smooth scrolling behavior
MOUSE_WHEEL_SCROLL_SPEED = 30
@@ -38,51 +40,54 @@ class GuiScrollPanel:
self._bounds_rect: rl.Rectangle | None = None
def handle_scroll(self, bounds: rl.Rectangle, content: rl.Rectangle) -> rl.Vector2:
# TODO: HACK: this class is driven by mouse events, so we need to ensure we have at least one event to process
for mouse_event in gui_app.mouse_events or [MouseEvent(MousePos(0, 0), False, False, False, time.monotonic())]:
self._handle_mouse_event(mouse_event, bounds, content)
return self._offset
def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, content: rl.Rectangle):
# Store rectangles for reference
self._content_rect = content
self._bounds_rect = bounds
# Calculate time delta
current_time = rl.get_time()
mouse_pos = rl.get_mouse_position()
max_scroll_y = max(content.height - bounds.height, 0)
# Start dragging on mouse press
if rl.check_collision_point_rec(mouse_pos, bounds) and rl.is_mouse_button_pressed(rl.MouseButton.MOUSE_BUTTON_LEFT):
if rl.check_collision_point_rec(mouse_event.pos, bounds) and mouse_event.left_pressed:
if self._scroll_state == ScrollState.IDLE or self._scroll_state == ScrollState.BOUNCING:
self._scroll_state = ScrollState.DRAGGING_CONTENT
if self._show_vertical_scroll_bar:
scrollbar_width = rl.gui_get_style(rl.GuiControl.LISTVIEW, rl.GuiListViewProperty.SCROLLBAR_WIDTH)
scrollbar_x = bounds.x + bounds.width - scrollbar_width
if mouse_pos.x >= scrollbar_x:
if mouse_event.pos.x >= scrollbar_x:
self._scroll_state = ScrollState.DRAGGING_SCROLLBAR
# TODO: hacky
# when clicking while moving, go straight into dragging
self._is_dragging = abs(self._velocity_y) > MIN_VELOCITY
self._last_mouse_y = mouse_pos.y
self._start_mouse_y = mouse_pos.y
self._last_drag_time = current_time
self._last_mouse_y = mouse_event.pos.y
self._start_mouse_y = mouse_event.pos.y
self._last_drag_time = mouse_event.t
self._velocity_history.clear()
self._velocity_y = 0.0
self._bounce_offset = 0.0
# Handle active dragging
if self._scroll_state == ScrollState.DRAGGING_CONTENT or self._scroll_state == ScrollState.DRAGGING_SCROLLBAR:
if rl.is_mouse_button_down(rl.MouseButton.MOUSE_BUTTON_LEFT):
delta_y = mouse_pos.y - self._last_mouse_y
if mouse_event.left_down:
delta_y = mouse_event.pos.y - self._last_mouse_y
# Track velocity for inertia
time_since_last_drag = current_time - self._last_drag_time
time_since_last_drag = mouse_event.t - self._last_drag_time
if time_since_last_drag > 0:
drag_velocity = delta_y / time_since_last_drag / 60.0
# TODO: HACK: /2 since we usually get two touch events per frame
drag_velocity = delta_y / time_since_last_drag / 60.0 / 2 # TODO: shouldn't be hardcoded
self._velocity_history.append(drag_velocity)
self._last_drag_time = current_time
self._last_drag_time = mouse_event.t
# Detect actual dragging
total_drag = abs(mouse_pos.y - self._start_mouse_y)
total_drag = abs(mouse_event.pos.y - self._start_mouse_y)
if total_drag > DRAG_THRESHOLD:
self._is_dragging = True
@@ -96,9 +101,9 @@ class GuiScrollPanel:
scroll_ratio = content.height / bounds.height
self._offset.y -= delta_y * scroll_ratio
self._last_mouse_y = mouse_pos.y
self._last_mouse_y = mouse_event.pos.y
elif rl.is_mouse_button_released(rl.MouseButton.MOUSE_BUTTON_LEFT):
elif mouse_event.left_released:
# Calculate flick velocity
if self._velocity_history:
total_weight = 0
@@ -167,8 +172,6 @@ class GuiScrollPanel:
elif self._offset.y < -(max_scroll_y + MAX_BOUNCE_DISTANCE):
self._offset.y = -(max_scroll_y + MAX_BOUNCE_DISTANCE)
return self._offset
def is_touch_valid(self):
return not self._is_dragging
+2 -1
View File
@@ -1,4 +1,5 @@
import pyray as rl
from openpilot.system.ui.lib.application import MousePos
from openpilot.system.ui.lib.widget import Widget
ON_COLOR = rl.Color(51, 171, 76, 255)
@@ -23,7 +24,7 @@ class Toggle(Widget):
def set_rect(self, rect: rl.Rectangle):
self._rect = rl.Rectangle(rect.x, rect.y, WIDTH, HEIGHT)
def _handle_mouse_release(self, mouse_pos: rl.Vector2):
def _handle_mouse_release(self, mouse_pos: MousePos):
if not self._enabled:
return
+12 -11
View File
@@ -2,6 +2,7 @@ import abc
import pyray as rl
from enum import IntEnum
from collections.abc import Callable
from openpilot.system.ui.lib.application import gui_app, MousePos
class DialogResult(IntEnum):
@@ -66,18 +67,18 @@ class Widget(abc.ABC):
ret = self._render(self._rect)
# Keep track of whether mouse down started within the widget's rectangle
mouse_pos = rl.get_mouse_position()
if rl.is_mouse_button_pressed(rl.MouseButton.MOUSE_BUTTON_LEFT) and self._touch_valid():
if rl.check_collision_point_rec(mouse_pos, self._rect):
self._is_pressed = True
for mouse_event in gui_app.mouse_events:
if mouse_event.left_pressed and self._touch_valid():
if rl.check_collision_point_rec(mouse_event.pos, self._rect):
self._is_pressed = True
elif not self._touch_valid():
self._is_pressed = False
elif not self._touch_valid():
self._is_pressed = False
elif rl.is_mouse_button_released(rl.MouseButton.MOUSE_BUTTON_LEFT):
if self._is_pressed and rl.check_collision_point_rec(mouse_pos, self._rect):
self._handle_mouse_release(mouse_pos)
self._is_pressed = False
elif mouse_event.left_released:
if self._is_pressed and rl.check_collision_point_rec(mouse_event.pos, self._rect):
self._handle_mouse_release(mouse_event.pos)
self._is_pressed = False
return ret
@@ -91,6 +92,6 @@ class Widget(abc.ABC):
def _update_layout_rects(self) -> None:
"""Optionally update any layout rects on Widget rect change."""
def _handle_mouse_release(self, mouse_pos: rl.Vector2) -> bool:
def _handle_mouse_release(self, mouse_pos: MousePos) -> bool:
"""Optionally handle mouse release events."""
return False
+5 -2
View File
@@ -204,7 +204,7 @@ class WifiManager:
'connection': {
'type': Variant('s', '802-11-wireless'),
'uuid': Variant('s', str(uuid.uuid4())),
'id': Variant('s', ssid),
'id': Variant('s', f'openpilot connection {ssid}'),
'autoconnect-retries': Variant('i', 0),
},
'802-11-wireless': {
@@ -212,7 +212,10 @@ class WifiManager:
'hidden': Variant('b', is_hidden),
'mode': Variant('s', 'infrastructure'),
},
'ipv4': {'method': Variant('s', 'auto')},
'ipv4': {
'method': Variant('s', 'auto'),
'dns-priority': Variant('i', 600),
},
'ipv6': {'method': Variant('s', 'ignore')},
}