clearpilot/selfdrive/clearpilot/dashcamd.cc

/*
 * 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 <cstdio>
#include <ctime>
#include <string>
#include <errno.h>
#include <signal.h>
#include <sched.h>
#include <execinfo.h>
#include <sys/stat.h>
#include <sys/resource.h>
#include <unistd.h>

#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;
}