diff --git a/selfdrive/SConscript b/selfdrive/SConscript index f9d1c88..a03a014 100755 --- a/selfdrive/SConscript +++ b/selfdrive/SConscript @@ -3,4 +3,5 @@ SConscript(['controls/lib/lateral_mpc_lib/SConscript']) SConscript(['controls/lib/longitudinal_mpc_lib/SConscript']) SConscript(['locationd/SConscript']) SConscript(['modeld/SConscript']) -SConscript(['ui/SConscript']) \ No newline at end of file +SConscript(['ui/SConscript']) +SConscript(['clearpilot/SConscript']) \ No newline at end of file diff --git a/selfdrive/clearpilot/SConscript b/selfdrive/clearpilot/SConscript new file mode 100644 index 0000000..2a58a7e --- /dev/null +++ b/selfdrive/clearpilot/SConscript @@ -0,0 +1,16 @@ +Import('env', 'arch', 'common', 'messaging', 'visionipc', 'cereal') + +clearpilot_env = env.Clone() +clearpilot_env['CPPPATH'] += ['#selfdrive/frogpilot/screenrecorder/openmax/include/'] +# Disable --as-needed so static lib ordering doesn't matter +clearpilot_env['LINKFLAGS'] = [f for f in clearpilot_env.get('LINKFLAGS', []) if f != '-Wl,--as-needed'] + +if arch == "larch64": + omx_obj = File('#selfdrive/frogpilot/screenrecorder/omx_encoder.o') + clearpilot_env.Program( + 'dashcamd', + ['dashcamd.cc', omx_obj], + LIBS=[common, 'json11', cereal, visionipc, messaging, + 'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread', + 'OmxCore', 'avformat', 'avcodec', 'avutil', 'yuv'] + ) diff --git a/selfdrive/clearpilot/dashcamd b/selfdrive/clearpilot/dashcamd new file mode 100755 index 0000000..da98ac4 Binary files /dev/null and b/selfdrive/clearpilot/dashcamd differ diff --git a/selfdrive/clearpilot/dashcamd.cc b/selfdrive/clearpilot/dashcamd.cc new file mode 100644 index 0000000..97f84fa --- /dev/null +++ b/selfdrive/clearpilot/dashcamd.cc @@ -0,0 +1,409 @@ +/* + * CLEARPILOT dashcamd — records raw camera footage to MP4 using OMX H.264 hardware encoder. + * + * Connects to camerad via VisionIPC, receives NV12 frames, and feeds them directly + * to the Qualcomm OMX encoder. Produces 3-minute MP4 segments organized by trip. + * + * Trip directory structure: + * /data/media/0/videos/YYYYMMDD-HHMMSS/ (trip directory, named at trip start) + * YYYYMMDD-HHMMSS.mp4 (3-minute segments) + * YYYYMMDD-HHMMSS.srt (GPS subtitle sidecar) + * + * Trip lifecycle state machine: + * + * WAITING: + * - Process starts in this state + * - Waits for valid system time (year >= 2024) AND car in drive gear + * - Transitions to RECORDING when both conditions met + * + * RECORDING: + * - Actively encoding frames, car is in drive + * - Car leaves drive → start 10-min idle timer → IDLE_TIMEOUT + * + * IDLE_TIMEOUT: + * - Car left drive, still recording with timer running + * - Car re-enters drive → cancel timer → RECORDING + * - Timer expires → close trip → WAITING + * - Ignition off → close trip → WAITING + * + * Graceful shutdown (DashcamShutdown param): + * - thermald sets DashcamShutdown="1" before device power-off + * - dashcamd closes current segment, acks, exits + * + * Published params (memory, every 5s): + * - DashcamState: "waiting" or "recording" + * - DashcamFrames: per-trip encoded frame count (resets each trip) + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "cereal/visionipc/visionipc_client.h" +#include "common/params.h" +#include "common/timing.h" +#include "common/swaglog.h" +#include "common/util.h" +#include "selfdrive/frogpilot/screenrecorder/omx_encoder.h" + +const std::string VIDEOS_BASE = "/data/media/0/videos"; +const int SEGMENT_SECONDS = 180; // 3 minutes +const int SOURCE_FPS = 20; +const int CAMERA_FPS = 10; +const int FRAMES_PER_SEGMENT = SEGMENT_SECONDS * CAMERA_FPS; +const int BITRATE = 2500 * 1024; // 2500 kbps +const double IDLE_TIMEOUT_SECONDS = 600.0; // 10 minutes + +ExitHandler do_exit; + +enum TripState { + WAITING, // no trip, waiting for valid time + drive gear + RECORDING, // actively recording, car in drive + IDLE_TIMEOUT, // car left drive, recording with 10-min timer +}; + +static std::string make_timestamp() { + char buf[64]; + time_t t = time(NULL); + struct tm tm = *localtime(&t); + snprintf(buf, sizeof(buf), "%04d%02d%02d-%02d%02d%02d", + tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, + tm.tm_hour, tm.tm_min, tm.tm_sec); + return std::string(buf); +} + +static bool system_time_valid() { + time_t t = time(NULL); + struct tm tm = *gmtime(&t); + return (tm.tm_year + 1900) >= 2024; +} + +static std::string make_utc_timestamp() { + char buf[32]; + time_t t = time(NULL); + struct tm tm = *gmtime(&t); + snprintf(buf, sizeof(buf), "%04d-%02d-%02d %02d:%02d:%02d UTC", + tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, + tm.tm_hour, tm.tm_min, tm.tm_sec); + return std::string(buf); +} + +// Format SRT timestamp: HH:MM:SS,mmm +static std::string srt_time(int seconds) { + int h = seconds / 3600; + int m = (seconds % 3600) / 60; + int s = seconds % 60; + char buf[16]; + snprintf(buf, sizeof(buf), "%02d:%02d:%02d,000", h, m, s); + return std::string(buf); +} + +static void crash_handler(int sig) { + FILE *f = fopen("/tmp/dashcamd_crash.log", "w"); + if (f) { + fprintf(f, "CRASH: signal %d\n", sig); + void *bt[30]; + int n = backtrace(bt, 30); + backtrace_symbols_fd(bt, n, fileno(f)); + fclose(f); + } + _exit(1); +} + +int main(int argc, char *argv[]) { + signal(SIGSEGV, crash_handler); + signal(SIGABRT, crash_handler); + setpriority(PRIO_PROCESS, 0, -10); + + // CLEARPILOT: pin to cores 0-3 (little cluster). Avoids cache/memory-bandwidth + // contention with the RT-pinned processes on the big cluster: + // core 4 = controlsd, core 5 = plannerd/radard, core 7 = modeld. + // OMX offloads actual H.264 work to hardware, so the main thread just copies + // frames and muxes MP4 — fine on the little cluster. + cpu_set_t mask; + CPU_ZERO(&mask); + for (int i = 0; i < 4; i++) CPU_SET(i, &mask); + if (sched_setaffinity(0, sizeof(mask), &mask) != 0) { + LOGW("dashcamd: sched_setaffinity failed (%d), continuing unpinned", errno); + } else { + LOGW("dashcamd: pinned to cores 0-3"); + } + + // Ensure base output directory exists + mkdir(VIDEOS_BASE.c_str(), 0755); + + LOGW("dashcamd: started, connecting to camerad road stream"); + VisionIpcClient vipc("camerad", VISION_STREAM_ROAD, false); + while (!do_exit && !vipc.connect(false)) { + usleep(100000); + } + if (do_exit) return 0; + LOGW("dashcamd: vipc connected, waiting for valid frame"); + + // Wait for a frame with valid dimensions (camerad may still be initializing) + VisionBuf *init_buf = nullptr; + while (!do_exit) { + init_buf = vipc.recv(); + if (init_buf != nullptr && init_buf->width > 0 && init_buf->height > 0) break; + usleep(100000); + } + if (do_exit) return 0; + + int width = init_buf->width; + int height = init_buf->height; + int y_stride = init_buf->stride; + int uv_stride = y_stride; + LOGW("dashcamd: first valid frame %dx%d stride=%d", width, height, y_stride); + + // Subscribe to carState (gear), deviceState (ignition), gpsLocation (subtitles) + SubMaster sm({"carState", "deviceState", "gpsLocation"}); + Params params; + Params params_memory("/dev/shm/params"); + + // Trip state + TripState state = WAITING; + OmxEncoder *encoder = nullptr; + std::string trip_dir; + int frame_count = 0; // per-segment (for rotation) + int trip_frames = 0; // per-trip (published to params) + int recv_count = 0; + uint64_t segment_start_ts = 0; + double idle_timer_start = 0.0; + + // SRT subtitle state + FILE *srt_file = nullptr; + int srt_index = 0; + int srt_segment_sec = 0; + double last_srt_write = 0; + + // Ignition tracking + bool prev_started = false; + bool started_initialized = false; + + // Param publish throttle + int param_check_counter = 0; + double last_param_write = 0; + + // Publish initial state + params_memory.put("DashcamState", "waiting"); + params_memory.put("DashcamFrames", "0"); + + LOGW("dashcamd: entering main loop in WAITING state"); + + // Helper: start a new trip + auto start_new_trip = [&]() { + trip_dir = VIDEOS_BASE + "/" + make_timestamp(); + mkdir(trip_dir.c_str(), 0755); + LOGW("dashcamd: new trip %s", trip_dir.c_str()); + + encoder = new OmxEncoder(trip_dir.c_str(), width, height, CAMERA_FPS, BITRATE); + + std::string seg_name = make_timestamp(); + LOGW("dashcamd: opening segment %s", seg_name.c_str()); + encoder->encoder_open((seg_name + ".mp4").c_str()); + + std::string srt_path = trip_dir + "/" + seg_name + ".srt"; + srt_file = fopen(srt_path.c_str(), "w"); + srt_index = 0; + srt_segment_sec = 0; + last_srt_write = 0; + + frame_count = 0; + trip_frames = 0; + segment_start_ts = nanos_since_boot(); + state = RECORDING; + + params_memory.put("DashcamState", "recording"); + params_memory.put("DashcamFrames", "0"); + }; + + // Helper: close current trip + auto close_trip = [&]() { + if (srt_file) { fclose(srt_file); srt_file = nullptr; } + if (encoder) { + encoder->encoder_close(); + LOGW("dashcamd: segment closed"); + delete encoder; + encoder = nullptr; + } + state = WAITING; + frame_count = 0; + trip_frames = 0; + idle_timer_start = 0.0; + LOGW("dashcamd: trip ended, returning to WAITING"); + + params_memory.put("DashcamState", "waiting"); + params_memory.put("DashcamFrames", "0"); + }; + + while (!do_exit) { + VisionBuf *buf = vipc.recv(); + if (buf == nullptr) continue; + + // Skip frames to match target FPS (SOURCE_FPS -> CAMERA_FPS) + recv_count++; + if (SOURCE_FPS > CAMERA_FPS && (recv_count % (SOURCE_FPS / CAMERA_FPS)) != 0) continue; + + sm.update(0); + double now = nanos_since_boot() / 1e9; + + // Read vehicle state + bool started = sm.valid("deviceState") && + sm["deviceState"].getDeviceState().getStarted(); + auto gear = sm.valid("carState") ? + sm["carState"].getCarState().getGearShifter() : + cereal::CarState::GearShifter::UNKNOWN; + bool in_drive = (gear == cereal::CarState::GearShifter::DRIVE || + gear == cereal::CarState::GearShifter::SPORT || + gear == cereal::CarState::GearShifter::LOW || + gear == cereal::CarState::GearShifter::MANUMATIC); + + // Detect ignition off → close any active trip + if (started_initialized && prev_started && !started) { + LOGW("dashcamd: ignition off"); + if (state == RECORDING || state == IDLE_TIMEOUT) { + close_trip(); + } + } + prev_started = started; + started_initialized = true; + + // Check for graceful shutdown request (every ~1 second) + if (++param_check_counter >= CAMERA_FPS) { + param_check_counter = 0; + if (params_memory.getBool("DashcamShutdown")) { + LOGW("dashcamd: shutdown requested"); + if (state == RECORDING || state == IDLE_TIMEOUT) { + close_trip(); + } + params_memory.putBool("DashcamShutdown", false); + LOGW("dashcamd: shutdown ack sent, exiting"); + break; + } + } + + // State machine + switch (state) { + case WAITING: { + bool has_gps = sm.valid("gpsLocation") && sm["gpsLocation"].getGpsLocation().getHasFix(); + if (in_drive && system_time_valid() && has_gps) { + start_new_trip(); + } + break; + } + + case RECORDING: + if (!in_drive) { + idle_timer_start = now; + state = IDLE_TIMEOUT; + LOGW("dashcamd: car left drive, starting 10-min idle timer"); + } + break; + + case IDLE_TIMEOUT: + if (in_drive) { + idle_timer_start = 0.0; + state = RECORDING; + LOGW("dashcamd: back in drive, resuming trip"); + } else if ((now - idle_timer_start) >= IDLE_TIMEOUT_SECONDS) { + LOGW("dashcamd: idle timeout expired"); + close_trip(); + } + break; + } + + // Only encode frames when we have an active recording + if (state != RECORDING && state != IDLE_TIMEOUT) continue; + + // Segment rotation + if (frame_count >= FRAMES_PER_SEGMENT) { + if (srt_file) { fclose(srt_file); srt_file = nullptr; } + encoder->encoder_close(); + + std::string seg_name = make_timestamp(); + LOGW("dashcamd: opening segment %s", seg_name.c_str()); + encoder->encoder_open((seg_name + ".mp4").c_str()); + + std::string srt_path = trip_dir + "/" + seg_name + ".srt"; + srt_file = fopen(srt_path.c_str(), "w"); + srt_index = 0; + srt_segment_sec = 0; + last_srt_write = 0; + + frame_count = 0; + segment_start_ts = nanos_since_boot(); + } + + uint64_t ts = nanos_since_boot() - segment_start_ts; + + // Validate buffer before encoding + if (buf->y == nullptr || buf->uv == nullptr || buf->width == 0 || buf->height == 0) { + LOGE("dashcamd: invalid frame buf y=%p uv=%p %zux%zu, skipping", buf->y, buf->uv, buf->width, buf->height); + continue; + } + + // Feed NV12 frame directly to OMX encoder + encoder->encode_frame_nv12(buf->y, y_stride, buf->uv, uv_stride, width, height, ts); + frame_count++; + trip_frames++; + + // Publish state every 5 seconds + if (now - last_param_write >= 5.0) { + params_memory.put("DashcamFrames", std::to_string(trip_frames)); + last_param_write = now; + } + + // Write GPS subtitle at most once per second + if (srt_file && (now - last_srt_write) >= 1.0) { + last_srt_write = now; + srt_index++; + + double lat = 0, lon = 0, speed_ms = 0; + bool has_gps = sm.valid("gpsLocation") && sm["gpsLocation"].getGpsLocation().getHasFix(); + if (has_gps) { + auto gps = sm["gpsLocation"].getGpsLocation(); + lat = gps.getLatitude(); + lon = gps.getLongitude(); + speed_ms = gps.getSpeed(); + } + + double speed_mph = speed_ms * 2.23694; + std::string utc = make_utc_timestamp(); + std::string t_start = srt_time(srt_segment_sec); + std::string t_end = srt_time(srt_segment_sec + 1); + srt_segment_sec++; + + if (has_gps) { + const char *deg = "\xC2\xB0"; // UTF-8 degree sign + fprintf(srt_file, "%d\n%s --> %s\n%.0f MPH | %.4f%s%c %.4f%s%c | %s\n\n", + srt_index, t_start.c_str(), t_end.c_str(), + speed_mph, + std::abs(lat), deg, lat >= 0 ? 'N' : 'S', + std::abs(lon), deg, lon >= 0 ? 'E' : 'W', + utc.c_str()); + } else { + fprintf(srt_file, "%d\n%s --> %s\nNo GPS | %s\n\n", + srt_index, t_start.c_str(), t_end.c_str(), utc.c_str()); + } + fflush(srt_file); + } + } + + // Clean exit + if (state == RECORDING || state == IDLE_TIMEOUT) { + close_trip(); + } + params_memory.put("DashcamState", "stopped"); + params_memory.put("DashcamFrames", "0"); + LOGW("dashcamd: stopped"); + + return 0; +} diff --git a/selfdrive/frogpilot/screenrecorder/omx_encoder.cc b/selfdrive/frogpilot/screenrecorder/omx_encoder.cc index 332e541..bbb1888 100755 --- a/selfdrive/frogpilot/screenrecorder/omx_encoder.cc +++ b/selfdrive/frogpilot/screenrecorder/omx_encoder.cc @@ -35,120 +35,19 @@ int ABGRToNV12(const uint8_t* src_abgr, int halfwidth = (width + 1) >> 1; void (*ABGRToUVRow)(const uint8_t* src_abgr0, int src_stride_abgr, uint8_t* dst_u, uint8_t* dst_v, int width) = - ABGRToUVRow_C; + ABGRToUVRow_NEON; void (*ABGRToYRow)(const uint8_t* src_abgr, uint8_t* dst_y, int width) = - ABGRToYRow_C; - void (*MergeUVRow_)(const uint8_t* src_u, const uint8_t* src_v, uint8_t* dst_uv, int width) = MergeUVRow_C; + ABGRToYRow_NEON; + void (*MergeUVRow_)(const uint8_t* src_u, const uint8_t* src_v, uint8_t* dst_uv, int width) = MergeUVRow_NEON; if (!src_abgr || !dst_y || !dst_uv || width <= 0 || height == 0) { return -1; } -if (height < 0) { // Negative height means invert the image. - height = -height; - src_abgr = src_abgr + (height - 1) * src_stride_abgr; - src_stride_abgr = -src_stride_abgr; -} -#if defined(HAS_ABGRTOYROW_SSSE3) && defined(HAS_ABGRTOUVROW_SSSE3) - if (TestCpuFlag(kCpuHasSSSE3)) { - ABGRToUVRow = ABGRToUVRow_Any_SSSE3; - ABGRToYRow = ABGRToYRow_Any_SSSE3; - if (IS_ALIGNED(width, 16)) { - ABGRToUVRow = ABGRToUVRow_SSSE3; - ABGRToYRow = ABGRToYRow_SSSE3; - } + if (height < 0) { + height = -height; + src_abgr = src_abgr + (height - 1) * src_stride_abgr; + src_stride_abgr = -src_stride_abgr; } -#endif -#if defined(HAS_ABGRTOYROW_AVX2) && defined(HAS_ABGRTOUVROW_AVX2) - if (TestCpuFlag(kCpuHasAVX2)) { - ABGRToUVRow = ABGRToUVRow_Any_AVX2; - ABGRToYRow = ABGRToYRow_Any_AVX2; - if (IS_ALIGNED(width, 32)) { - ABGRToUVRow = ABGRToUVRow_AVX2; - ABGRToYRow = ABGRToYRow_AVX2; - } - } -#endif -#if defined(HAS_ABGRTOYROW_NEON) - if (TestCpuFlag(kCpuHasNEON)) { - ABGRToYRow = ABGRToYRow_Any_NEON; - if (IS_ALIGNED(width, 8)) { - ABGRToYRow = ABGRToYRow_NEON; - } - } -#endif -#if defined(HAS_ABGRTOUVROW_NEON) - if (TestCpuFlag(kCpuHasNEON)) { - ABGRToUVRow = ABGRToUVRow_Any_NEON; - if (IS_ALIGNED(width, 16)) { - ABGRToUVRow = ABGRToUVRow_NEON; - } - } -#endif -#if defined(HAS_ABGRTOYROW_MMI) && defined(HAS_ABGRTOUVROW_MMI) - if (TestCpuFlag(kCpuHasMMI)) { - ABGRToYRow = ABGRToYRow_Any_MMI; - ABGRToUVRow = ABGRToUVRow_Any_MMI; - if (IS_ALIGNED(width, 8)) { - ABGRToYRow = ABGRToYRow_MMI; - } - if (IS_ALIGNED(width, 16)) { - ABGRToUVRow = ABGRToUVRow_MMI; - } - } -#endif -#if defined(HAS_ABGRTOYROW_MSA) && defined(HAS_ABGRTOUVROW_MSA) - if (TestCpuFlag(kCpuHasMSA)) { - ABGRToYRow = ABGRToYRow_Any_MSA; - ABGRToUVRow = ABGRToUVRow_Any_MSA; - if (IS_ALIGNED(width, 16)) { - ABGRToYRow = ABGRToYRow_MSA; - } - if (IS_ALIGNED(width, 32)) { - ABGRToUVRow = ABGRToUVRow_MSA; - } - } -#endif -#if defined(HAS_MERGEUVROW_SSE2) - if (TestCpuFlag(kCpuHasSSE2)) { - MergeUVRow_ = MergeUVRow_Any_SSE2; - if (IS_ALIGNED(halfwidth, 16)) { - MergeUVRow_ = MergeUVRow_SSE2; - } - } -#endif -#if defined(HAS_MERGEUVROW_AVX2) - if (TestCpuFlag(kCpuHasAVX2)) { - MergeUVRow_ = MergeUVRow_Any_AVX2; - if (IS_ALIGNED(halfwidth, 32)) { - MergeUVRow_ = MergeUVRow_AVX2; - } - } -#endif -#if defined(HAS_MERGEUVROW_NEON) - if (TestCpuFlag(kCpuHasNEON)) { - MergeUVRow_ = MergeUVRow_Any_NEON; - if (IS_ALIGNED(halfwidth, 16)) { - MergeUVRow_ = MergeUVRow_NEON; - } - } -#endif -#if defined(HAS_MERGEUVROW_MMI) - if (TestCpuFlag(kCpuHasMMI)) { - MergeUVRow_ = MergeUVRow_Any_MMI; - if (IS_ALIGNED(halfwidth, 8)) { - MergeUVRow_ = MergeUVRow_MMI; - } - } -#endif -#if defined(HAS_MERGEUVROW_MSA) - if (TestCpuFlag(kCpuHasMSA)) { - MergeUVRow_ = MergeUVRow_Any_MSA; - if (IS_ALIGNED(halfwidth, 16)) { - MergeUVRow_ = MergeUVRow_MSA; - } - } -#endif { - // Allocate a rows of uv. align_buffer_64(row_u, ((halfwidth + 31) & ~31) * 2); uint8_t* row_v = row_u + ((halfwidth + 31) & ~31); @@ -182,9 +81,9 @@ extern ExitHandler do_exit; // ***** OMX callback functions ***** void OmxEncoder::wait_for_state(OMX_STATETYPE state_) { - std::unique_lock lk(this->state_lock); - while (this->state != state_) { - this->state_cv.wait(lk); + std::unique_lock lk(state_lock); + while (state != state_) { + state_cv.wait(lk); } } @@ -236,270 +135,203 @@ static const char* omx_color_fomat_name(uint32_t format) __attribute__((unused)) static const char* omx_color_fomat_name(uint32_t format) { switch (format) { case OMX_COLOR_FormatUnused: return "OMX_COLOR_FormatUnused"; - case OMX_COLOR_FormatMonochrome: return "OMX_COLOR_FormatMonochrome"; - case OMX_COLOR_Format8bitRGB332: return "OMX_COLOR_Format8bitRGB332"; - case OMX_COLOR_Format12bitRGB444: return "OMX_COLOR_Format12bitRGB444"; - case OMX_COLOR_Format16bitARGB4444: return "OMX_COLOR_Format16bitARGB4444"; - case OMX_COLOR_Format16bitARGB1555: return "OMX_COLOR_Format16bitARGB1555"; - case OMX_COLOR_Format16bitRGB565: return "OMX_COLOR_Format16bitRGB565"; - case OMX_COLOR_Format16bitBGR565: return "OMX_COLOR_Format16bitBGR565"; - case OMX_COLOR_Format18bitRGB666: return "OMX_COLOR_Format18bitRGB666"; - case OMX_COLOR_Format18bitARGB1665: return "OMX_COLOR_Format18bitARGB1665"; - case OMX_COLOR_Format19bitARGB1666: return "OMX_COLOR_Format19bitARGB1666"; - case OMX_COLOR_Format24bitRGB888: return "OMX_COLOR_Format24bitRGB888"; - case OMX_COLOR_Format24bitBGR888: return "OMX_COLOR_Format24bitBGR888"; - case OMX_COLOR_Format24bitARGB1887: return "OMX_COLOR_Format24bitARGB1887"; - case OMX_COLOR_Format25bitARGB1888: return "OMX_COLOR_Format25bitARGB1888"; - case OMX_COLOR_Format32bitBGRA8888: return "OMX_COLOR_Format32bitBGRA8888"; - case OMX_COLOR_Format32bitARGB8888: return "OMX_COLOR_Format32bitARGB8888"; - case OMX_COLOR_FormatYUV411Planar: return "OMX_COLOR_FormatYUV411Planar"; - case OMX_COLOR_FormatYUV411PackedPlanar: return "OMX_COLOR_FormatYUV411PackedPlanar"; - case OMX_COLOR_FormatYUV420Planar: return "OMX_COLOR_FormatYUV420Planar"; - case OMX_COLOR_FormatYUV420PackedPlanar: return "OMX_COLOR_FormatYUV420PackedPlanar"; case OMX_COLOR_FormatYUV420SemiPlanar: return "OMX_COLOR_FormatYUV420SemiPlanar"; - case OMX_COLOR_FormatYUV422Planar: return "OMX_COLOR_FormatYUV422Planar"; - case OMX_COLOR_FormatYUV422PackedPlanar: return "OMX_COLOR_FormatYUV422PackedPlanar"; - case OMX_COLOR_FormatYUV422SemiPlanar: return "OMX_COLOR_FormatYUV422SemiPlanar"; - case OMX_COLOR_FormatYCbYCr: return "OMX_COLOR_FormatYCbYCr"; - case OMX_COLOR_FormatYCrYCb: return "OMX_COLOR_FormatYCrYCb"; - case OMX_COLOR_FormatCbYCrY: return "OMX_COLOR_FormatCbYCrY"; - case OMX_COLOR_FormatCrYCbY: return "OMX_COLOR_FormatCrYCbY"; - case OMX_COLOR_FormatYUV444Interleaved: return "OMX_COLOR_FormatYUV444Interleaved"; - case OMX_COLOR_FormatRawBayer8bit: return "OMX_COLOR_FormatRawBayer8bit"; - case OMX_COLOR_FormatRawBayer10bit: return "OMX_COLOR_FormatRawBayer10bit"; - case OMX_COLOR_FormatRawBayer8bitcompressed: return "OMX_COLOR_FormatRawBayer8bitcompressed"; - case OMX_COLOR_FormatL2: return "OMX_COLOR_FormatL2"; - case OMX_COLOR_FormatL4: return "OMX_COLOR_FormatL4"; - case OMX_COLOR_FormatL8: return "OMX_COLOR_FormatL8"; - case OMX_COLOR_FormatL16: return "OMX_COLOR_FormatL16"; - case OMX_COLOR_FormatL24: return "OMX_COLOR_FormatL24"; - case OMX_COLOR_FormatL32: return "OMX_COLOR_FormatL32"; - case OMX_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_COLOR_FormatYUV420PackedSemiPlanar"; - case OMX_COLOR_FormatYUV422PackedSemiPlanar: return "OMX_COLOR_FormatYUV422PackedSemiPlanar"; - case OMX_COLOR_Format18BitBGR666: return "OMX_COLOR_Format18BitBGR666"; - case OMX_COLOR_Format24BitARGB6666: return "OMX_COLOR_Format24BitARGB6666"; - case OMX_COLOR_Format24BitABGR6666: return "OMX_COLOR_Format24BitABGR6666"; - - case OMX_COLOR_FormatAndroidOpaque: return "OMX_COLOR_FormatAndroidOpaque"; - case OMX_TI_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_TI_COLOR_FormatYUV420PackedSemiPlanar"; - case OMX_QCOM_COLOR_FormatYVU420SemiPlanar: return "OMX_QCOM_COLOR_FormatYVU420SemiPlanar"; - case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka"; - case OMX_SEC_COLOR_FormatNV12Tiled: return "OMX_SEC_COLOR_FormatNV12Tiled"; case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m"; - - case QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka: return "QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka"; - case QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka: return "QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka"; - case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView"; - case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed"; case QOMX_COLOR_Format32bitRGBA8888: return "QOMX_COLOR_Format32bitRGBA8888"; - case QOMX_COLOR_Format32bitRGBA8888Compressed: return "QOMX_COLOR_Format32bitRGBA8888Compressed"; - - default: - return "unkn"; + default: return "unkn"; } } // ***** encoder functions ***** -OmxEncoder::OmxEncoder(const char* path, int width, int height, int fps, int bitrate, bool h265, bool downscale) { +OmxEncoder::OmxEncoder(const char* path, int width, int height, int fps, int bitrate) { this->path = path; this->width = width; this->height = height; this->fps = fps; - this->remuxing = !h265; - this->downscale = downscale; - if (this->downscale) { - this->y_ptr2 = (uint8_t *)malloc(this->width*this->height); - this->u_ptr2 = (uint8_t *)malloc(this->width*this->height/4); - this->v_ptr2 = (uint8_t *)malloc(this->width*this->height/4); + OMX_ERRORTYPE err = OMX_Init(); + if (err != OMX_ErrorNone) { + LOGE("OMX_Init failed: %x", err); + return; } - auto component = (OMX_STRING)(h265 ? "OMX.qcom.video.encoder.hevc" : "OMX.qcom.video.encoder.avc"); - int err = OMX_GetHandle(&this->handle, component, this, &omx_callbacks); + OMX_STRING component = (OMX_STRING)("OMX.qcom.video.encoder.avc"); + err = OMX_GetHandle(&handle, component, this, &omx_callbacks); if (err != OMX_ErrorNone) { - LOGE("error getting codec: %x", err); + LOGE("Error getting codec: %x", err); + OMX_Deinit(); + return; } // setup input port - OMX_PARAM_PORTDEFINITIONTYPE in_port = {0}; in_port.nSize = sizeof(in_port); in_port.nPortIndex = (OMX_U32) PORT_INDEX_IN; - OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port)); + OMX_CHECK(OMX_GetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port)); - in_port.format.video.nFrameWidth = this->width; - in_port.format.video.nFrameHeight = this->height; - in_port.format.video.nStride = VENUS_Y_STRIDE(COLOR_FMT_NV12, this->width); - in_port.format.video.nSliceHeight = this->height; - in_port.nBufferSize = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, this->width, this->height); - in_port.format.video.xFramerate = (this->fps * 65536); + in_port.format.video.nFrameWidth = width; + in_port.format.video.nFrameHeight = height; + in_port.format.video.nStride = VENUS_Y_STRIDE(COLOR_FMT_NV12, width); + in_port.format.video.nSliceHeight = height; + in_port.nBufferSize = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, width, height); + in_port.format.video.xFramerate = (fps * 65536); in_port.format.video.eCompressionFormat = OMX_VIDEO_CodingUnused; in_port.format.video.eColorFormat = (OMX_COLOR_FORMATTYPE)QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m; - OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port)); - OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port)); - this->in_buf_headers.resize(in_port.nBufferCountActual); + OMX_CHECK(OMX_SetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port)); + OMX_CHECK(OMX_GetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port)); + in_buf_headers.resize(in_port.nBufferCountActual); // setup output port - - OMX_PARAM_PORTDEFINITIONTYPE out_port = {0}; + OMX_PARAM_PORTDEFINITIONTYPE out_port; + memset(&out_port, 0, sizeof(OMX_PARAM_PORTDEFINITIONTYPE)); out_port.nSize = sizeof(out_port); + out_port.nVersion.s.nVersionMajor = 1; + out_port.nVersion.s.nVersionMinor = 0; + out_port.nVersion.s.nRevision = 0; + out_port.nVersion.s.nStep = 0; out_port.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR)&out_port)); - out_port.format.video.nFrameWidth = this->width; - out_port.format.video.nFrameHeight = this->height; + + OMX_ERRORTYPE error = OMX_GetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR)&out_port); + if (error != OMX_ErrorNone) { + LOGE("Error getting output port parameters: 0x%08x", error); + return; + } + + out_port.format.video.nFrameWidth = width; + out_port.format.video.nFrameHeight = height; out_port.format.video.xFramerate = 0; out_port.format.video.nBitrate = bitrate; - if (h265) { - out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingHEVC; - } else { - out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingAVC; - } + out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingAVC; out_port.format.video.eColorFormat = OMX_COLOR_FormatUnused; - OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port)); + error = OMX_SetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port); + if (error != OMX_ErrorNone) { + LOGE("Error setting output port parameters: 0x%08x", error); + return; + } - OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port)); - this->out_buf_headers.resize(out_port.nBufferCountActual); + error = OMX_GetParameter(handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port); + if (error != OMX_ErrorNone) { + LOGE("Error getting updated output port parameters: 0x%08x", error); + return; + } + + out_buf_headers.resize(out_port.nBufferCountActual); OMX_VIDEO_PARAM_BITRATETYPE bitrate_type = {0}; bitrate_type.nSize = sizeof(bitrate_type); bitrate_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type)); + OMX_CHECK(OMX_GetParameter(handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type)); bitrate_type.eControlRate = OMX_Video_ControlRateVariable; bitrate_type.nTargetBitrate = bitrate; - OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type)); + OMX_CHECK(OMX_SetParameter(handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type)); - if (h265) { - // setup HEVC - #ifndef QCOM2 - OMX_VIDEO_PARAM_HEVCTYPE hevc_type = {0}; - OMX_INDEXTYPE index_type = (OMX_INDEXTYPE) OMX_IndexParamVideoHevc; - #else - OMX_VIDEO_PARAM_PROFILELEVELTYPE hevc_type = {0}; - OMX_INDEXTYPE index_type = OMX_IndexParamVideoProfileLevelCurrent; - #endif - hevc_type.nSize = sizeof(hevc_type); - hevc_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - OMX_CHECK(OMX_GetParameter(this->handle, index_type, (OMX_PTR) &hevc_type)); + // setup h264 + OMX_VIDEO_PARAM_AVCTYPE avc = {0}; + avc.nSize = sizeof(avc); + avc.nPortIndex = (OMX_U32) PORT_INDEX_OUT; + OMX_CHECK(OMX_GetParameter(handle, OMX_IndexParamVideoAvc, &avc)); - hevc_type.eProfile = OMX_VIDEO_HEVCProfileMain; - hevc_type.eLevel = OMX_VIDEO_HEVCHighTierLevel5; + avc.nBFrames = 0; + avc.nPFrames = 15; - OMX_CHECK(OMX_SetParameter(this->handle, index_type, (OMX_PTR) &hevc_type)); - } else { - // setup h264 - OMX_VIDEO_PARAM_AVCTYPE avc = { 0 }; - avc.nSize = sizeof(avc); - avc.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamVideoAvc, &avc)); + avc.eProfile = OMX_VIDEO_AVCProfileHigh; + avc.eLevel = OMX_VIDEO_AVCLevel31; - avc.nBFrames = 0; - avc.nPFrames = 15; + avc.nAllowedPictureTypes |= OMX_VIDEO_PictureTypeB; + avc.eLoopFilterMode = OMX_VIDEO_AVCLoopFilterEnable; - avc.eProfile = OMX_VIDEO_AVCProfileHigh; - avc.eLevel = OMX_VIDEO_AVCLevel31; + avc.nRefFrames = 1; + avc.bUseHadamard = OMX_TRUE; + avc.bEntropyCodingCABAC = OMX_TRUE; + avc.bWeightedPPrediction = OMX_TRUE; + avc.bconstIpred = OMX_TRUE; - avc.nAllowedPictureTypes |= OMX_VIDEO_PictureTypeB; - avc.eLoopFilterMode = OMX_VIDEO_AVCLoopFilterEnable; + OMX_CHECK(OMX_SetParameter(handle, OMX_IndexParamVideoAvc, &avc)); - avc.nRefFrames = 1; - avc.bUseHadamard = OMX_TRUE; - avc.bEntropyCodingCABAC = OMX_TRUE; - avc.bWeightedPPrediction = OMX_TRUE; - avc.bconstIpred = OMX_TRUE; + OMX_CHECK(OMX_SendCommand(handle, OMX_CommandStateSet, OMX_StateIdle, NULL)); - OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamVideoAvc, &avc)); + for (OMX_BUFFERHEADERTYPE* &buf : in_buf_headers) { + OMX_CHECK(OMX_AllocateBuffer(handle, &buf, PORT_INDEX_IN, this, in_port.nBufferSize)); } - OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL)); - - for (auto &buf : this->in_buf_headers) { - OMX_CHECK(OMX_AllocateBuffer(this->handle, &buf, PORT_INDEX_IN, this, - in_port.nBufferSize)); - } - - for (auto &buf : this->out_buf_headers) { - OMX_CHECK(OMX_AllocateBuffer(this->handle, &buf, PORT_INDEX_OUT, this, - out_port.nBufferSize)); + for (OMX_BUFFERHEADERTYPE* &buf : out_buf_headers) { + OMX_CHECK(OMX_AllocateBuffer(handle, &buf, PORT_INDEX_OUT, this, out_port.nBufferSize)); } wait_for_state(OMX_StateIdle); - OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateExecuting, NULL)); + OMX_CHECK(OMX_SendCommand(handle, OMX_CommandStateSet, OMX_StateExecuting, NULL)); wait_for_state(OMX_StateExecuting); // give omx all the output buffers - for (auto &buf : this->out_buf_headers) { - OMX_CHECK(OMX_FillThisBuffer(this->handle, buf)); + for (OMX_BUFFERHEADERTYPE* &buf : out_buf_headers) { + OMX_CHECK(OMX_FillThisBuffer(handle, buf)); } // fill the input free queue - for (auto &buf : this->in_buf_headers) { - this->free_in.push(buf); + for (OMX_BUFFERHEADERTYPE* &buf : in_buf_headers) { + free_in.push(buf); } } -void OmxEncoder::handle_out_buf(OmxEncoder *e, OMX_BUFFERHEADERTYPE *out_buf) { +void OmxEncoder::handle_out_buf(OmxEncoder *encoder, OMX_BUFFERHEADERTYPE *out_buf) { int err; uint8_t *buf_data = out_buf->pBuffer + out_buf->nOffset; if (out_buf->nFlags & OMX_BUFFERFLAG_CODECCONFIG) { - if (e->codec_config_len < out_buf->nFilledLen) { - e->codec_config = (uint8_t *)realloc(e->codec_config, out_buf->nFilledLen); + if (encoder->codec_config_len < out_buf->nFilledLen) { + encoder->codec_config = (uint8_t *)realloc(encoder->codec_config, out_buf->nFilledLen); } - e->codec_config_len = out_buf->nFilledLen; - memcpy(e->codec_config, buf_data, out_buf->nFilledLen); + encoder->codec_config_len = out_buf->nFilledLen; + memcpy(encoder->codec_config, buf_data, out_buf->nFilledLen); #ifdef QCOM2 out_buf->nTimeStamp = 0; #endif } - if (e->of) { - fwrite(buf_data, out_buf->nFilledLen, 1, e->of); + if (encoder->of) { + fwrite(buf_data, out_buf->nFilledLen, 1, encoder->of); } - if (e->remuxing) { - if (!e->wrote_codec_config && e->codec_config_len > 0) { - // extradata will be freed by av_free() in avcodec_free_context() - e->codec_ctx->extradata = (uint8_t*)av_mallocz(e->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE); - e->codec_ctx->extradata_size = e->codec_config_len; - memcpy(e->codec_ctx->extradata, e->codec_config, e->codec_config_len); + if (!encoder->wrote_codec_config && encoder->codec_config_len > 0) { + // extradata will be freed by av_free() in avcodec_free_context() + encoder->out_stream->codecpar->extradata = (uint8_t*)av_mallocz(encoder->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE); + encoder->out_stream->codecpar->extradata_size = encoder->codec_config_len; + memcpy(encoder->out_stream->codecpar->extradata, encoder->codec_config, encoder->codec_config_len); - err = avcodec_parameters_from_context(e->out_stream->codecpar, e->codec_ctx); - assert(err >= 0); - err = avformat_write_header(e->ofmt_ctx, NULL); - assert(err >= 0); + err = avformat_write_header(encoder->ofmt_ctx, NULL); + assert(err >= 0); - e->wrote_codec_config = true; + encoder->wrote_codec_config = true; + } + + if (out_buf->nTimeStamp > 0) { + // input timestamps are in microseconds + AVRational in_timebase = {1, 1000000}; + + AVPacket pkt; + av_init_packet(&pkt); + pkt.data = buf_data; + pkt.size = out_buf->nFilledLen; + + enum AVRounding rnd = static_cast(AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX); + pkt.pts = pkt.dts = av_rescale_q_rnd(out_buf->nTimeStamp, in_timebase, encoder->out_stream->time_base, rnd); + pkt.duration = av_rescale_q(1, AVRational{1, encoder->fps}, encoder->out_stream->time_base); + + if (out_buf->nFlags & OMX_BUFFERFLAG_SYNCFRAME) { + pkt.flags |= AV_PKT_FLAG_KEY; } - if (out_buf->nTimeStamp > 0) { - // input timestamps are in microseconds - AVRational in_timebase = {1, 1000000}; + err = av_write_frame(encoder->ofmt_ctx, &pkt); + if (err < 0) { LOGW("ts encoder write issue"); } - AVPacket pkt; - av_init_packet(&pkt); - pkt.data = buf_data; - pkt.size = out_buf->nFilledLen; - - enum AVRounding rnd = static_cast(AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX); - pkt.pts = pkt.dts = av_rescale_q_rnd(out_buf->nTimeStamp, in_timebase, e->ofmt_ctx->streams[0]->time_base, rnd); - pkt.duration = av_rescale_q(50*1000, in_timebase, e->ofmt_ctx->streams[0]->time_base); - - if (out_buf->nFlags & OMX_BUFFERFLAG_SYNCFRAME) { - pkt.flags |= AV_PKT_FLAG_KEY; - } - - err = av_write_frame(e->ofmt_ctx, &pkt); - if (err < 0) { LOGW("ts encoder write issue"); } - - av_free_packet(&pkt); - } + av_packet_unref(&pkt); } // give omx back the buffer @@ -508,134 +340,186 @@ void OmxEncoder::handle_out_buf(OmxEncoder *e, OMX_BUFFERHEADERTYPE *out_buf) { out_buf->nTimeStamp = 0; } #endif - OMX_CHECK(OMX_FillThisBuffer(e->handle, out_buf)); + OMX_CHECK(OMX_FillThisBuffer(encoder->handle, out_buf)); } int OmxEncoder::encode_frame_rgba(const uint8_t *ptr, int in_width, int in_height, uint64_t ts) { - int err; - if (!this->is_open) { + if (!is_open) { return -1; } - // 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 OMX_BUFFERHEADERTYPE* in_buf = nullptr; - while (!this->free_in.try_pop(in_buf, 20)) { + while (!free_in.try_pop(in_buf, 20)) { if (do_exit) { return -1; } } - int ret = this->counter; + int ret = counter; uint8_t *in_buf_ptr = in_buf->pBuffer; uint8_t *in_y_ptr = in_buf_ptr; - int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, this->width); - int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, this->width); - uint8_t *in_uv_ptr = in_buf_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, this->height)); + int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, width); + int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, width); + uint8_t *in_uv_ptr = in_buf_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, height)); - err = ABGRToNV12(ptr, this->width*4, - in_y_ptr, in_y_stride, - in_uv_ptr, in_uv_stride, - this->width, this->height); + int err = ABGRToNV12(ptr, width * 4, in_y_ptr, in_y_stride, in_uv_ptr, in_uv_stride, width, height); assert(err == 0); - in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, this->width, this->height); + in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, width, height); in_buf->nFlags = OMX_BUFFERFLAG_ENDOFFRAME; in_buf->nOffset = 0; - in_buf->nTimeStamp = ts/1000LL; // OMX_TICKS, in microseconds - this->last_t = in_buf->nTimeStamp; + in_buf->nTimeStamp = ts / 1000LL; // OMX_TICKS, in microseconds + last_t = in_buf->nTimeStamp; - OMX_CHECK(OMX_EmptyThisBuffer(this->handle, in_buf)); + OMX_CHECK(OMX_EmptyThisBuffer(handle, in_buf)); // pump output while (true) { OMX_BUFFERHEADERTYPE *out_buf; - if (!this->done_out.try_pop(out_buf)) { + if (!done_out.try_pop(out_buf)) { break; } handle_out_buf(this, out_buf); } - this->dirty = true; + dirty = true; - this->counter++; + counter++; + + return ret; +} + +// CLEARPILOT: encode raw NV12 frames directly (no RGBA conversion needed) +int OmxEncoder::encode_frame_nv12(const uint8_t *y_ptr, int y_stride, const uint8_t *uv_ptr, int uv_stride, + int in_width, int in_height, uint64_t ts) { + if (!is_open) { + return -1; + } + + OMX_BUFFERHEADERTYPE* in_buf = nullptr; + while (!free_in.try_pop(in_buf, 20)) { + if (do_exit) { + return -1; + } + } + + int ret = counter; + + uint8_t *in_buf_ptr = in_buf->pBuffer; + int venus_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, width); + int venus_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, width); + uint8_t *dst_y = in_buf_ptr; + uint8_t *dst_uv = in_buf_ptr + (venus_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, height)); + + // Copy Y plane row by row (source stride may differ from VENUS stride) + for (int row = 0; row < in_height; row++) { + memcpy(dst_y + row * venus_y_stride, y_ptr + row * y_stride, in_width); + } + // Copy UV plane row by row + int uv_height = in_height / 2; + for (int row = 0; row < uv_height; row++) { + memcpy(dst_uv + row * venus_uv_stride, uv_ptr + row * uv_stride, in_width); + } + + in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, width, height); + in_buf->nFlags = OMX_BUFFERFLAG_ENDOFFRAME; + in_buf->nOffset = 0; + in_buf->nTimeStamp = ts / 1000LL; + last_t = in_buf->nTimeStamp; + + OMX_CHECK(OMX_EmptyThisBuffer(handle, in_buf)); + + while (true) { + OMX_BUFFERHEADERTYPE *out_buf; + if (!done_out.try_pop(out_buf)) { + break; + } + handle_out_buf(this, out_buf); + } + + dirty = true; + counter++; return ret; } void OmxEncoder::encoder_open(const char* filename) { - int err; + if (!filename || strlen(filename) == 0) { + return; + } + + if (strlen(filename) + path.size() + 2 > sizeof(vid_path)) { + return; + } struct stat st = {0}; - if (stat(this->path.c_str(), &st) == -1) { - mkdir(this->path.c_str(), 0755); - } - - snprintf(this->vid_path, sizeof(this->vid_path), "%s/%s", this->path.c_str(), filename); - printf("encoder_open %s remuxing:%d\n", this->vid_path, this->remuxing); - - if (this->remuxing) { - avformat_alloc_output_context2(&this->ofmt_ctx, NULL, NULL, this->vid_path); - assert(this->ofmt_ctx); - - this->out_stream = avformat_new_stream(this->ofmt_ctx, NULL); - assert(this->out_stream); - - // set codec correctly - av_register_all(); - - AVCodec *codec = NULL; - codec = avcodec_find_encoder(AV_CODEC_ID_H264); - assert(codec); - - this->codec_ctx = avcodec_alloc_context3(codec); - assert(this->codec_ctx); - this->codec_ctx->width = this->width; - this->codec_ctx->height = this->height; - this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P; - this->codec_ctx->time_base = (AVRational){ 1, this->fps }; - - err = avio_open(&this->ofmt_ctx->pb, this->vid_path, AVIO_FLAG_WRITE); - assert(err >= 0); - - this->wrote_codec_config = false; - } else { - this->of = fopen(this->vid_path, "wb"); - assert(this->of); -#ifndef QCOM2 - if (this->codec_config_len > 0) { - fwrite(this->codec_config, this->codec_config_len, 1, this->of); + if (stat(path.c_str(), &st) == -1) { + if (mkdir(path.c_str(), 0755) == -1) { + return; } -#endif } - // create camera lock file - snprintf(this->lock_path, sizeof(this->lock_path), "%s/%s.lock", this->path.c_str(), filename); - int lock_fd = HANDLE_EINTR(open(this->lock_path, O_RDWR | O_CREAT, 0664)); - assert(lock_fd >= 0); + snprintf(vid_path, sizeof(vid_path), "%s/%s", path.c_str(), filename); + + if (avformat_alloc_output_context2(&ofmt_ctx, NULL, NULL, vid_path) < 0 || !ofmt_ctx) { + return; + } + + out_stream = avformat_new_stream(ofmt_ctx, NULL); + if (!out_stream) { + avformat_free_context(ofmt_ctx); + ofmt_ctx = nullptr; + return; + } + + out_stream->time_base = AVRational{1, fps}; + + out_stream->codecpar->codec_id = AV_CODEC_ID_H264; + out_stream->codecpar->codec_type = AVMEDIA_TYPE_VIDEO; + out_stream->codecpar->width = width; + out_stream->codecpar->height = height; + + int err = avio_open(&ofmt_ctx->pb, vid_path, AVIO_FLAG_WRITE); + if (err < 0) { + avformat_free_context(ofmt_ctx); + ofmt_ctx = nullptr; + return; + } + + wrote_codec_config = false; + + snprintf(lock_path, sizeof(lock_path), "%s/%s.lock", path.c_str(), filename); + int lock_fd = HANDLE_EINTR(open(lock_path, O_RDWR | O_CREAT, 0664)); + if (lock_fd < 0) { + avio_closep(&ofmt_ctx->pb); + avformat_free_context(ofmt_ctx); + ofmt_ctx = nullptr; + return; + } close(lock_fd); - this->is_open = true; - this->counter = 0; + is_open = true; + counter = 0; } void OmxEncoder::encoder_close() { - if (this->is_open) { - if (this->dirty) { - // drain output only if there could be frames in the encoder + if (!is_open) return; - OMX_BUFFERHEADERTYPE* in_buf = this->free_in.pop(); + if (dirty) { + OMX_BUFFERHEADERTYPE* in_buf = free_in.pop(); + if (in_buf) { in_buf->nFilledLen = 0; in_buf->nOffset = 0; in_buf->nFlags = OMX_BUFFERFLAG_EOS; - in_buf->nTimeStamp = this->last_t + 1000000LL/this->fps; + in_buf->nTimeStamp = last_t + 1000000LL / fps; - OMX_CHECK(OMX_EmptyThisBuffer(this->handle, in_buf)); + OMX_CHECK(OMX_EmptyThisBuffer(handle, in_buf)); while (true) { - OMX_BUFFERHEADERTYPE *out_buf = this->done_out.pop(); + OMX_BUFFERHEADERTYPE *out_buf = done_out.pop(); + if (!out_buf) break; handle_out_buf(this, out_buf); @@ -643,55 +527,112 @@ void OmxEncoder::encoder_close() { break; } } - this->dirty = false; } - - if (this->remuxing) { - av_write_trailer(this->ofmt_ctx); - avcodec_free_context(&this->codec_ctx); - avio_closep(&this->ofmt_ctx->pb); - avformat_free_context(this->ofmt_ctx); - } else { - fclose(this->of); - this->of = nullptr; - } - unlink(this->lock_path); + dirty = false; + } + + if (out_stream) { + out_stream->nb_frames = counter; + out_stream->duration = av_rescale_q(counter, AVRational{1, fps}, out_stream->time_base); + } + + if (ofmt_ctx) { + av_write_trailer(ofmt_ctx); + ofmt_ctx->duration = out_stream ? out_stream->duration : 0; + avio_closep(&ofmt_ctx->pb); + avformat_free_context(ofmt_ctx); + ofmt_ctx = nullptr; + out_stream = nullptr; + } + + if (lock_path[0] != '\0') { + unlink(lock_path); + } + + is_open = false; + + // Remux with faststart for streaming/seeking support + if (strlen(vid_path) > 0) { + char fixed_path[1024]; + snprintf(fixed_path, sizeof(fixed_path), "%s.fixed.mp4", vid_path); + + char cmd[2048]; + snprintf(cmd, sizeof(cmd), "ffmpeg -y -i \"%s\" -c copy -movflags +faststart \"%s\" && mv \"%s\" \"%s\"", + vid_path, fixed_path, fixed_path, vid_path); + + int ret = system(cmd); + if (ret != 0) { + LOGW("ffmpeg faststart remux failed with exit code %d", ret); + } } - this->is_open = false; } OmxEncoder::~OmxEncoder() { - assert(!this->is_open); - - OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL)); - - wait_for_state(OMX_StateIdle); - - OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateLoaded, NULL)); - - for (auto &buf : this->in_buf_headers) { - OMX_CHECK(OMX_FreeBuffer(this->handle, PORT_INDEX_IN, buf)); + if (is_open) { + LOGE("OmxEncoder closed with is_open=true, calling encoder_close()"); + encoder_close(); } - for (auto &buf : this->out_buf_headers) { - OMX_CHECK(OMX_FreeBuffer(this->handle, PORT_INDEX_OUT, buf)); + if (!handle) { + LOGE("OMX handle is null in destructor, skipping teardown."); + return; + } + + OMX_ERRORTYPE err; + + err = OMX_SendCommand(handle, OMX_CommandStateSet, OMX_StateIdle, NULL); + if (err != OMX_ErrorNone) { + LOGE("Failed to set OMX state to Idle: %x", err); + } else { + wait_for_state(OMX_StateIdle); + } + + err = OMX_SendCommand(handle, OMX_CommandStateSet, OMX_StateLoaded, NULL); + if (err != OMX_ErrorNone) { + LOGE("Failed to set OMX state to Loaded: %x", err); + } + + for (OMX_BUFFERHEADERTYPE *buf : in_buf_headers) { + if (buf) { + err = OMX_FreeBuffer(handle, PORT_INDEX_IN, buf); + if (err != OMX_ErrorNone) { + LOGE("Failed to free input buffer: %x", err); + } + } + } + + for (OMX_BUFFERHEADERTYPE *buf : out_buf_headers) { + if (buf) { + err = OMX_FreeBuffer(handle, PORT_INDEX_OUT, buf); + if (err != OMX_ErrorNone) { + LOGE("Failed to free output buffer: %x", err); + } + } } wait_for_state(OMX_StateLoaded); - OMX_CHECK(OMX_FreeHandle(this->handle)); + err = OMX_FreeHandle(handle); + if (err != OMX_ErrorNone) { + LOGE("Failed to free OMX handle: %x", err); + } + + handle = nullptr; + + err = OMX_Deinit(); + if (err != OMX_ErrorNone) { + LOGE("OMX_Deinit failed: %x", err); + } OMX_BUFFERHEADERTYPE *out_buf; - while (this->free_in.try_pop(out_buf)); - while (this->done_out.try_pop(out_buf)); + while (free_in.try_pop(out_buf)); + while (done_out.try_pop(out_buf)); - if (this->codec_config) { - free(this->codec_config); + if (codec_config) { + free(codec_config); + codec_config = nullptr; } - if (this->downscale) { - free(this->y_ptr2); - free(this->u_ptr2); - free(this->v_ptr2); - } + in_buf_headers.clear(); + out_buf_headers.clear(); } diff --git a/selfdrive/frogpilot/screenrecorder/omx_encoder.h b/selfdrive/frogpilot/screenrecorder/omx_encoder.h index 5cb630c..2cabdcd 100755 --- a/selfdrive/frogpilot/screenrecorder/omx_encoder.h +++ b/selfdrive/frogpilot/screenrecorder/omx_encoder.h @@ -12,13 +12,15 @@ extern "C" { #include "common/queue.h" -// OmxEncoder, lossey codec using hardware hevc +// OmxEncoder, lossey codec using hardware H.264 class OmxEncoder { public: - OmxEncoder(const char* path, int width, int height, int fps, int bitrate, bool h265, bool downscale); + OmxEncoder(const char* path, int width, int height, int fps, int bitrate); ~OmxEncoder(); int encode_frame_rgba(const uint8_t *ptr, int in_width, int in_height, uint64_t ts); + int encode_frame_nv12(const uint8_t *y_ptr, int y_stride, const uint8_t *uv_ptr, int uv_stride, + int in_width, int in_height, uint64_t ts); void encoder_open(const char* filename); void encoder_close(); @@ -42,31 +44,26 @@ private: int counter = 0; std::string path; - FILE *of; + FILE *of = nullptr; - size_t codec_config_len; - uint8_t *codec_config = NULL; - bool wrote_codec_config; + size_t codec_config_len = 0; + uint8_t *codec_config = nullptr; + bool wrote_codec_config = false; std::mutex state_lock; std::condition_variable state_cv; OMX_STATETYPE state = OMX_StateLoaded; - OMX_HANDLETYPE handle; + OMX_HANDLETYPE handle = nullptr; std::vector in_buf_headers; std::vector out_buf_headers; - uint64_t last_t; + uint64_t last_t = 0; SafeQueue free_in; SafeQueue done_out; - AVFormatContext *ofmt_ctx; - AVCodecContext *codec_ctx; - AVStream *out_stream; - bool remuxing; - - bool downscale; - uint8_t *y_ptr2, *u_ptr2, *v_ptr2; + AVFormatContext *ofmt_ctx = nullptr; + AVStream *out_stream = nullptr; }; diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 74fd50e..eb8cacc 100755 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -54,7 +54,9 @@ def allow_uploads(started, params, CP: car.CarParams) -> bool: procs = [ DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), - NativeProcess("camerad", "system/camerad", ["./camerad"], driverview), + # CLEARPILOT: camerad runs always (was driverview) so dashcamd can record + # the moment ignition+drive-gear arrives without waiting for camera startup. + NativeProcess("camerad", "system/camerad", ["./camerad"], always_run), NativeProcess("logcatd", "system/logcatd", ["./logcatd"], allow_logging), NativeProcess("proclogd", "system/proclogd", ["./proclogd"], allow_logging), PythonProcess("logmessaged", "system.logmessaged", allow_logging), @@ -89,6 +91,9 @@ procs = [ # CLEARPILOT: speed/cruise overlay daemon. Reads gpsLocation + carState, writes display # params for the onroad UI; asserts ClearpilotPlayDing on speed-limit warning transitions. PythonProcess("speed_logicd", "selfdrive.clearpilot.speed_logicd", only_onroad), + # CLEARPILOT: dashcam — VisionIPC frames → OMX H.264 → 3-min MP4 segments + SRT + # GPS subtitles in /data/media/0/videos/. Manages its own trip lifecycle. + NativeProcess("dashcamd", "selfdrive/clearpilot", ["./dashcamd"], always_run), # PythonProcess("ugpsd", "system.ugpsd", only_onroad, enabled=TICI), #PythonProcess("navd", "selfdrive.navd.navd", only_onroad), PythonProcess("pandad", "selfdrive.boardd.pandad", always_run), diff --git a/system/loggerd/deleter.py b/system/loggerd/deleter.py index 2f0b96c..46fba08 100755 --- a/system/loggerd/deleter.py +++ b/system/loggerd/deleter.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import os import shutil +import sys import threading from openpilot.system.hardware.hw import Paths from openpilot.common.swaglog import cloudlog @@ -8,11 +9,17 @@ from openpilot.system.loggerd.config import get_available_bytes, get_available_p from openpilot.system.loggerd.uploader import listdir_by_creation from openpilot.system.loggerd.xattr_cache import getxattr -MIN_BYTES = 5 * 1024 * 1024 * 1024 +# CLEARPILOT: bumped from 5 GB to 9 GB so dashcam footage has headroom +MIN_BYTES = 9 * 1024 * 1024 * 1024 MIN_PERCENT = 10 DELETE_LAST = ['boot', 'crash'] +# CLEARPILOT: dashcam footage directory (trip dirs YYYYMMDD-HHMMSS/ with .mp4 segments) +VIDEOS_DIR = '/data/media/0/videos' +# CLEARPILOT: max total size for /data/log2 session logs +LOG2_MAX_BYTES = 4 * 1024 * 1024 * 1024 + PRESERVE_ATTR_NAME = 'user.preserve' PRESERVE_ATTR_VALUE = b'1' PRESERVE_COUNT = 5 @@ -44,12 +51,105 @@ def get_preserved_segments(dirs_by_creation: list[str]) -> list[str]: return preserved +def delete_oldest_video(): + """CLEARPILOT: prune dashcam footage when disk space is low. + Trip directories are /data/media/0/videos/YYYYMMDD-HHMMSS/ containing .mp4 + segments. Deletes entire oldest trip directory first. If only one trip + remains (the active one), deletes individual segments oldest-first within + it. Also cleans up legacy flat .mp4 files. + Returns True if something was deleted.""" + try: + if not os.path.isdir(VIDEOS_DIR): + return False + + legacy_files = [] + trip_dirs = [] + for entry in os.listdir(VIDEOS_DIR): + path = os.path.join(VIDEOS_DIR, entry) + if os.path.isfile(path) and entry.endswith('.mp4'): + legacy_files.append(entry) + elif os.path.isdir(path): + trip_dirs.append(entry) + + if legacy_files: + legacy_files.sort() + delete_path = os.path.join(VIDEOS_DIR, legacy_files[0]) + print(f"CLP deleter: deleting legacy video {delete_path}", file=sys.stderr, flush=True) + os.remove(delete_path) + return True + + if not trip_dirs: + return False + + trip_dirs.sort() # timestamp names = chronological order + + if len(trip_dirs) > 1: + delete_path = os.path.join(VIDEOS_DIR, trip_dirs[0]) + print(f"CLP deleter: deleting trip {delete_path}", file=sys.stderr, flush=True) + shutil.rmtree(delete_path) + return True + + # Only one trip left (likely active) — drop its oldest segment + trip_path = os.path.join(VIDEOS_DIR, trip_dirs[0]) + segments = sorted(f for f in os.listdir(trip_path) if f.endswith('.mp4')) + if not segments: + return False + delete_path = os.path.join(trip_path, segments[0]) + print(f"CLP deleter: deleting segment {delete_path}", file=sys.stderr, flush=True) + os.remove(delete_path) + return True + except OSError as e: + print(f"CLP deleter: issue deleting video from {VIDEOS_DIR}: {e}", file=sys.stderr, flush=True) + return False + + +def cleanup_log2(): + """CLEARPILOT: keep /data/log2 session logs under LOG2_MAX_BYTES total. + Deletes oldest dated session directories first (the 'current' symlink/dir + is preserved). Runs even when disk space is fine.""" + log_base = "/data/log2" + if not os.path.isdir(log_base): + return + dirs = [] + for entry in sorted(os.listdir(log_base)): + if entry == "current": + continue + path = os.path.join(log_base, entry) + if os.path.isdir(path) and not os.path.islink(path): + try: + size = sum(f.stat().st_size for f in os.scandir(path) if f.is_file()) + except OSError: + size = 0 + dirs.append((entry, path, size)) + total = sum(s for _, _, s in dirs) + current = os.path.join(log_base, "current") + if os.path.isdir(current): + try: + total += sum(f.stat().st_size for f in os.scandir(current) if f.is_file()) + except OSError: + pass + while total > LOG2_MAX_BYTES and dirs: + entry, path, size = dirs.pop(0) + try: + print(f"CLP deleter: deleting log session {path} ({size // 1024 // 1024} MB)", + file=sys.stderr, flush=True) + shutil.rmtree(path) + total -= size + except OSError as e: + print(f"CLP deleter: issue deleting log {path}: {e}", file=sys.stderr, flush=True) + + def deleter_thread(exit_event): while not exit_event.is_set(): out_of_bytes = get_available_bytes(default=MIN_BYTES + 1) < MIN_BYTES out_of_percent = get_available_percent(default=MIN_PERCENT + 1) < MIN_PERCENT if out_of_percent or out_of_bytes: + # CLEARPILOT: drop oldest dashcam footage first, fall back to log segments + if delete_oldest_video(): + exit_event.wait(.1) + continue + dirs = listdir_by_creation(Paths.log_root()) # skip deleting most recent N preserved segments (and their prior segment) @@ -73,6 +173,8 @@ def deleter_thread(exit_event): cloudlog.exception(f"issue deleting {delete_path}") exit_event.wait(.1) else: + # CLEARPILOT: keep /data/log2 quota even when disk space is fine + cleanup_log2() exit_event.wait(30)