CameraCapture.cpp

#include "CameraCapture.h"

#include <driver/isp.h>
#include <esp_cam_ctlr.h>
#include <esp_cam_ctlr_csi.h>
#include <esp_heap_caps.h>
#include <esp_ldo_regulator.h>

#include "CameraSensor.h"
#include "Config.h"

namespace {

// 受け取った絵（PSRAM上）を2つ持ちます。
//
// 1つでは足りません。1枚受け取り終えると、ドライバはその場で次の書き込み先を
// 求めてきます。同じ領域を渡すと、こちらが中身を読んでいる最中に次の絵で
// 上書きされます。
//
// 「読み終わるまで渡さない」ことはできません。予備の領域を持たせない設定
// （bk_buffer_dis = true）では、渡すものが無いとドライバが異常終了します
// （esp_cam_ctlr_csi.c の "no new buffer, and no driver internal buffer"）。
uint8_t *g_frame[2] = {nullptr, nullptr};
size_t g_frameLen = 0;
int g_average = -1;
uint8_t g_min = 0;
uint8_t g_max = 0;
size_t g_receivedSize = 0;
String g_lastError;

// 今の露出（1/16行）とゲインの段数。前回合わせた値から始めると、次はすぐ合います。
uint32_t g_exposure = CAMERA_FIXED_EXPOSURE;
int g_gainIndex = CAMERA_GAIN_INDEX_MIN;

// 今の感度＝露出×ゲイン倍率。光の量をこれで割ると、部屋の明るさの目安になります。
uint64_t sensitivity() {
  return uint64_t(g_exposure) * CameraSensor::gainMilliForIndex(g_gainIndex) / 1000;
}

uint64_t maxSensitivity() {
  return uint64_t(CAMERA_EXPOSURE_MAX) *
         CameraSensor::gainMilliForIndex(CAMERA_GAIN_INDEX_MAX) / 1000;
}

// 明るさから黒レベルを引いた「光の量」。光が無ければ0です。
int signalOf(int average) {
  if (average < 0) return -1;
  int signal = average - CAMERA_BLACK_LEVEL;
  return signal > 0 ? signal : 0;
}

// 目標の感度を、露出とゲインに割り振ります。
//
// 露出を先に伸ばし、足りない分をゲインで補います。増幅はノイズも一緒に
// 増やすので、使わずに済むなら使わない方が素直な値になります。
void applySensitivity(uint64_t want) {
  if (want < CAMERA_EXPOSURE_MIN) want = CAMERA_EXPOSURE_MIN;
  uint32_t exposure = (want > CAMERA_EXPOSURE_MAX) ? uint32_t(CAMERA_EXPOSURE_MAX)
                                                   : uint32_t(want);
  uint32_t gainMilli = uint32_t(want * 1000 / exposure);
  int gainIndex = CameraSensor::gainIndexForMilli(gainMilli);
  g_exposure = exposure;
  g_gainIndex = gainIndex;
  CameraSensor::setExposure(g_exposure);
  CameraSensor::setGainIndex(g_gainIndex);
}

// 割り込みから書かれるのでvolatileにします。
volatile uint32_t g_frameCount = 0;  // 受け取り終えた枚数
volatile size_t g_gotSize = 0;       // 直前に受け取れた量
volatile int g_doneIndex = 1;        // 直前に受け取り終えた領域（読んでよい方）
uint32_t g_seenCount = 0;            // こちらが見終えた枚数

// 受信の準備ができたときに呼ばれ、書き込み先を教えます。
// 割り込みから呼ばれるので、ここでは何も確保せず、用意済みの領域を渡すだけです。
//
// 直前に受け取り終えた方とは逆の領域を渡します。これで、こちらが読んでいる
// 領域が上書きされません。
//
// 戻り値は「渡す領域があるか」です。ヘッダには「高優先度のタスクを起こしたか」
// と書かれていますが、CSIのドライバはfalseを「渡すものが無い」と解釈します。
// falseを返すと異常終了するので、常にtrueを返します。
bool IRAM_ATTR onGetNewTrans(esp_cam_ctlr_handle_t handle, esp_cam_ctlr_trans_t *trans,
                             void *user_data) {
  int fill = 1 - g_doneIndex;
  trans->buffer = g_frame[fill];
  trans->buflen = g_frameLen;
  return true;
}

// 1枚受け取り終えたときに呼ばれます。受け取れた量とどちらの領域かを控えて、
// 待っている側に知らせます。
//
// 受け取りはこのコールバックだけで行い、esp_cam_ctlr_receive()は使いません。
// Espressifのesp_videoも同じ作りです（esp_video_csi_device.c）。
bool IRAM_ATTR onTransFinished(esp_cam_ctlr_handle_t handle, esp_cam_ctlr_trans_t *trans,
                               void *user_data) {
  g_gotSize = trans->received_size;
  g_doneIndex = (trans->buffer == g_frame[0]) ? 0 : 1;
  g_frameCount++;
  return true;
}

// 絵の明るさを求めます。
//
// 全画素を見る必要はありません。ただし拾い方が大事です。飛び飛びに1画素ずつ
// 拾うと、そのたびにキャッシュを外してPSRAMを読みに行くことになり、1枚あたり
// 11ミリ秒かかります。1秒ごとに呼ぶ処理としては長すぎます。
//
// キャッシュ1行を選んだら、その中は続けて読みます。読み込みの回数が64分の1に
// なり、拾える画素はむしろ増えます。
//
// RGB565は1画素2バイトで、上位から赤5ビット・緑6ビット・青5ビットです。
// 明るさは緑を重く見た近似（緑の比率が最も高い）で求めます。
int computeAverage(const uint8_t *buf, size_t len, uint8_t *outMin, uint8_t *outMax) {
  if (!buf || len < CAMERA_CACHE_LINE_BYTES) return -1;
  uint64_t sum = 0;
  size_t count = 0;
  uint8_t lo = 255, hi = 0;
  const size_t lineBytes = CAMERA_CACHE_LINE_BYTES;
  const size_t stride = lineBytes * CAMERA_SAMPLE_LINE_SKIP;
  for (size_t base = 0; base + lineBytes <= len; base += stride) {
    for (size_t i = base; i < base + lineBytes; i += CAMERA_BYTES_PER_PIXEL) {
      uint16_t px = uint16_t(buf[i]) | (uint16_t(buf[i + 1]) << 8);
      uint8_t r = (px >> 11) & 0x1F;
      uint8_t g = (px >> 5) & 0x3F;
      uint8_t b = px & 0x1F;
      // 5ビット・6ビットのまま重みを付け、最後に0〜255へ揃えます。
      uint8_t v = uint8_t((r * 4 + g * 5 + b * 1) * 255 / (31 * 4 + 63 * 5 + 31 * 1));
      sum += v;
      if (v < lo) lo = v;
      if (v > hi) hi = v;
      count++;
    }
  }
  if (outMin) *outMin = lo;
  if (outMax) *outMax = hi;
  return count ? int(sum / count) : -1;
}

// 1枚届くのを待ちます。届いたらその大きさを返します。
//
// 待っている間は本体の処理が止まるので、使うのは調査用の機能だけです。
// 明るさの測定は待たない窓口（frameReady/measureFrame）を使います。
bool grabFrame(size_t *outSize) {
  g_seenCount = g_frameCount;
  uint32_t started = millis();
  while (g_frameCount == g_seenCount && (millis() - started) < CAMERA_RECEIVE_TIMEOUT_MS) {
    delay(5);
  }
  g_seenCount = g_frameCount;
  size_t got = g_gotSize;
  if (got == 0) return false;
  if (outSize) *outSize = got;
  return true;
}

// 取り込みに必要なものをまとめて用意します。
//
// 電源・センサ・受け皿・CSI・ISPをこの順に整え、最後にセンサの出力を始めます。
// 逆にすると、行き先が無いまま映像が流れます。
//
// 途中で失敗したら、そこまでに用意したものを片付けてから戻ります。
//
// 一度開いたら開いたままにします。1秒ごとに測るために毎回開き直すと、130件の
// レジスタ書き込みが毎回発生し、本体の処理もそのぶん長く止まるためです。
struct Pipeline {
  esp_ldo_channel_handle_t ldo = nullptr;
  esp_cam_ctlr_handle_t ctlr = nullptr;
  isp_proc_handle_t isp = nullptr;
  bool ispReady = false;
  bool streaming = false;
  bool open = false;
};

Pipeline g_pipe;

void closePipeline(Pipeline &p) {
  if (p.streaming) CameraSensor::stopStream();
  if (p.ispReady) {
    esp_isp_disable(p.isp);
    esp_isp_del_processor(p.isp);
  }
  if (p.ctlr) {
    esp_cam_ctlr_stop(p.ctlr);
    esp_cam_ctlr_disable(p.ctlr);
    esp_cam_ctlr_del(p.ctlr);
  }
  if (p.ldo) esp_ldo_release_channel(p.ldo);
  p = Pipeline();
}

bool openPipeline(Pipeline &p) {
  if (p.open) return true;
  g_frameCount = 0;
  g_seenCount = 0;
  g_gotSize = 0;
  g_doneIndex = 1;

  // MIPIの物理層に電源を供給します。
  //
  // 画面（DSI）とカメラ（CSI）で同じLDOを共有しており、画面の側はM5GFXが
  // 確保済みですが、こちらでも確保します。確保は数えられているので二重でも
  // 構いません。これが無いと、エラーも出ないまま1バイトも届きません。
  esp_ldo_channel_config_t ldoCfg = {};
  ldoCfg.chan_id = CAMERA_LDO_CHANNEL;
  ldoCfg.voltage_mv = CAMERA_LDO_VOLTAGE_MV;
  if (esp_ldo_acquire_channel(&ldoCfg, &p.ldo) != ESP_OK) {
    g_lastError = "MIPIの電源を確保できません";
    return false;
  }

  // センサは設定するだけで、出力はまだ始めさせません。受け取る側を先に
  // 用意します。
  if (!CameraSensor::begin()) {
    g_lastError = "センサを設定できません";
    closePipeline(p);
    return false;
  }
  // 前回合わせた露出とゲインから始めます。部屋の明るさは急には変わらないので、
  // たいてい1枚目で目標に入ります。
  CameraSensor::setExposure(g_exposure);
  CameraSensor::setGainIndex(g_gainIndex);

  // ISPで色を解かせるので、届くのはRGB565（1画素2バイト）です。
  //
  // 先頭番地も大きさも、キャッシュ1行分（CAMERA_CACHE_LINE_BYTES）の倍数で
  // なければなりません。DMAが直接書き込んだ内容をCPUから読むために、ドライバが
  // キャッシュを同期させるからです。端数があると同期の要求が拒否されます。
  size_t needed = size_t(CAMERA_WIDTH) * CAMERA_HEIGHT * CAMERA_BYTES_PER_PIXEL;
  needed = (needed + CAMERA_CACHE_LINE_BYTES - 1) / CAMERA_CACHE_LINE_BYTES *
           CAMERA_CACHE_LINE_BYTES;
  if (g_frame[0] && g_frameLen != needed) CameraCapture::release();
  for (int i = 0; i < 2; i++) {
    if (g_frame[i]) continue;
    // 絵は大きいので内蔵RAMには置けません。PSRAMから確保します。
    // 2つで約3.7MBです。
    g_frame[i] = (uint8_t *)heap_caps_aligned_alloc(CAMERA_CACHE_LINE_BYTES, needed,
                                                    MALLOC_CAP_SPIRAM);
    if (!g_frame[i]) {
      g_lastError = "PSRAMを確保できません";
      closePipeline(p);
      return false;
    }
  }
  g_frameLen = needed;

  esp_cam_ctlr_csi_config_t cfg = {};
  cfg.ctlr_id = 0;
  cfg.h_res = CAMERA_WIDTH;
  cfg.v_res = CAMERA_HEIGHT;
  cfg.data_lane_num = CAMERA_CSI_LANES;
  cfg.lane_bit_rate_mbps = CAMERA_CSI_MBPS;
  cfg.input_data_color_type = CAM_CTLR_COLOR_RAW8;
  cfg.output_data_color_type = CAM_CTLR_COLOR_RGB565;
  cfg.queue_items = 1;
  cfg.byte_swap_en = false;
  cfg.bk_buffer_dis = true;  // 予備の領域は持たせません。1枚取れれば十分です

  if (esp_cam_new_csi_ctlr(&cfg, &p.ctlr) != ESP_OK) {
    g_lastError = "CSIを構成できません";
    closePipeline(p);
    return false;
  }

  // ISP（画像処理部）を用意します。
  //
  // CSIから来たデータは必ずISPを通ってからメモリに書かれます。ISPを構成
  // しないと、エラーも出ないまま1バイトも届きません。
  //
  // ここではRAW8を色に解かせてRGB565で受け取ります。Tab5のデモも同じ経路です。
  esp_isp_processor_cfg_t ispCfg = {};
  ispCfg.clk_src = ISP_CLK_SRC_DEFAULT;
  ispCfg.clk_hz = CAMERA_ISP_CLK_HZ;
  ispCfg.input_data_source = ISP_INPUT_DATA_SOURCE_CSI;
  ispCfg.input_data_color_type = ISP_COLOR_RAW8;
  ispCfg.output_data_color_type = ISP_COLOR_RGB565;
  ispCfg.h_res = CAMERA_WIDTH;
  ispCfg.v_res = CAMERA_HEIGHT;
  // ベイヤー配列の並び順。SC202CSはBGGRです（Espressifのドライバの定義）。
  ispCfg.bayer_order = COLOR_RAW_ELEMENT_ORDER_BGGR;
  ispCfg.has_line_start_packet = false;
  ispCfg.has_line_end_packet = false;

  esp_cam_ctlr_evt_cbs_t cbs = {};
  cbs.on_get_new_trans = onGetNewTrans;
  cbs.on_trans_finished = onTransFinished;

  bool ok = esp_cam_ctlr_register_event_callbacks(p.ctlr, &cbs, nullptr) == ESP_OK;
  ok = ok && esp_cam_ctlr_enable(p.ctlr) == ESP_OK;
  ok = ok && esp_cam_ctlr_start(p.ctlr) == ESP_OK;
  if (!ok) {
    g_lastError = "CSIを開始できません";
    closePipeline(p);
    return false;
  }

  if (esp_isp_new_processor(&ispCfg, &p.isp) != ESP_OK || esp_isp_enable(p.isp) != ESP_OK) {
    g_lastError = "ISPを開始できません";
    closePipeline(p);
    return false;
  }
  p.ispReady = true;

  p.streaming = CameraSensor::startStream();
  if (!p.streaming) {
    g_lastError = "センサの出力を開始できません";
    closePipeline(p);
    return false;
  }
  // 出力の指示が本当に入ったかを、レジスタを読み戻して確かめます。
  CameraSensor::status();

  // 出力開始の直後の1枚は整う前のものなので捨てます。
  grabFrame(nullptr);
  p.open = true;
  return true;
}

// 測った明るさを控えます。
void storeMeasurement(int avg, uint8_t lo, uint8_t hi, size_t got) {
  g_average = avg;
  g_min = lo;
  g_max = hi;
  g_receivedSize = got;
}

// 今の測定結果をもとに、感度を合わせ直します。
//
// 目標に入っていればtrueを返します（合わせ直す必要が無い、という意味です）。
// falseなら感度を変えたので、次の1枚は捨ててから測り直してください。
bool adjustSensitivity(int avg, uint8_t lo, uint8_t hi) {
  int signal = signalOf(avg);
  uint64_t before = sensitivity();
  uint64_t want;
  if (signal <= 0) {
    // 光が拾えていません。比では動かせないので、一気に上限まで上げます。
    want = maxSensitivity();
  } else {
    int diff = signal - CAMERA_TARGET_SIGNAL;
    if (diff < 0) diff = -diff;
    if (diff <= CAMERA_SIGNAL_TOLERANCE) return true;
    // 光の量は感度におおむね比例するので、比で掛け直します。
    //
    // ただし飽和していると、比の分母が本当の明るさより小さく出るので
    // 当てになりません。どれだけ超えているか分からないため、下げ幅を
    // 決め打ちにします。
    //
    //   ・端まで真っ白（最小値まで飽和）… どれだけ超えているか見当が
    //     付かないので、8分の1へ大きく下げます
    //   ・一部だけ飽和 … 半分にします
    //
    // 半分ずつしか下げないと、暗い部屋の設定から明るい場所へ移ったときに
    // 回数が足りず、飽和したまま終わります。
    if (lo >= 250) {
      want = before / 8;
    } else if (hi >= 250) {
      want = before / 2;
    } else {
      want = before * CAMERA_TARGET_SIGNAL / uint32_t(signal);
    }
  }
  applySensitivity(want);
  return sensitivity() == before;  // 動かせなかったなら、そこが限界です
}

}  // namespace

namespace CameraCapture {

bool captureOnce() {
  g_lastError = "";
  g_average = -1;
  g_receivedSize = 0;

  // 既に開いていればそのまま使い、こちらで開いた場合だけ閉じます。
  bool opened = !g_pipe.open;
  if (!openPipeline(g_pipe)) return false;

  bool ok = false;
  {
    // 感度（露出×ゲイン）を合わせながら、何回か測ります。
    //
    // 飽和（255に張り付く）も黒つぶれも、明るさの目安として使えません。
    // 目標に近づくまで感度を掛け直します。
    //
    // 感度を変えた直後の1枚は、変える前の設定で撮られたものが出てきます。
    // それを測ると1つ前の設定の結果を今の設定の結果と取り違えるので、
    // 変えるたびに1枚捨てます。捨てずに測って、実際に取り違えました。
    for (int i = 0; i < CAMERA_AE_MAX_FRAMES; i++) {
      uint8_t lo = 0, hi = 0;
      size_t got = 0;
      if (!grabFrame(&got)) {
        g_lastError = "映像が届きません";
        break;
      }
      // 受け取った分だけを見ます。
      //
      // 受信量が0のときに領域全体を見てはいけません。確保したばかりのPSRAMには
      // 前の内容が残っており、0x55や0xAAが並んでいます。それを平均すると
      // それらしい数字が出るので、絵が取れたと読み違えます。
      int avg = computeAverage(g_frame[g_doneIndex], got, &lo, &hi);
      storeMeasurement(avg, lo, hi, got);
      ok = true;
      Serial.printf(
          "[CAM] %d回目 露出=%u ゲイン=%u/1000 明るさ 平均=%d 最小=%u 最大=%u 光=%d 指標=%ld\n",
          i + 1, unsigned(g_exposure),
          unsigned(CameraSensor::gainMilliForIndex(g_gainIndex)), avg, unsigned(lo),
          unsigned(hi), signalOf(avg), lightValue());

      if (adjustSensitivity(avg, lo, hi)) break;
      grabFrame(nullptr);  // 変える前の設定で撮られた1枚を捨てます
    }
    // 最後に効いていた露出を、レジスタから読み戻して確かめます。
    // 書いた値がそのまま入っているかを見るためです。
    CameraSensor::status();
  }

  if (opened) closePipeline(g_pipe);

  Serial.printf("[CAM] 取り込み %s 明るさ=%d 光=%d 指標=%ld\n", ok ? "成功" : "失敗",
                g_average, signalOf(g_average), lightValue());
  return ok;
}

// 露出を段階的に変えて、明るさがどう動くかを表にします。
//
// 露出が上限付近で本当に効いているかを確かめるための調査用です。ゲインは
// 固定するので、変わるのは露出だけになります。
//
// 明るい部屋では飽和し、真っ暗では信号がゼロなので、どちらでも判定できません。
// うっすら明かりのある状態で行ってください。
bool sweepExposure() {
  g_lastError = "";

  bool opened = !g_pipe.open;
  if (!openPipeline(g_pipe)) return false;

  // 測定で使ってよい上限（CAMERA_EXPOSURE_MAX）を超える値も試します。
  // 効かなくなる境目がどこかを知るための調査だからです。
  static const uint32_t kSteps[] = {4096,  8192,  16384, 24576, 32768,
                                    40960, 49152, 57344, 65535};
  Serial.printf("[CAM] 露出の掃引を始めます（ゲイン=%u/1000 固定）\n",
                unsigned(CameraSensor::gainMilliForIndex(g_gainIndex)));
  bool ok = false;
  for (uint32_t step : kSteps) {
    CameraSensor::setExposure(step);
    grabFrame(nullptr);  // 変える前の設定で撮られた1枚を捨てます
    uint8_t lo = 0, hi = 0;
    size_t got = 0;
    if (!grabFrame(&got)) {
      g_lastError = "映像が届きません";
      break;
    }
    int avg = computeAverage(g_frame[g_doneIndex], got, &lo, &hi);
    Serial.printf("[CAM] 掃引 露出=%u 平均=%d 最小=%u 最大=%u 光=%d\n", unsigned(step), avg,
                  unsigned(lo), unsigned(hi), signalOf(avg));
    ok = true;
  }
  // 掃引で動かした露出を、記録に使っている値へ戻します。
  CameraSensor::setExposure(g_exposure);
  if (opened) closePipeline(g_pipe);
  Serial.println("[CAM] 露出の掃引を終えました");
  return ok;
}

bool beginSession() {
  g_lastError = "";
  return openPipeline(g_pipe);
}

void endSession() { closePipeline(g_pipe); }

bool sessionOpen() { return g_pipe.open; }

// まだ見ていない絵が届いているか。
bool frameReady() { return g_frameCount != g_seenCount; }

bool measureFrame() {
  size_t got = g_gotSize;
  if (!frameReady() || got == 0) return false;
  g_seenCount = g_frameCount;
  uint8_t lo = 0, hi = 0;
  int avg = computeAverage(g_frame[g_doneIndex], got, &lo, &hi);
  storeMeasurement(avg, lo, hi, got);
  return true;
}

bool settleStep() { return adjustSensitivity(g_average, g_min, g_max); }

// 届いている絵を、見たことにします。
//
// 受け取りは止められません。カメラは開いている間ずっと絵を送り続けており、
// こちらはそのうち必要なものだけを見ます。
void releaseFrame() { g_seenCount = g_frameCount; }

int averageBrightness() { return g_average; }

uint32_t exposureValue() { return g_exposure; }

int gainIndex() { return g_gainIndex; }

// 光の量を感度（露出×ゲイン）で割った値。これが部屋の明るさの目安になります。
//
// 露出やゲインを変えれば絵の明るさも変わるので、平均だけでは比べられません。
// 感度で割ると、どの設定で測っても同じ明るさの部屋なら同じ値になります。
//
// 割る前に黒レベルを引いているので、光が無ければ0になります。
//
// 単位はまだ決めていません。実際の部屋で値を見てから、照度（ルクス）との
// 対応を決めます。
long lightValue() {
  uint64_t s = sensitivity();
  if (g_average < 0 || s == 0) return -1;
  return long(uint64_t(signalOf(g_average)) * 100000 / s);
}

bool hasFrame() { return g_frame[g_doneIndex] != nullptr && g_receivedSize > 0; }

// 直前に受け取り終えた方の領域を返します。
//
// 明るさの測定を動かしたままブラウザから取り出すと、次の絵が書き込まれている
// 最中の領域を読むことがあり、絵が途中でずれます。落ち着いて見たいときは、
// 設定画面の「カメラを止める」で測定を止めてから取り出してください。
const uint8_t *frameData() { return g_frame[g_doneIndex]; }

size_t frameLength() { return g_receivedSize; }

int frameWidth() { return CAMERA_WIDTH; }

int frameHeight() { return CAMERA_HEIGHT; }

void release() {
  for (int i = 0; i < 2; i++) {
    if (!g_frame[i]) continue;
    heap_caps_free(g_frame[i]);
    g_frame[i] = nullptr;
  }
  g_frameLen = 0;
  g_average = -1;
  g_receivedSize = 0;
}

String status() {
  if (g_lastError.length() > 0) return "取り込み失敗: " + g_lastError;
  if (g_average < 0) return "まだ取り込んでいません";
  char buf[200];
  // 最小と最大を並べるのは、絵が書き込まれたかを見分けるためです。
  // 両方が同じ値なら、平均に意味はありません。
  snprintf(buf, sizeof(buf),
           "取り込み成功: %dx%d\n明るさ 平均%d 最小%u 最大%u 光%d\n露出%u ゲイン%u/1000 明るさ指標%ld",
           CAMERA_WIDTH, CAMERA_HEIGHT, g_average, unsigned(g_min), unsigned(g_max),
           signalOf(g_average), unsigned(g_exposure),
           unsigned(CameraSensor::gainMilliForIndex(g_gainIndex)), lightValue());
  return String(buf);
}

}  // namespace CameraCapture