diff --git a/selfdrive/loggerd/encoder.c b/selfdrive/loggerd/encoder.c index 1509ff1130..c66fc38700 100644 --- a/selfdrive/loggerd/encoder.c +++ b/selfdrive/loggerd/encoder.c @@ -470,17 +470,16 @@ int encoder_encode_frame(EncoderState *s, // this sometimes freezes... put it outside the encoder lock so we can still trigger rotates... // THIS IS A REALLY BAD IDEA, but apparently the race has to happen 30 times to trigger this - pthread_mutex_unlock(&s->lock); + //pthread_mutex_unlock(&s->lock); OMX_BUFFERHEADERTYPE* in_buf = queue_pop(&s->free_in); - pthread_mutex_lock(&s->lock); - - if (s->rotating) { - encoder_close(s); - encoder_open(s, s->next_path); - s->segment = s->next_segment; - s->rotating = false; - } + //pthread_mutex_lock(&s->lock); + // if (s->rotating) { + // encoder_close(s); + // encoder_open(s, s->next_path); + // s->segment = s->next_segment; + // s->rotating = false; + // } int ret = s->counter; uint8_t *in_buf_ptr = in_buf->pBuffer; diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 645b528811..6bd26e59ba 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -54,9 +55,26 @@ #include "cereal/gen/cpp/log.capnp.h" +#define CAM_IDX_FCAM 0 +#define CAM_IDX_DCAM 1 +#define CAM_IDX_ECAM 2 + #define CAMERA_FPS 20 #define SEGMENT_LENGTH 60 + +#define MAIN_BITRATE 5000000 +#ifndef QCOM2 +#define DCAM_BITRATE 2500000 +#else +#define DCAM_BITRATE 5000000 +#endif + +#ifndef QCOM2 #define LOG_ROOT "/data/media/0/realdata" +#else +#define LOG_ROOT "/userdata" +#endif + #define ENABLE_LIDAR 0 #define RAW_CLIP_LENGTH 100 // 5 seconds at 20fps @@ -86,6 +104,12 @@ struct LoggerdState { uint32_t last_frame_id; uint32_t rotate_last_frame_id; int rotate_segment; + int rotate_seq_id; + + pthread_mutex_t rotate_lock; + int num_encoder; + int should_close; + int finish_close; }; LoggerdState s; @@ -93,20 +117,25 @@ LoggerdState s; void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { int err; - if (cam_idx == 2) { + // 0:f, 1:d, 2:e + if (cam_idx == CAM_IDX_DCAM) { // TODO: add this back - /*char *value; +#ifndef QCOM2 + char *value; const int result = read_db_value("RecordFront", &value, NULL); if (result != 0) return; if (value[0] != '1') { free(value); return; } free(value); - LOGW("recording front camera");*/ - + LOGW("recording front camera"); +#endif set_thread_name("FrontCameraEncoder"); - } else if (cam_idx == 0) { + } else if (cam_idx == CAM_IDX_FCAM) { set_thread_name("RearCameraEncoder"); - } else if (cam_idx == 1) { + } else if (cam_idx == CAM_IDX_ECAM) { set_thread_name("WideCameraEncoder"); + } else { + printf("unexpected camera index provided"); + assert(false); } VisionStream stream; @@ -119,19 +148,18 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { int encoder_segment = -1; int cnt = 0; - // TODO: add this back - // PubSocket *idx_sock = PubSocket::create(s.ctx, front ? "frontEncodeIdx" : "encodeIdx"); - // assert(idx_sock != NULL); + PubSocket *idx_sock = PubSocket::create(s.ctx, cam_idx == CAM_IDX_DCAM ? "frontEncodeIdx" : (cam_idx == CAM_IDX_ECAM ? "wideEncodeIdx" : "encodeIdx")); + assert(idx_sock != NULL); LoggerHandle *lh = NULL; while (!do_exit) { VisionStreamBufs buf_info; - if (cam_idx == 2) { + if (cam_idx == CAM_IDX_DCAM) { err = visionstream_init(&stream, VISION_STREAM_YUV_FRONT, false, &buf_info); - } else if (cam_idx == 0) { + } else if (cam_idx == CAM_IDX_FCAM) { err = visionstream_init(&stream, VISION_STREAM_YUV, false, &buf_info); - } else if (cam_idx == 1) { + } else if (cam_idx == CAM_IDX_ECAM) { err = visionstream_init(&stream, VISION_STREAM_YUV_WIDE, false, &buf_info); } if (err != 0) { @@ -142,11 +170,11 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { if (!encoder_inited) { LOGD("encoder init %dx%d", buf_info.width, buf_info.height); - encoder_init(&encoder, cam_idx == 2 ? "dcamera.hevc" : (cam_idx == 1 ? "ecamera.hevc" : "fcamera.hevc"), buf_info.width, buf_info.height, CAMERA_FPS, cam_idx > 0 ? 2500000 : 5000000, true, false); + encoder_init(&encoder, cam_idx == CAM_IDX_DCAM ? "dcamera.hevc" : (cam_idx == CAM_IDX_ECAM ? "ecamera.hevc" : "fcamera.hevc"), buf_info.width, buf_info.height, CAMERA_FPS, cam_idx == CAM_IDX_DCAM ? DCAM_BITRATE:MAIN_BITRATE, true, false); #ifndef QCOM2 // TODO: fix qcamera on tici - if (cam_idx == 0) { + if (cam_idx == CAM_IDX_FCAM) { encoder_init(&encoder_alt, "qcamera.ts", 480, 360, CAMERA_FPS, 128000, false, true); has_encoder_alt = true; } @@ -188,19 +216,20 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { uint8_t *v = u + (buf_info.width/2)*(buf_info.height/2); { + // all the rotation stuff bool should_rotate = false; std::unique_lock lk(s.lock); - if (cam_idx==0) { + if (cam_idx == CAM_IDX_FCAM) { // TODO: should wait for three cameras? // wait if log camera is older on back camera while ( extra.frame_id > s.last_frame_id //if the log camera is older, wait for it to catch up. && (extra.frame_id-s.last_frame_id) < 8 // but if its too old then there probably was a discontinuity (visiond restarted) && !do_exit) { s.cv.wait(lk); } - should_rotate = extra.frame_id > s.rotate_last_frame_id && encoder_segment < s.rotate_segment; + should_rotate = extra.frame_id > s.rotate_last_frame_id && encoder_segment < s.rotate_segment && s.rotate_seq_id == cam_idx; } else { // front camera is best effort - should_rotate = encoder_segment < s.rotate_segment; + should_rotate = encoder_segment < s.rotate_segment && s.rotate_seq_id == cam_idx; } if (do_exit) break; @@ -209,6 +238,8 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { LOG("rotate encoder to %s", s.segment_path); encoder_rotate(&encoder, s.segment_path, s.rotate_segment); + s.rotate_seq_id = (cam_idx + 1) % s.num_encoder; + if (has_encoder_alt) { encoder_rotate(&encoder_alt, s.segment_path, s.rotate_segment); } @@ -223,8 +254,37 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { } lh = logger_get_handle(&s.logger); } - } + if (encoder.rotating) { + pthread_mutex_lock(&s.rotate_lock); + s.should_close += 1; + pthread_mutex_unlock(&s.rotate_lock); + + while(s.should_close > 0 && s.should_close < s.num_encoder) { + // printf("%d waiting for others to reach close, %d/%d \n", cam_idx, s.should_close, s.num_encoder); + s.cv.wait(lk); + } + + pthread_mutex_lock(&s.rotate_lock); + if (s.should_close == s.num_encoder) { + s.should_close = 1 - s.num_encoder; + } else { + s.should_close += 1; + } + encoder_close(&encoder); + encoder_open(&encoder, encoder.next_path); + encoder.segment = encoder.next_segment; + encoder.rotating = false; + s.finish_close += 1; + pthread_mutex_unlock(&s.rotate_lock); + + while(s.finish_close > 0 && s.finish_close < s.num_encoder) { + // printf("%d waiting for others to actually close, %d/%d \n", cam_idx, s.finish_close, s.num_encoder); + s.cv.wait(lk); + } + s.finish_close = 0; + } + } { // encode hevc int out_segment = -1; @@ -232,7 +292,6 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { y, u, v, buf_info.width, buf_info.height, &out_segment, &extra); - if (has_encoder_alt) { int out_segment_alt = -1; encoder_encode_frame(&encoder_alt, @@ -241,27 +300,30 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { &out_segment_alt, &extra); } - // TODO: add this back // publish encode index - //capnp::MallocMessageBuilder msg; - //cereal::Event::Builder event = msg.initRoot(); - //event.setLogMonoTime(nanos_since_boot()); - //auto eidx = event.initEncodeIdx(); - //eidx.setFrameId(extra.frame_id); - //eidx.setType(front ? cereal::EncodeIndex::Type::FRONT : cereal::EncodeIndex::Type::FULL_H_E_V_C); - //eidx.setEncodeId(cnt); - //eidx.setSegmentNum(out_segment); - //eidx.setSegmentId(out_id); - - //auto words = capnp::messageToFlatArray(msg); - //auto bytes = words.asBytes(); + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto eidx = event.initEncodeIdx(); + eidx.setFrameId(extra.frame_id); +#ifdef QCOM2 + eidx.setType(cereal::EncodeIndex::Type::FULL_H_E_V_C); +#else + eidx.setType(cam_idx == CAM_IDX_DCAM ? cereal::EncodeIndex::Type::FRONT : cereal::EncodeIndex::Type::FULL_H_E_V_C); +#endif + eidx.setEncodeId(cnt); + eidx.setSegmentNum(out_segment); + eidx.setSegmentId(out_id); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); - //if (idx_sock->send((char*)bytes.begin(), bytes.size()) < 0) { - // printf("err sending encodeIdx pkt: %s\n", strerror(errno)); - //} - //if (lh) { - // lh_log(lh, bytes.begin(), bytes.size(), false); - //} + if (idx_sock->send((char*)bytes.begin(), bytes.size()) < 0) { + printf("err sending encodeIdx pkt: %s\n", strerror(errno)); + } + if (lh) { + lh_log(lh, bytes.begin(), bytes.size(), false); + } } if (raw_clips) { @@ -321,8 +383,7 @@ void encoder_thread(bool is_streaming, bool raw_clips, int cam_idx) { visionstream_destroy(&stream); } - // TODO: add this back - //delete idx_sock; + delete idx_sock; if (encoder_inited) { LOG("encoder destroy"); @@ -644,16 +705,25 @@ int main(int argc, char** argv) { double start_ts = seconds_since_boot(); double last_rotate_ts = start_ts; - + s.rotate_seq_id = 0; + s.should_close = 0; + s.finish_close = 0; + s.num_encoder = 0; + pthread_mutex_init(&s.rotate_lock, NULL); #ifndef DISABLE_ENCODER // rear camera - std::thread encoder_thread_handle(encoder_thread, is_streaming, false, 0); + std::thread encoder_thread_handle(encoder_thread, is_streaming, false, CAM_IDX_FCAM); + s.num_encoder += 1; // front camera - std::thread front_encoder_thread_handle(encoder_thread, false, false, 2); + std::thread front_encoder_thread_handle(encoder_thread, false, false, CAM_IDX_DCAM); + s.num_encoder += 1; +#ifdef QCOM2 // wide camera - std::thread wide_encoder_thread_handle(encoder_thread, false, false, 1); + std::thread wide_encoder_thread_handle(encoder_thread, false, false, CAM_IDX_ECAM); + s.num_encoder += 1; +#endif #endif #if ENABLE_LIDAR @@ -729,8 +799,10 @@ int main(int argc, char** argv) { #ifndef DISABLE_ENCODER - front_encoder_thread_handle.join(); +#ifdef QCOM2 wide_encoder_thread_handle.join(); +#endif + front_encoder_thread_handle.join(); encoder_thread_handle.join(); LOGW("encoder joined"); #endif