brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compile FrogPilot

Browse Source
This commit is contained in:
FrogAi
2024-03-10 20:59:55 -07:00
parent 69e7feaf01
commit f1acd339d7
1485 changed files with 426154 additions and 398339 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
system/loggerd/SConscript
View File

@@ -1,27 +0,0 @@
Import('env', 'arch', 'cereal', 'messaging', 'common', 'visionipc')
libs = [common, cereal, messaging, visionipc,
'zmq', 'capnp', 'kj', 'z',
'avformat', 'avcodec', 'swscale', 'avutil',
'yuv', 'OpenCL', 'pthread']
src = ['logger.cc', 'video_writer.cc', 'encoder/encoder.cc', 'encoder/v4l_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)
env.Program('bootlog.cc', LIBS=libs)
if GetOption('extras'):
env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_logger.cc'], LIBS=libs + ['curl', 'crypto'])

BIN
system/loggerd/bootlog Executable file
View File

Binary file not shown.

70
system/loggerd/bootlog.cc
View File

@@ -1,70 +0,0 @@
#include <cassert>
#include <string>
#include "cereal/messaging/messaging.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "system/loggerd/logger.h"
static kj::Array<capnp::word> build_boot_log() {
MessageBuilder msg;
auto boot = msg.initEvent().initBoot();
boot.setWallTimeNanos(nanos_since_epoch());
std::string pstore = "/sys/fs/pstore";
std::map<std::string, std::string> pstore_map = util::read_files_in_dir(pstore);
int i = 0;
auto lpstore = boot.initPstore().initEntries(pstore_map.size());
for (auto& kv : pstore_map) {
auto lentry = lpstore[i];
lentry.setKey(kv.first);
lentry.setValue(capnp::Data::Reader((const kj::byte*)kv.second.data(), kv.second.size()));
i++;
}
// Gather output of commands
std::vector<std::string> bootlog_commands = {
"[ -x \"$(command -v journalctl)\" ] && journalctl",
};
if (Hardware::TICI()) {
bootlog_commands.push_back("[ -e /dev/nvme0 ] && sudo nvme smart-log --output-format=json /dev/nvme0");
}
auto commands = boot.initCommands().initEntries(bootlog_commands.size());
for (int j = 0; j < bootlog_commands.size(); j++) {
auto lentry = commands[j];
lentry.setKey(bootlog_commands[j]);
const std::string result = util::check_output(bootlog_commands[j]);
lentry.setValue(capnp::Data::Reader((const kj::byte*)result.data(), result.size()));
}
boot.setLaunchLog(util::read_file("/tmp/launch_log"));
return capnp::messageToFlatArray(msg);
}
int main(int argc, char** argv) {
const std::string id = logger_get_identifier("BootCount");
const std::string path = Path::log_root() + "/boot/" + id;
LOGW("bootlog to %s", path.c_str());
// Open bootlog
bool r = util::create_directories(Path::log_root() + "/boot/", 0775);
assert(r);
RawFile file(path.c_str());
// Write initdata
file.write(logger_build_init_data().asBytes());
// Write bootlog
file.write(build_boot_log().asBytes());
// Write out bootlog param to match routes with bootlog
Params().put("CurrentBootlog", id.c_str());
return 0;
}

43
system/loggerd/encoder/encoder.cc
View File

@@ -1,43 +0,0 @@
#include "system/loggerd/encoder/encoder.h"
VideoEncoder::VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in_height)
: encoder_info(encoder_info), in_width(in_width), in_height(in_height) {
out_width = encoder_info.frame_width > 0 ? encoder_info.frame_width : in_width;
out_height = encoder_info.frame_height > 0 ? encoder_info.frame_height : in_height;
pm.reset(new PubMaster({encoder_info.publish_name}));
}
void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra,
unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat) {
// broadcast packet
MessageBuilder msg;
auto event = msg.initEvent(true);
auto edat = (event.*(e->encoder_info.init_encode_data_func))();
auto edata = edat.initIdx();
struct timespec ts;
timespec_get(&ts, TIME_UTC);
edat.setUnixTimestampNanos((uint64_t)ts.tv_sec*1000000000 + ts.tv_nsec);
edata.setFrameId(extra.frame_id);
edata.setTimestampSof(extra.timestamp_sof);
edata.setTimestampEof(extra.timestamp_eof);
edata.setType(e->encoder_info.encode_type);
edata.setEncodeId(e->cnt++);
edata.setSegmentNum(segment_num);
edata.setSegmentId(idx);
edata.setFlags(flags);
edata.setLen(dat.size());
edat.setData(dat);
edat.setWidth(out_width);
edat.setHeight(out_height);
if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header);
uint32_t bytes_size = capnp::computeSerializedSizeInWords(msg) * sizeof(capnp::word);
if (e->msg_cache.size() < bytes_size) {
e->msg_cache.resize(bytes_size);
}
kj::ArrayOutputStream output_stream(kj::ArrayPtr<capnp::byte>(e->msg_cache.data(), bytes_size));
capnp::writeMessage(output_stream, msg);
e->pm->send(e->encoder_info.publish_name, e->msg_cache.data(), bytes_size);
}

37
system/loggerd/encoder/encoder.h
View File

@@ -1,37 +0,0 @@
#pragma once
#include <cassert>
#include <cstdint>
#include <memory>
#include <thread>
#include <vector>
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/visionipc.h"
#include "common/queue.h"
#include "system/camerad/cameras/camera_common.h"
#include "system/loggerd/loggerd.h"
#define V4L2_BUF_FLAG_KEYFRAME 8
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(const char* path) = 0;
virtual void encoder_close() = 0;
void publisher_publish(VideoEncoder *e, 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;
};

150
system/loggerd/encoder/ffmpeg_encoder.cc
View File

@@ -1,150 +0,0 @@
#pragma clang diagnostic ignored "-Wdeprecated-declarations"
#include "system/loggerd/encoder/ffmpeg_encoder.h"
#include <fcntl.h>
#include <unistd.h>
#include <cassert>
#include <cstdio>
#include <cstdlib>
#define __STDC_CONSTANT_MACROS
#include "third_party/libyuv/include/libyuv.h"
extern "C" {
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libavutil/imgutils.h>
}
#include "common/swaglog.h"
#include "common/util.h"
const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0;
FfmpegEncoder::FfmpegEncoder(const EncoderInfo &encoder_info, int in_width, int in_height)
: VideoEncoder(encoder_info, in_width, in_height) {
frame = av_frame_alloc();
assert(frame);
frame->format = AV_PIX_FMT_YUV420P;
frame->width = out_width;
frame->height = out_height;
frame->linesize[0] = out_width;
frame->linesize[1] = out_width/2;
frame->linesize[2] = out_width/2;
convert_buf.resize(in_width * in_height * 3 / 2);
if (in_width != out_width || in_height != out_height) {
downscale_buf.resize(out_width * out_height * 3 / 2);
}
}
FfmpegEncoder::~FfmpegEncoder() {
encoder_close();
av_frame_free(&frame);
}
void FfmpegEncoder::encoder_open(const char* path) {
const AVCodec *codec = avcodec_find_encoder(AV_CODEC_ID_FFVHUFF);
this->codec_ctx = avcodec_alloc_context3(codec);
assert(this->codec_ctx);
this->codec_ctx->width = frame->width;
this->codec_ctx->height = frame->height;
this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
this->codec_ctx->time_base = (AVRational){ 1, encoder_info.fps };
int err = avcodec_open2(this->codec_ctx, codec, NULL);
assert(err >= 0);
is_open = true;
segment_num++;
counter = 0;
}
void FfmpegEncoder::encoder_close() {
if (!is_open) return;
avcodec_free_context(&codec_ctx);
is_open = false;
}
int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) {
assert(buf->width == this->in_width);
assert(buf->height == this->in_height);
uint8_t *cy = convert_buf.data();
uint8_t *cu = cy + in_width * in_height;
uint8_t *cv = cu + (in_width / 2) * (in_height / 2);
libyuv::NV12ToI420(buf->y, buf->stride,
buf->uv, buf->stride,
cy, in_width,
cu, in_width/2,
cv, in_width/2,
in_width, in_height);
if (downscale_buf.size() > 0) {
uint8_t *out_y = downscale_buf.data();
uint8_t *out_u = out_y + frame->width * frame->height;
uint8_t *out_v = out_u + (frame->width / 2) * (frame->height / 2);
libyuv::I420Scale(cy, in_width,
cu, in_width/2,
cv, in_width/2,
in_width, in_height,
out_y, frame->width,
out_u, frame->width/2,
out_v, frame->width/2,
frame->width, frame->height,
libyuv::kFilterNone);
frame->data[0] = out_y;
frame->data[1] = out_u;
frame->data[2] = out_v;
} else {
frame->data[0] = cy;
frame->data[1] = cu;
frame->data[2] = cv;
}
frame->pts = counter*50*1000; // 50ms per frame
int ret = counter;
int err = avcodec_send_frame(this->codec_ctx, frame);
if (err < 0) {
LOGE("avcodec_send_frame error %d", err);
ret = -1;
}
AVPacket pkt;
av_init_packet(&pkt);
pkt.data = NULL;
pkt.size = 0;
while (ret >= 0) {
err = avcodec_receive_packet(this->codec_ctx, &pkt);
if (err == AVERROR_EOF) {
break;
} else if (err == AVERROR(EAGAIN)) {
// Encoder might need a few frames on startup to get started. Keep going
ret = 0;
break;
} else if (err < 0) {
LOGE("avcodec_receive_packet error %d", err);
ret = -1;
break;
}
if (env_debug_encoder) {
printf("%20s got %8d bytes flags %8x idx %4d id %8d\n", encoder_info.publish_name, pkt.size, pkt.flags, counter, extra->frame_id);
}
publisher_publish(this, segment_num, counter, *extra,
(pkt.flags & AV_PKT_FLAG_KEY) ? V4L2_BUF_FLAG_KEYFRAME : 0,
kj::arrayPtr<capnp::byte>(pkt.data, (size_t)0), // TODO: get the header
kj::arrayPtr<capnp::byte>(pkt.data, pkt.size));
counter++;
}
av_packet_unref(&pkt);
return ret;
}

34
system/loggerd/encoder/ffmpeg_encoder.h
View File

@@ -1,34 +0,0 @@
#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(const char* path);
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;
};

326
system/loggerd/encoder/v4l_encoder.cc
View File

@@ -1,326 +0,0 @@
#include <cassert>
#include <string>
#include <sys/ioctl.h>
#include <poll.h>
#include "system/loggerd/encoder/v4l_encoder.h"
#include "common/util.h"
#include "common/timing.h"
#include "third_party/libyuv/include/libyuv.h"
#include "third_party/linux/include/msm_media_info.h"
// has to be in this order
#include "third_party/linux/include/v4l2-controls.h"
#include <linux/videodev2.h>
#define V4L2_QCOM_BUF_FLAG_CODECCONFIG 0x00020000
#define V4L2_QCOM_BUF_FLAG_EOS 0x02000000
// echo 0x7fffffff > /sys/kernel/debug/msm_vidc/debug_level
const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0;
static void checked_ioctl(int fd, unsigned long request, void *argp) {
int ret = util::safe_ioctl(fd, request, argp);
if (ret != 0) {
LOGE("checked_ioctl failed with error %d (%d %lx %p)", errno, fd, request, argp);
assert(0);
}
}
static void dequeue_buffer(int fd, v4l2_buf_type buf_type, unsigned int *index=NULL, unsigned int *bytesused=NULL, unsigned int *flags=NULL, struct timeval *timestamp=NULL) {
v4l2_plane plane = {0};
v4l2_buffer v4l_buf = {
.type = buf_type,
.memory = V4L2_MEMORY_USERPTR,
.m = { .planes = &plane, },
.length = 1,
};
checked_ioctl(fd, VIDIOC_DQBUF, &v4l_buf);
if (index) *index = v4l_buf.index;
if (bytesused) *bytesused = v4l_buf.m.planes[0].bytesused;
if (flags) *flags = v4l_buf.flags;
if (timestamp) *timestamp = v4l_buf.timestamp;
assert(v4l_buf.m.planes[0].data_offset == 0);
}
static void queue_buffer(int fd, v4l2_buf_type buf_type, unsigned int index, VisionBuf *buf, struct timeval timestamp={}) {
v4l2_plane plane = {
.length = (unsigned int)buf->len,
.m = { .userptr = (unsigned long)buf->addr, },
.bytesused = (uint32_t)buf->len,
.reserved = {(unsigned int)buf->fd}
};
v4l2_buffer v4l_buf = {
.type = buf_type,
.index = index,
.memory = V4L2_MEMORY_USERPTR,
.m = { .planes = &plane, },
.length = 1,
.flags = V4L2_BUF_FLAG_TIMESTAMP_COPY,
.timestamp = timestamp
};
checked_ioctl(fd, VIDIOC_QBUF, &v4l_buf);
}
static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count) {
struct v4l2_requestbuffers reqbuf = {
.type = buf_type,
.memory = V4L2_MEMORY_USERPTR,
.count = count
};
checked_ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
}
void V4LEncoder::dequeue_handler(V4LEncoder *e) {
std::string dequeue_thread_name = "dq-"+std::string(e->encoder_info.publish_name);
util::set_thread_name(dequeue_thread_name.c_str());
e->segment_num++;
uint32_t idx = -1;
bool exit = false;
// POLLIN is capture, POLLOUT is frame
struct pollfd pfd;
pfd.events = POLLIN | POLLOUT;
pfd.fd = e->fd;
// save the header
kj::Array<capnp::byte> header;
while (!exit) {
int rc = poll(&pfd, 1, 1000);
if (rc < 0) {
if (errno != EINTR) {
// TODO: exit encoder?
// ignore the error and keep going
LOGE("poll failed (%d - %d)", rc, errno);
}
continue;
} else if (rc == 0) {
LOGE("encoder dequeue poll timeout");
continue;
}
if (env_debug_encoder >= 2) {
printf("%20s poll %x at %.2f ms\n", e->encoder_info.publish_name, pfd.revents, millis_since_boot());
}
int frame_id = -1;
if (pfd.revents & POLLIN) {
unsigned int bytesused, flags, index;
struct timeval timestamp;
dequeue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, &index, &bytesused, &flags, &timestamp);
e->buf_out[index].sync(VISIONBUF_SYNC_FROM_DEVICE);
uint8_t *buf = (uint8_t*)e->buf_out[index].addr;
int64_t ts = timestamp.tv_sec * 1000000 + timestamp.tv_usec;
// eof packet, we exit
if (flags & V4L2_QCOM_BUF_FLAG_EOS) {
exit = true;
} else if (flags & V4L2_QCOM_BUF_FLAG_CODECCONFIG) {
// save header
header = kj::heapArray<capnp::byte>(buf, bytesused);
} else {
VisionIpcBufExtra extra = e->extras.pop();
assert(extra.timestamp_eof/1000 == ts); // stay in sync
frame_id = extra.frame_id;
++idx;
e->publisher_publish(e, e->segment_num, idx, extra, flags, header, kj::arrayPtr<capnp::byte>(buf, bytesused));
}
if (env_debug_encoder) {
printf("%20s got(%d) %6d bytes flags %8x idx %3d/%4d id %8d ts %ld lat %.2f ms (%lu frames free)\n",
e->encoder_info.publish_name, index, bytesused, flags, e->segment_num, idx, frame_id, ts, millis_since_boot()-(ts/1000.), e->free_buf_in.size());
}
// requeue the buffer
queue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, index, &e->buf_out[index]);
}
if (pfd.revents & POLLOUT) {
unsigned int index;
dequeue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, &index);
e->free_buf_in.push(index);
}
}
}
V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_height)
: VideoEncoder(encoder_info, in_width, in_height) {
fd = open("/dev/v4l/by-path/platform-aa00000.qcom_vidc-video-index1", O_RDWR|O_NONBLOCK);
assert(fd >= 0);
struct v4l2_capability cap;
checked_ioctl(fd, VIDIOC_QUERYCAP, &cap);
LOGD("opened encoder device %s %s = %d", cap.driver, cap.card, fd);
assert(strcmp((const char *)cap.driver, "msm_vidc_driver") == 0);
assert(strcmp((const char *)cap.card, "msm_vidc_venc") == 0);
struct v4l2_format fmt_out = {
.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
.fmt = {
.pix_mp = {
// downscales are free with v4l
.width = (unsigned int)(out_width),
.height = (unsigned int)(out_height),
.pixelformat = (encoder_info.encode_type == cereal::EncodeIndex::Type::FULL_H_E_V_C) ? V4L2_PIX_FMT_HEVC : V4L2_PIX_FMT_H264,
.field = V4L2_FIELD_ANY,
.colorspace = V4L2_COLORSPACE_DEFAULT,
}
}
};
checked_ioctl(fd, VIDIOC_S_FMT, &fmt_out);
v4l2_streamparm streamparm = {
.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
.parm = {
.output = {
// TODO: more stuff here? we don't know
.timeperframe = {
.numerator = 1,
.denominator = 20
}
}
}
};
checked_ioctl(fd, VIDIOC_S_PARM, &streamparm);
struct v4l2_format fmt_in = {
.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
.fmt = {
.pix_mp = {
.width = (unsigned int)in_width,
.height = (unsigned int)in_height,
.pixelformat = V4L2_PIX_FMT_NV12,
.field = V4L2_FIELD_ANY,
.colorspace = V4L2_COLORSPACE_470_SYSTEM_BG,
}
}
};
checked_ioctl(fd, VIDIOC_S_FMT, &fmt_in);
LOGD("in buffer size %d, out buffer size %d",
fmt_in.fmt.pix_mp.plane_fmt[0].sizeimage,
fmt_out.fmt.pix_mp.plane_fmt[0].sizeimage);
// shared ctrls
{
struct v4l2_control ctrls[] = {
{ .id = V4L2_CID_MPEG_VIDEO_HEADER_MODE, .value = V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE},
{ .id = V4L2_CID_MPEG_VIDEO_BITRATE, .value = encoder_info.bitrate},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL, .value = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_VBR_CFR},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_PRIORITY, .value = V4L2_MPEG_VIDC_VIDEO_PRIORITY_REALTIME_DISABLE},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_IDR_PERIOD, .value = 1},
};
for (auto ctrl : ctrls) {
checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl);
}
}
if (encoder_info.encode_type == cereal::EncodeIndex::Type::FULL_H_E_V_C) {
struct v4l2_control ctrls[] = {
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_PROFILE, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_PROFILE_MAIN},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_TIER_LEVEL, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_LEVEL_HIGH_TIER_LEVEL_5},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_VUI_TIMING_INFO, .value = V4L2_MPEG_VIDC_VIDEO_VUI_TIMING_INFO_ENABLED},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES, .value = 29},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES, .value = 0},
};
for (auto ctrl : ctrls) {
checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl);
}
} else {
struct v4l2_control ctrls[] = {
{ .id = V4L2_CID_MPEG_VIDEO_H264_PROFILE, .value = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH},
{ .id = V4L2_CID_MPEG_VIDEO_H264_LEVEL, .value = V4L2_MPEG_VIDEO_H264_LEVEL_UNKNOWN},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES, .value = 14},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES, .value = 0},
{ .id = V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE, .value = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL, .value = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL_0},
{ .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE, .value = 0},
{ .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA, .value = 0},
{ .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA, .value = 0},
{ .id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE, .value = 0},
};
for (auto ctrl : ctrls) {
checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl);
}
}
// allocate buffers
request_buffers(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, BUF_OUT_COUNT);
request_buffers(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, BUF_IN_COUNT);
// start encoder
v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
checked_ioctl(fd, VIDIOC_STREAMON, &buf_type);
buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
checked_ioctl(fd, VIDIOC_STREAMON, &buf_type);
// queue up output buffers
for (unsigned int i = 0; i < BUF_OUT_COUNT; i++) {
buf_out[i].allocate(fmt_out.fmt.pix_mp.plane_fmt[0].sizeimage);
queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, i, &buf_out[i]);
}
// queue up input buffers
for (unsigned int i = 0; i < BUF_IN_COUNT; i++) {
free_buf_in.push(i);
}
}
void V4LEncoder::encoder_open(const char* path) {
dequeue_handler_thread = std::thread(V4LEncoder::dequeue_handler, this);
this->is_open = true;
this->counter = 0;
}
int V4LEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) {
struct timeval timestamp {
.tv_sec = (long)(extra->timestamp_eof/1000000000),
.tv_usec = (long)((extra->timestamp_eof/1000) % 1000000),
};
// reserve buffer
int buffer_in = free_buf_in.pop();
// push buffer
extras.push(*extra);
//buf->sync(VISIONBUF_SYNC_TO_DEVICE);
queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, buffer_in, buf, timestamp);
return this->counter++;
}
void V4LEncoder::encoder_close() {
if (this->is_open) {
// pop all the frames before closing, then put the buffers back
for (int i = 0; i < BUF_IN_COUNT; i++) free_buf_in.pop();
for (int i = 0; i < BUF_IN_COUNT; i++) free_buf_in.push(i);
// no frames, stop the encoder
struct v4l2_encoder_cmd encoder_cmd = { .cmd = V4L2_ENC_CMD_STOP };
checked_ioctl(fd, VIDIOC_ENCODER_CMD, &encoder_cmd);
// join waits for V4L2_QCOM_BUF_FLAG_EOS
dequeue_handler_thread.join();
assert(extras.empty());
}
this->is_open = false;
}
V4LEncoder::~V4LEncoder() {
encoder_close();
v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
checked_ioctl(fd, VIDIOC_STREAMOFF, &buf_type);
request_buffers(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, 0);
buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
checked_ioctl(fd, VIDIOC_STREAMOFF, &buf_type);
request_buffers(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 0);
close(fd);
for (int i = 0; i < BUF_OUT_COUNT; i++) {
if (buf_out[i].free() != 0) {
LOGE("Failed to free buffer");
}
}
}

30
system/loggerd/encoder/v4l_encoder.h
View File

@@ -1,30 +0,0 @@
#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(const char* path);
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;
};

BIN
system/loggerd/encoderd Executable file
View File

Binary file not shown.

161
system/loggerd/encoderd.cc
View File

@@ -1,161 +0,0 @@
#include <cassert>
#include "system/loggerd/loggerd.h"
#ifdef QCOM2
#include "system/loggerd/encoder/v4l_encoder.h"
#define Encoder V4LEncoder
#else
#include "system/loggerd/encoder/ffmpeg_encoder.h"
#define Encoder FfmpegEncoder
#endif
ExitHandler do_exit;
struct EncoderdState {
int max_waiting = 0;
// Sync logic for startup
std::atomic<int> encoders_ready = 0;
std::atomic<uint32_t> start_frame_id = 0;
bool camera_ready[WideRoadCam + 1] = {};
bool camera_synced[WideRoadCam + 1] = {};
};
// Handle initial encoder syncing by waiting for all encoders to reach the same frame id
bool sync_encoders(EncoderdState *s, CameraType cam_type, uint32_t frame_id) {
if (s->camera_synced[cam_type]) return true;
if (s->max_waiting > 1 && s->encoders_ready != s->max_waiting) {
// add a small margin to the start frame id in case one of the encoders already dropped the next frame
update_max_atomic(s->start_frame_id, frame_id + 2);
if (std::exchange(s->camera_ready[cam_type], true) == false) {
++s->encoders_ready;
LOGD("camera %d encoder ready", cam_type);
}
return false;
} else {
if (s->max_waiting == 1) update_max_atomic(s->start_frame_id, frame_id);
bool synced = frame_id >= s->start_frame_id;
s->camera_synced[cam_type] = synced;
if (!synced) LOGD("camera %d waiting for frame %d, cur %d", cam_type, (int)s->start_frame_id, frame_id);
return synced;
}
}
void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) {
util::set_thread_name(cam_info.thread_name);
std::vector<std::unique_ptr<Encoder>> encoders;
VisionIpcClient vipc_client = VisionIpcClient("camerad", cam_info.stream_type, false);
int cur_seg = 0;
while (!do_exit) {
if (!vipc_client.connect(false)) {
util::sleep_for(5);
continue;
}
// init encoders
if (encoders.empty()) {
VisionBuf buf_info = vipc_client.buffers[0];
LOGW("encoder %s init %zux%zu", cam_info.thread_name, buf_info.width, buf_info.height);
assert(buf_info.width > 0 && buf_info.height > 0);
for (const auto &encoder_info : cam_info.encoder_infos) {
auto &e = encoders.emplace_back(new Encoder(encoder_info, buf_info.width, buf_info.height));
e->encoder_open(nullptr);
}
}
bool lagging = false;
while (!do_exit) {
VisionIpcBufExtra extra;
VisionBuf* buf = vipc_client.recv(&extra);
if (buf == nullptr) continue;
// detect loop around and drop the frames
if (buf->get_frame_id() != extra.frame_id) {
if (!lagging) {
LOGE("encoder %s lag buffer id: %" PRIu64 " extra id: %d", cam_info.thread_name, buf->get_frame_id(), extra.frame_id);
lagging = true;
}
continue;
}
lagging = false;
if (!sync_encoders(s, cam_info.type, extra.frame_id)) {
continue;
}
if (do_exit) break;
// do rotation if required
const int frames_per_seg = SEGMENT_LENGTH * MAIN_FPS;
if (cur_seg >= 0 && extra.frame_id >= ((cur_seg + 1) * frames_per_seg) + s->start_frame_id) {
for (auto &e : encoders) {
e->encoder_close();
e->encoder_open(NULL);
}
++cur_seg;
}
// encode a frame
for (int i = 0; i < encoders.size(); ++i) {
int out_id = encoders[i]->encode_frame(buf, &extra);
if (out_id == -1) {
LOGE("Failed to encode frame. frame_id: %d", extra.frame_id);
}
}
}
}
}
template <size_t N>
void encoderd_thread(const LogCameraInfo (&cameras)[N]) {
EncoderdState s;
std::set<VisionStreamType> streams;
while (!do_exit) {
streams = VisionIpcClient::getAvailableStreams("camerad", false);
if (!streams.empty()) {
break;
}
util::sleep_for(100);
}
if (!streams.empty()) {
std::vector<std::thread> encoder_threads;
for (auto stream : streams) {
auto it = std::find_if(std::begin(cameras), std::end(cameras),
[stream](auto &cam) { return cam.stream_type == stream; });
assert(it != std::end(cameras));
++s.max_waiting;
encoder_threads.push_back(std::thread(encoder_thread, &s, *it));
}
for (auto &t : encoder_threads) t.join();
}
}
int main(int argc, char* argv[]) {
if (!Hardware::PC()) {
int ret;
ret = util::set_realtime_priority(52);
assert(ret == 0);
ret = util::set_core_affinity({3});
assert(ret == 0);
}
if (argc > 1) {
std::string arg1(argv[1]);
if (arg1 == "--stream") {
encoderd_thread(stream_cameras_logged);
} else {
LOGE("Argument '%s' is not supported", arg1.c_str());
}
} else {
encoderd_thread(cameras_logged);
}
return 0;
}

172
system/loggerd/logger.cc
View File

@@ -1,172 +0,0 @@
#include "system/loggerd/logger.h"
#include <fstream>
#include <map>
#include <vector>
#include <iostream>
#include <sstream>
#include <random>
#include "common/params.h"
#include "common/swaglog.h"
#include "common/version.h"
// ***** log metadata *****
kj::Array<capnp::word> logger_build_init_data() {
uint64_t wall_time = nanos_since_epoch();
MessageBuilder msg;
auto init = msg.initEvent().initInitData();
init.setWallTimeNanos(wall_time);
init.setVersion(COMMA_VERSION);
init.setDirty(!getenv("CLEAN"));
init.setDeviceType(Hardware::get_device_type());
// log kernel args
std::ifstream cmdline_stream("/proc/cmdline");
std::vector<std::string> kernel_args;
std::string buf;
while (cmdline_stream >> buf) {
kernel_args.push_back(buf);
}
auto lkernel_args = init.initKernelArgs(kernel_args.size());
for (int i=0; i<kernel_args.size(); i++) {
lkernel_args.set(i, kernel_args[i]);
}
init.setKernelVersion(util::read_file("/proc/version"));
init.setOsVersion(util::read_file("/VERSION"));
// log params
auto params = Params(util::getenv("PARAMS_COPY_PATH", ""));
std::map<std::string, std::string> params_map = params.readAll();
init.setGitCommit(params_map["GitCommit"]);
init.setGitCommitDate(params_map["GitCommitDate"]);
init.setGitBranch(params_map["GitBranch"]);
init.setGitRemote(params_map["GitRemote"]);
init.setPassive(false);
init.setDongleId(params_map["DongleId"]);
auto lparams = init.initParams().initEntries(params_map.size());
int j = 0;
for (auto& [key, value] : params_map) {
auto lentry = lparams[j];
lentry.setKey(key);
if ( !(params.getKeyType(key) & DONT_LOG) ) {
lentry.setValue(capnp::Data::Reader((const kj::byte*)value.data(), value.size()));
}
j++;
}
// log commands
std::vector<std::string> log_commands = {
"df -h", // usage for all filesystems
};
auto hw_logs = Hardware::get_init_logs();
auto commands = init.initCommands().initEntries(log_commands.size() + hw_logs.size());
for (int i = 0; i < log_commands.size(); i++) {
auto lentry = commands[i];
lentry.setKey(log_commands[i]);
const std::string result = util::check_output(log_commands[i]);
lentry.setValue(capnp::Data::Reader((const kj::byte*)result.data(), result.size()));
}
int i = log_commands.size();
for (auto &[key, value] : hw_logs) {
auto lentry = commands[i];
lentry.setKey(key);
lentry.setValue(capnp::Data::Reader((const kj::byte*)value.data(), value.size()));
i++;
}
return capnp::messageToFlatArray(msg);
}
std::string logger_get_route_name() {
char route_name[64] = {'\0'};
time_t rawtime = time(NULL);
struct tm timeinfo;
localtime_r(&rawtime, &timeinfo);
strftime(route_name, sizeof(route_name), "%Y-%m-%d--%H-%M-%S", &timeinfo);
return route_name;
}
std::string logger_get_identifier(std::string key) {
// a log identifier is a 32 bit counter, plus a 10 character unique ID.
// e.g. 000001a3--c20ba54385
Params params;
uint32_t cnt;
try {
cnt = std::stol(params.get(key));
} catch (std::exception &e) {
cnt = 0;
}
params.put(key, std::to_string(cnt + 1));
std::stringstream ss;
std::random_device rd;
std::mt19937 mt(rd());
std::uniform_int_distribution<int> dist(0, 15);
for (int i = 0; i < 10; ++i) {
ss << std::hex << dist(mt);
}
return util::string_format("%08x--%s", cnt, ss.str().c_str());
}
static void log_sentinel(LoggerState *log, SentinelType type, int eixt_signal = 0) {
MessageBuilder msg;
auto sen = msg.initEvent().initSentinel();
sen.setType(type);
sen.setSignal(eixt_signal);
log->write(msg.toBytes(), true);
}
LoggerState::LoggerState(const std::string &log_root) {
route_name = logger_get_route_name();
route_path = log_root + "/" + route_name;
init_data = logger_build_init_data();
}
LoggerState::~LoggerState() {
if (rlog) {
log_sentinel(this, SentinelType::END_OF_ROUTE, exit_signal);
std::remove(lock_file.c_str());
}
}
bool LoggerState::next() {
if (rlog) {
log_sentinel(this, SentinelType::END_OF_SEGMENT);
std::remove(lock_file.c_str());
}
segment_path = route_path + "--" + std::to_string(++part);
bool ret = util::create_directories(segment_path, 0775);
assert(ret == true);
const std::string rlog_path = segment_path + "/rlog";
lock_file = rlog_path + ".lock";
std::ofstream{lock_file};
rlog.reset(new RawFile(rlog_path));
qlog.reset(new RawFile(segment_path + "/qlog"));
// log init data & sentinel type.
write(init_data.asBytes(), true);
log_sentinel(this, part > 0 ? SentinelType::START_OF_SEGMENT : SentinelType::START_OF_ROUTE);
return true;
}
void LoggerState::write(uint8_t* data, size_t size, bool in_qlog) {
rlog->write(data, size);
if (in_qlog) qlog->write(data, size);
}

56
system/loggerd/logger.h
View File

@@ -1,56 +0,0 @@
#pragma once
#include <cassert>
#include <memory>
#include <string>
#include "cereal/messaging/messaging.h"
#include "common/util.h"
#include "system/hardware/hw.h"
class RawFile {
public:
RawFile(const std::string &path) {
file = util::safe_fopen(path.c_str(), "wb");
assert(file != nullptr);
}
~RawFile() {
util::safe_fflush(file);
int err = fclose(file);
assert(err == 0);
}
inline void write(void* data, size_t size) {
int written = util::safe_fwrite(data, 1, size, file);
assert(written == size);
}
inline void write(kj::ArrayPtr<capnp::byte> array) { write(array.begin(), array.size()); }
private:
FILE* file = nullptr;
};
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<RawFile> rlog, qlog;
};
kj::Array<capnp::word> logger_build_init_data();
std::string logger_get_route_name();
std::string logger_get_identifier(std::string key);

BIN
system/loggerd/loggerd Executable file
View File

Binary file not shown.

313
system/loggerd/loggerd.cc
View File

@@ -1,313 +0,0 @@
#include <sys/xattr.h>
#include <map>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#include "common/params.h"
#include "system/loggerd/encoder/encoder.h"
#include "system/loggerd/loggerd.h"
#include "system/loggerd/video_writer.h"
ExitHandler do_exit;
struct LoggerdState {
LoggerState logger;
std::atomic<double> last_camera_seen_tms;
std::atomic<int> ready_to_rotate; // count of encoders ready to rotate
int max_waiting = 0;
double last_rotate_tms = 0.; // last rotate time in ms
};
void logger_rotate(LoggerdState *s) {
bool ret =s->logger.next();
assert(ret);
s->ready_to_rotate = 0;
s->last_rotate_tms = millis_since_boot();
LOGW((s->logger.segment() == 0) ? "logging to %s" : "rotated to %s", s->logger.segmentPath().c_str());
}
void rotate_if_needed(LoggerdState *s) {
// all encoders ready, trigger rotation
bool all_ready = s->ready_to_rotate == s->max_waiting;
// fallback logic to prevent extremely long segments in the case of camera, encoder, etc. malfunctions
bool timed_out = false;
double tms = millis_since_boot();
double seg_length_secs = (tms - s->last_rotate_tms) / 1000.;
if ((seg_length_secs > SEGMENT_LENGTH) && !LOGGERD_TEST) {
// TODO: might be nice to put these reasons in the sentinel
if ((tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE) {
timed_out = true;
LOGE("no camera packets seen. auto rotating");
} else if (seg_length_secs > SEGMENT_LENGTH*1.2) {
timed_out = true;
LOGE("segment too long. auto rotating");
}
}
if (all_ready || timed_out) {
logger_rotate(s);
}
}
struct RemoteEncoder {
std::unique_ptr<VideoWriter> writer;
int encoderd_segment_offset;
int current_segment = -1;
std::vector<Message *> q;
int dropped_frames = 0;
bool recording = false;
bool marked_ready_to_rotate = false;
bool seen_first_packet = false;
};
int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct RemoteEncoder &re, const EncoderInfo &encoder_info) {
int bytes_count = 0;
// extract the message
capnp::FlatArrayMessageReader cmsg(kj::ArrayPtr<capnp::word>((capnp::word *)msg->getData(), msg->getSize() / sizeof(capnp::word)));
auto event = cmsg.getRoot<cereal::Event>();
auto edata = (event.*(encoder_info.get_encode_data_func))();
auto idx = edata.getIdx();
auto flags = idx.getFlags();
// encoderd can have started long before loggerd
if (!re.seen_first_packet) {
re.seen_first_packet = true;
re.encoderd_segment_offset = idx.getSegmentNum();
LOGD("%s: has encoderd offset %d", name.c_str(), re.encoderd_segment_offset);
}
int offset_segment_num = idx.getSegmentNum() - re.encoderd_segment_offset;
if (offset_segment_num == s->logger.segment()) {
// loggerd is now on the segment that matches this packet
// 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();
re.recording = false;
}
re.current_segment = s->logger.segment();
re.marked_ready_to_rotate = false;
// we are in this segment now, process any queued messages before this one
if (!re.q.empty()) {
for (auto &qmsg : re.q) {
bytes_count += handle_encoder_msg(s, qmsg, name, re, encoder_info);
}
re.q.clear();
}
}
// if we aren't recording yet, try to start, since we are in the correct segment
if (!re.recording) {
if (flags & V4L2_BUF_FLAG_KEYFRAME) {
// only create on iframe
if (re.dropped_frames) {
// this should only happen for the first segment, maybe
LOGW("%s: dropped %d non iframe packets before init", name.c_str(), 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);
}
re.recording = true;
} else {
// this is a sad case when we aren't recording, but don't have an iframe
// nothing we can do but drop the frame
delete msg;
++re.dropped_frames;
return bytes_count;
}
}
// we have to be recording if we are here
assert(re.recording);
// if we are actually writing the video file, do so
if (re.writer) {
auto data = edata.getData();
re.writer->write((uint8_t *)data.begin(), data.size(), idx.getTimestampEof()/1000, false, flags & V4L2_BUF_FLAG_KEYFRAME);
}
// put it in log stream as the idx packet
MessageBuilder bmsg;
auto evt = bmsg.initEvent(event.getValid());
evt.setLogMonoTime(event.getLogMonoTime());
(evt.*(encoder_info.set_encode_idx_func))(idx);
auto new_msg = bmsg.toBytes();
s->logger.write((uint8_t *)new_msg.begin(), new_msg.size(), true); // always in qlog?
bytes_count += new_msg.size();
// free the message, we used it
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) {
re.marked_ready_to_rotate = true;
++s->ready_to_rotate;
LOGD("rotate %d -> %d ready %d/%d for %s",
s->logger.segment(), offset_segment_num,
s->ready_to_rotate.load(), s->max_waiting, name.c_str());
}
// queue up all the new segment messages, they go in after the rotate
re.q.push_back(msg);
} else {
LOGE("%s: encoderd packet has a older segment!!! idx.getSegmentNum():%d s->logger.segment():%d re.encoderd_segment_offset:%d",
name.c_str(), idx.getSegmentNum(), s->logger.segment(), re.encoderd_segment_offset);
// free the message, it's useless. this should never happen
// actually, this can happen if you restart encoderd
re.encoderd_segment_offset = -s->logger.segment();
delete msg;
}
return bytes_count;
}
void handle_user_flag(LoggerdState *s) {
static int prev_segment = -1;
if (s->logger.segment() == prev_segment) return;
LOGW("preserving %s", s->logger.segmentPath().c_str());
#ifdef __APPLE__
int ret = setxattr(s->logger.segmentPath().c_str(), PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0, 0);
#else
int ret = setxattr(s->logger.segmentPath().c_str(), PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0);
#endif
if (ret) {
LOGE("setxattr %s failed for %s: %s", PRESERVE_ATTR_NAME, s->logger.segmentPath().c_str(), strerror(errno));
}
// mark route for uploading
Params params;
std::string routes = Params().get("AthenadRecentlyViewedRoutes");
params.put("AthenadRecentlyViewedRoutes", routes + "," + s->logger.routeName());
prev_segment = s->logger.segment();
}
void loggerd_thread() {
// setup messaging
typedef struct ServiceState {
std::string name;
int counter, freq;
bool encoder, user_flag;
} ServiceState;
std::unordered_map<SubSocket*, ServiceState> service_state;
std::unordered_map<SubSocket*, struct RemoteEncoder> remote_encoders;
std::unique_ptr<Context> ctx(Context::create());
std::unique_ptr<Poller> poller(Poller::create());
// subscribe to all socks
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 (on port %d)", it.name.c_str(), it.port);
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",
};
}
LoggerdState s;
// init logger
logger_rotate(&s);
Params().put("CurrentRoute", s.logger.routeName());
std::map<std::string, EncoderInfo> encoder_infos_dict;
for (const auto &cam : cameras_logged) {
for (const auto &encoder_info : cam.encoder_infos) {
encoder_infos_dict[encoder_info.publish_name] = encoder_info;
s.max_waiting++;
}
}
uint64_t msg_count = 0, bytes_count = 0;
double start_ts = millis_since_boot();
while (!do_exit) {
// poll for new messages on all sockets
for (auto sock : poller->poll(1000)) {
if (do_exit) break;
ServiceState &service = service_state[sock];
if (service.user_flag) {
handle_user_flag(&s);
}
// drain socket
int count = 0;
Message *msg = nullptr;
while (!do_exit && (msg = sock->receive(true))) {
const bool in_qlog = service.freq != -1 && (service.counter++ % service.freq == 0);
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]);
} else {
s.logger.write((uint8_t *)msg->getData(), msg->getSize(), in_qlog);
bytes_count += msg->getSize();
delete msg;
}
rotate_if_needed(&s);
if ((++msg_count % 1000) == 0) {
double seconds = (millis_since_boot() - start_ts) / 1000.0;
LOGD("%" PRIu64 " messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count / seconds, bytes_count * 0.001 / seconds);
}
count++;
if (count >= 200) {
LOGD("large volume of '%s' messages", service.name.c_str());
break;
}
}
}
}
LOGW("closing logger");
s.logger.setExitSignal(do_exit.signal);
if (do_exit.power_failure) {
LOGE("power failure");
sync();
LOGE("sync done");
}
// messaging cleanup
for (auto &[sock, service] : service_state) delete sock;
}
int main(int argc, char** argv) {
if (!Hardware::PC()) {
int ret;
ret = util::set_core_affinity({0, 1, 2, 3});
assert(ret == 0);
// TODO: why does this impact camerad timings?
//ret = util::set_realtime_priority(1);
//assert(ret == 0);
}
loggerd_thread();
return 0;
}

154
system/loggerd/loggerd.h
View File

@@ -1,154 +0,0 @@
#pragma once
#include <vector>
#include "cereal/messaging/messaging.h"
#include "cereal/services.h"
#include "cereal/visionipc/visionipc_client.h"
#include "system/camerad/cameras/camera_common.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 = Params().getBool("HigherBitrate") ? 20000000 : 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 *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;
CameraType type;
VisionStreamType stream_type;
std::vector<EncoderInfo> encoder_infos;
};
const EncoderInfo main_road_encoder_info = {
.publish_name = "roadEncodeData",
.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 = Params().getBool("RecordFront"),
INIT_ENCODE_FUNCTIONS(DriverEncode),
};
const EncoderInfo stream_road_encoder_info = {
.publish_name = "livestreamRoadEncodeData",
.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",
.type = RoadCam,
.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",
.type = WideRoadCam,
.stream_type = VISION_STREAM_WIDE_ROAD,
.encoder_infos = {main_wide_road_encoder_info}
};
const LogCameraInfo driver_camera_info{
.thread_name = "driver_cam_encoder",
.type = DriverCam,
.stream_type = VISION_STREAM_DRIVER,
.encoder_infos = {main_driver_encoder_info}
};
const LogCameraInfo stream_road_camera_info{
.thread_name = "road_cam_encoder",
.type = RoadCam,
.stream_type = VISION_STREAM_ROAD,
.encoder_infos = {stream_road_encoder_info}
};
const LogCameraInfo stream_wide_road_camera_info{
.thread_name = "wide_road_cam_encoder",
.type = WideRoadCam,
.stream_type = VISION_STREAM_WIDE_ROAD,
.encoder_infos = {stream_wide_road_encoder_info}
};
const LogCameraInfo stream_driver_camera_info{
.thread_name = "driver_cam_encoder",
.type = DriverCam,
.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};

112
system/loggerd/video_writer.cc
View File

@@ -1,112 +0,0 @@
#pragma clang diagnostic ignored "-Wdeprecated-declarations"
#include <cassert>
#include "system/loggerd/video_writer.h"
#include "common/swaglog.h"
#include "common/util.h"
VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec)
: remuxing(remuxing) {
vid_path = util::string_format("%s/%s", path, filename);
lock_path = util::string_format("%s/%s.lock", path, filename);
int lock_fd = HANDLE_EINTR(open(lock_path.c_str(), O_RDWR | O_CREAT, 0664));
assert(lock_fd >= 0);
close(lock_fd);
LOGD("encoder_open %s remuxing:%d", this->vid_path.c_str(), this->remuxing);
if (this->remuxing) {
bool raw = (codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS);
avformat_alloc_output_context2(&this->ofmt_ctx, NULL, raw ? "matroska" : NULL, this->vid_path.c_str());
assert(this->ofmt_ctx);
// set codec correctly. needed?
assert(codec != cereal::EncodeIndex::Type::FULL_H_E_V_C);
const AVCodec *avcodec = avcodec_find_encoder(raw ? AV_CODEC_ID_FFVHUFF : AV_CODEC_ID_H264);
assert(avcodec);
this->codec_ctx = avcodec_alloc_context3(avcodec);
assert(this->codec_ctx);
this->codec_ctx->width = width;
this->codec_ctx->height = height;
this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
this->codec_ctx->time_base = (AVRational){ 1, fps };
if (codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS) {
// without this, there's just noise
int err = avcodec_open2(this->codec_ctx, avcodec, NULL);
assert(err >= 0);
}
this->out_stream = avformat_new_stream(this->ofmt_ctx, raw ? avcodec : NULL);
assert(this->out_stream);
int err = avio_open(&this->ofmt_ctx->pb, this->vid_path.c_str(), AVIO_FLAG_WRITE);
assert(err >= 0);
} else {
this->of = util::safe_fopen(this->vid_path.c_str(), "wb");
assert(this->of);
}
}
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);
if (written != len) {
LOGE("failed to write file.errno=%d", errno);
}
}
if (remuxing) {
if (codecconfig) {
if (len > 0) {
codec_ctx->extradata = (uint8_t*)av_mallocz(len + AV_INPUT_BUFFER_PADDING_SIZE);
codec_ctx->extradata_size = len;
memcpy(codec_ctx->extradata, data, len);
}
int err = avcodec_parameters_from_context(out_stream->codecpar, codec_ctx);
assert(err >= 0);
err = avformat_write_header(ofmt_ctx, NULL);
assert(err >= 0);
} else {
// input timestamps are in microseconds
AVRational in_timebase = {1, 1000000};
AVPacket pkt;
av_init_packet(&pkt);
pkt.data = data;
pkt.size = len;
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);
pkt.duration = av_rescale_q(50*1000, in_timebase, ofmt_ctx->streams[0]->time_base);
if (keyframe) {
pkt.flags |= AV_PKT_FLAG_KEY;
}
// TODO: can use av_write_frame for non raw?
int err = av_interleaved_write_frame(ofmt_ctx, &pkt);
if (err < 0) { LOGW("ts encoder write issue len: %d ts: %lld", len, timestamp); }
av_packet_unref(&pkt);
}
}
}
VideoWriter::~VideoWriter() {
if (this->remuxing) {
int err = av_write_trailer(this->ofmt_ctx);
if (err != 0) LOGE("av_write_trailer failed %d", err);
avcodec_free_context(&this->codec_ctx);
err = avio_closep(&this->ofmt_ctx->pb);
if (err != 0) LOGE("avio_closep failed %d", err);
avformat_free_context(this->ofmt_ctx);
} else {
util::safe_fflush(this->of);
fclose(this->of);
this->of = nullptr;
}
unlink(this->lock_path.c_str());
}

25
system/loggerd/video_writer.h
View File

@@ -1,25 +0,0 @@
#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;
};