CameraSensor.cpp

#include "CameraSensor.h"

#include <M5Unified.h>

#include "Config.h"

namespace {

// センサに書き込む設定の1行分です。
struct RegValue {
  uint16_t reg;
  uint8_t val;
};

// 1280x720・RAW8・30fps・MIPI 1レーン・入力クロック24MHz 用の初期化設定です。
//
// EspressifのSC202CS用ドライバ（Apache-2.0）に含まれる同名の設定表を書き
// 写したものです。個々の値の意味は公開されていないので、変更してはいけません。
//
// この解像度なのは、用意されている設定表の中で最も小さいためです。明るさを
// 求めるだけなら画素数は要りませんが、別の解像度にするにはMIPIの転送速度や
// タイミングを計算し直すことになり、間違えたときに原因を追えなくなります。
// 受け取った絵から間引いて使えば、大きさは問題になりません。
const RegValue kInitTable[] = {
  {0x0103, 0x01},
  {0x0100, 0x00},
  {0x36e9, 0x80},
  {0x36ea, 0x06},
  {0x36eb, 0x0a},
  {0x36ec, 0x01},
  {0x36ed, 0x18},
  {0x36e9, 0x24},
  {0x301f, 0x18},
  {0x3031, 0x08},
  {0x3037, 0x00},
  {0x3200, 0x00},
  {0x3201, 0xa0},
  {0x3202, 0x00},
  {0x3203, 0xf0},
  {0x3204, 0x05},
  {0x3205, 0xa7},
  {0x3206, 0x03},
  {0x3207, 0xc7},
  {0x3208, 0x05},
  {0x3209, 0x00},
  {0x320a, 0x02},
  {0x320b, 0xd0},
  {0x3210, 0x00},
  {0x3211, 0x04},
  {0x3212, 0x00},
  {0x3213, 0x04},
  {0x3301, 0xff},
  {0x3304, 0x68},
  {0x3306, 0x40},
  {0x3308, 0x08},
  {0x3309, 0xa8},
  {0x330b, 0xd0},
  {0x330c, 0x18},
  {0x330d, 0xff},
  {0x330e, 0x20},
  {0x331e, 0x59},
  {0x331f, 0x99},
  {0x3333, 0x10},
  {0x335e, 0x06},
  {0x335f, 0x08},
  {0x3364, 0x1f},
  {0x337c, 0x02},
  {0x337d, 0x0a},
  {0x338f, 0xa0},
  {0x3390, 0x01},
  {0x3391, 0x03},
  {0x3392, 0x1f},
  {0x3393, 0xff},
  {0x3394, 0xff},
  {0x3395, 0xff},
  {0x33a2, 0x04},
  {0x33ad, 0x0c},
  {0x33b1, 0x20},
  {0x33b3, 0x38},
  {0x33f9, 0x40},
  {0x33fb, 0x48},
  {0x33fc, 0x0f},
  {0x33fd, 0x1f},
  {0x349f, 0x03},
  {0x34a6, 0x03},
  {0x34a7, 0x1f},
  {0x34a8, 0x38},
  {0x34a9, 0x30},
  {0x34ab, 0xd0},
  {0x34ad, 0xd8},
  {0x34f8, 0x1f},
  {0x34f9, 0x20},
  {0x3630, 0xa0},
  {0x3631, 0x92},
  {0x3632, 0x64},
  {0x3633, 0x43},
  {0x3637, 0x49},
  {0x363a, 0x85},
  {0x363c, 0x0f},
  {0x3650, 0x31},
  {0x3670, 0x0d},
  {0x3674, 0xc0},
  {0x3675, 0xa0},
  {0x3676, 0xa0},
  {0x3677, 0x92},
  {0x3678, 0x96},
  {0x3679, 0x9a},
  {0x367c, 0x03},
  {0x367d, 0x0f},
  {0x367e, 0x01},
  {0x367f, 0x0f},
  {0x3698, 0x83},
  {0x3699, 0x86},
  {0x369a, 0x8c},
  {0x369b, 0x94},
  {0x36a2, 0x01},
  {0x36a3, 0x03},
  {0x36a4, 0x07},
  {0x36ae, 0x0f},
  {0x36af, 0x1f},
  {0x36bd, 0x22},
  {0x36be, 0x22},
  {0x36bf, 0x22},
  {0x36d0, 0x01},
  {0x370f, 0x02},
  {0x3721, 0x6c},
  {0x3722, 0x8d},
  {0x3725, 0xc5},
  {0x3727, 0x14},
  {0x3728, 0x04},
  {0x37b7, 0x04},
  {0x37b8, 0x04},
  {0x37b9, 0x06},
  {0x37bd, 0x07},
  {0x37be, 0x0f},
  {0x3901, 0x02},
  {0x3903, 0x40},
  {0x3905, 0x8d},
  {0x3907, 0x00},
  {0x3908, 0x41},
  {0x391f, 0x41},
  {0x3933, 0x80},
  {0x3934, 0x02},
  {0x3937, 0x6f},
  {0x393a, 0x01},
  {0x393d, 0x01},
  {0x393e, 0xc0},
  {0x39dd, 0x41},
  {0x3e00, 0x00},
  {0x3e01, 0x3d},
  {0x3e02, 0xc0},
  {0x3e09, 0x00},
  {0x4509, 0x28},
  {0x450d, 0x61},
};
constexpr size_t kInitTableLen = sizeof(kInitTable) / sizeof(kInitTable[0]);

bool g_running = false;    // 初期化の設定を書き終えた
bool g_streaming = false;  // 映像を出力している

// ゲインの表を32段ごとに区切ったときの、区切りごとの設定と倍率。
// EspressifのSC202CS用ドライバの表（192段）から読み取ったものです。
const uint8_t kAnalogRegister[6] = {0x00, 0x01, 0x03, 0x07, 0x0f, 0x0f};
const uint8_t kCoarseRegister[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x01};
const uint32_t kAnalogMultiplier[6] = {1, 2, 4, 8, 16, 16};
const uint32_t kCoarseMultiplier[6] = {1, 1, 1, 1, 1, 2};

// レジスタ番地は16ビットです。書き込みは「番地を上位・下位の順に送ってから
// 値を1バイト送る」という形になります。
bool writeReg(uint16_t reg, uint8_t val) {
  uint8_t buf[3] = {uint8_t(reg >> 8), uint8_t(reg & 0xFF), val};
  auto &i2c = M5.In_I2C;
  if (!i2c.start(CAMERA_SCCB_ADDR, false, CAMERA_SCCB_FREQ)) return false;
  bool ok = i2c.write(buf, 3);
  i2c.stop();
  return ok;
}

bool readReg(uint16_t reg, uint8_t *out) {
  auto &i2c = M5.In_I2C;
  uint8_t addr[2] = {uint8_t(reg >> 8), uint8_t(reg & 0xFF)};
  if (!i2c.start(CAMERA_SCCB_ADDR, false, CAMERA_SCCB_FREQ)) return false;
  if (!i2c.write(addr, 2)) {
    i2c.stop();
    return false;
  }
  if (!i2c.restart(CAMERA_SCCB_ADDR, true, CAMERA_SCCB_FREQ)) {
    i2c.stop();
    return false;
  }
  bool ok = i2c.read(out, 1, true);
  i2c.stop();
  return ok;
}

// 露出を書き込みます。
//
// 3つのレジスタに分けて入れます。下位4ビットが小数部なので、値は実際の
// 行数の16倍で表します（ドライバのSC202CS_FETCH_EXP_*と同じ分け方です）。
bool writeExposure(uint32_t value) {
  bool ok = writeReg(0x3e00, uint8_t((value >> 12) & 0x0F));
  ok &= writeReg(0x3e01, uint8_t((value >> 4) & 0xFF));
  ok &= writeReg(0x3e02, uint8_t((value & 0x0F) << 4));
  return ok;
}

// フレーム全体の高さ（VTS）を書き込みます。
//
// これを大きくすると1枚あたりの時間が延び、フレームレートが下がります。
// 明るさを1秒ごとに見るだけなら毎秒30枚は過剰で、下げるほどPSRAMの帯域も
// 消費電力も抑えられます。
bool writeFrameHeight(uint16_t vts) {
  bool ok = writeReg(0x320e, uint8_t(vts >> 8));
  ok &= writeReg(0x320f, uint8_t(vts & 0xFF));
  return ok;
}

}  // namespace

namespace CameraSensor {

bool begin() {
  if (g_running) return true;

  // 電源（IOエクスパンダのピン6）を入れます。実機では最初から入っていますが、
  // 明示しておかないと、状態が変わったときに気付けません。
  auto &ioexp = M5.getIOExpander(CAMERA_POWER_IO_EXPANDER);
  ioexp.setDirection(CAMERA_POWER_IO_PIN, true);
  ioexp.setHighImpedance(CAMERA_POWER_IO_PIN, false);
  ioexp.digitalWrite(CAMERA_POWER_IO_PIN, true);
  delay(10);

  size_t failed = 0;
  for (size_t i = 0; i < kInitTableLen; i++) {
    if (!writeReg(kInitTable[i].reg, kInitTable[i].val)) failed++;
  }
  if (failed > 0) {
    Serial.printf("[CAM] 初期化の書き込みに%u件失敗しました（全%u件）\n", unsigned(failed),
                  unsigned(kInitTableLen));
    return false;
  }

  // ゲインは最小に固定します。
  //
  // Espressifのゲイン表の先頭（1倍）と同じ値です。明るさを測るのが目的なので、
  // 増幅は入れず、露出だけで調整します。
  writeReg(0x3e06, 0x00);                     // デジタルゲイン 粗
  writeReg(0x3e07, CAMERA_FIXED_DIGITAL_GAIN);  // デジタルゲイン 微（0x80=1倍）
  writeReg(0x3e09, CAMERA_FIXED_ANALOG_GAIN);   // アナログゲイン

  writeExposure(CAMERA_FIXED_EXPOSURE);

  writeFrameHeight(CAMERA_FRAME_HEIGHT);

  g_running = true;
  Serial.printf("[CAM] センサを設定しました（%u件）\n", unsigned(kInitTableLen));
  return true;
}

uint32_t gainMilliForIndex(int index) {
  if (index < CAMERA_GAIN_INDEX_MIN) index = CAMERA_GAIN_INDEX_MIN;
  if (index > CAMERA_GAIN_INDEX_MAX) index = CAMERA_GAIN_INDEX_MAX;
  int block = index / CAMERA_GAIN_STEPS_PER_BLOCK;
  int step = index % CAMERA_GAIN_STEPS_PER_BLOCK;
  // デジタルの微調整は0x80が1倍で、4ごとに1/32倍ずつ増えます。
  uint32_t fineMilli = (0x80 + 4 * step) * 1000 / 0x80;
  return fineMilli * kAnalogMultiplier[block] * kCoarseMultiplier[block];
}

int gainIndexForMilli(uint32_t milli) {
  // 表は小さい順に並んでいるので、超えない範囲で一番大きい段を選びます。
  int found = CAMERA_GAIN_INDEX_MIN;
  for (int i = CAMERA_GAIN_INDEX_MIN; i <= CAMERA_GAIN_INDEX_MAX; i++) {
    if (gainMilliForIndex(i) > milli) break;
    found = i;
  }
  return found;
}

bool setGainIndex(int index) {
  if (index < CAMERA_GAIN_INDEX_MIN) index = CAMERA_GAIN_INDEX_MIN;
  if (index > CAMERA_GAIN_INDEX_MAX) index = CAMERA_GAIN_INDEX_MAX;
  int block = index / CAMERA_GAIN_STEPS_PER_BLOCK;
  int step = index % CAMERA_GAIN_STEPS_PER_BLOCK;
  bool ok = writeReg(0x3e06, kCoarseRegister[block]);
  ok = writeReg(0x3e07, uint8_t(0x80 + 4 * step)) && ok;
  ok = writeReg(0x3e09, kAnalogRegister[block]) && ok;
  return ok;
}

// ここで丸めるのはレジスタに書ける限界（65535）までです。
//
// 「測定で使ってよい上限」はこれより低く、そちらはCameraCapture側で守ります。
// 調査用の掃引で上限より上も試せるようにするためです。
bool setExposure(uint32_t value) {
  if (value < CAMERA_EXPOSURE_MIN) value = CAMERA_EXPOSURE_MIN;
  if (value > CAMERA_EXPOSURE_REG_MAX) value = CAMERA_EXPOSURE_REG_MAX;
  return writeExposure(value);
}

bool startStream() {
  if (!g_running) return false;
  if (g_streaming) return true;
  if (!writeReg(0x0100, 0x01)) {
    Serial.println("[CAM] 出力の開始に失敗しました");
    return false;
  }
  g_streaming = true;
  return true;
}

void stopStream() {
  if (!g_streaming) return;
  writeReg(0x0100, 0x00);
  g_streaming = false;
}

void end() {
  if (!g_running) return;
  stopStream();
  g_running = false;
  Serial.println("[CAM] センサを止めました");
}

String status() {
  struct Entry {
    uint16_t reg;
    const char *label;
  };
  static const Entry kEntries[] = {
    {0x0100, "動作中"},    {0x3e00, "露出H"},     {0x3e01, "露出M"},
    {0x3e02, "露出L"},     {0x3e07, "ゲインD微"}, {0x3e09, "ゲインA"},
    {0x320e, "全高H"},     {0x320f, "全高L"},
  };
  String report = g_running ? (g_streaming ? "出力中: " : "設定済み: ") : "停止中: ";
  for (const Entry &e : kEntries) {
    uint8_t v = 0;
    bool ok = readReg(e.reg, &v);
    char buf[32];
    snprintf(buf, sizeof(buf), "%s=%s%02X ", e.label, ok ? "" : "??", ok ? v : 0);
    report += buf;
  }
  Serial.printf("[CAM] %s\n", report.c_str());
  return report;
}

}  // namespace CameraSensor