Hi3516CV610 + MPU6050 DMP 软件 EIS 防抖笔记

1、vi_cfg 是根据摄像头 Sensor 类型生成的一套默认视频采集配置，包含 Sensor 基础信息、MIPI RX、VI Device、Device 与 Pipe 的绑定关系、WDR 融合组、VI Pipe、ISP 和 VI Channel 等配置。MIPI RX 负责接收并解析 Sensor 通过 MIPI CSI-2 发送的 RAW 数据；VI Device 负责把 MIPI RX 的数据接入 VI 系统；VI Pipe 负责建立图像处理通道并决定是否经过 ISP；ISP负责图像处理以及注册 Sensor、AE、AWB 等回调；VI Channel 则作为 Pipe 处理后的图像输出端口。

2、主要参数：

① MIPI RX 的主要参数包括 MIPI 设备号、Lane 数量和、输入尺寸、RAW 数据类型。

② VI Device 的主要参数包括 Device 编号、输入接口类型MIPI、RAW/YUV 数据类型、输入尺寸和逐行/隔行扫描模式，其中逐行扫描表示按完整帧输出。

③ VI Pipe 的主要参数包括输入尺寸、Bayer格式、帧率、是否绕过ISP以及是否启动ISP线程。

④ ISP主要配置Sensor尺寸、帧率并通过Sensor回调及I²C初始化SC4336P、设置曝光和增益，最终写入stream-on寄存器开始输出图像。

⑤ VI Channel主要配置输出尺寸、YUV格式，它的输入来自所属VI Pipe，因此没有单独的Channel输入尺寸参数。

3、这些参数由应用程序通过 ss_mpi_*() 接口下发。ss_mpi_*() 是用户态MPI接口，通常由 libss_mpi.so、libss_mpi.a、ISP库和Sensor库进行封装。MPI库会检查并整理参数，再通过 ioctl等方式交给 ot_mipi_rx.ko、ot_vi.ko、ot_isp.ko 等内核驱动。应用程序传入的设备号、Pipe号、Channel号、尺寸和格式都是逻辑编号或配置值，并不是寄存器地址；具体寄存器地址由内核驱动掌握，驱动负责完成时钟、复位、MMIO寄存器、DMA、中断和硬件状态机的配置。只有板外SC4336P的寄存器主要通过I²C进行配置。

4、完成这些配置并依次启动MIPI RX、VI Device、VI Pipe、ISP和VI Channel后，SC4336P输出的数据就会经过MIPI RX和VI/ISP处理，从VI Channel产生视频帧。视频数据通过SoC内部总线和DMA传输。离线模式下，VI Channel通常把完整图像写入由 ot_vb.ko 管理的VB视频缓冲池，再由应用程序或VPSS获取；ot_sys.ko 负责系统级媒体资源和模块协同，VB缓冲的具体配置和管理主要由SYS/VB接口及 ot_vb.ko 完成。

5、VPSS 负责对 VI 输出的视频帧进行裁剪、缩放、格式转换和帧率控制等处理。应用程序通过 ss_mpi_vpss_*() 接口创建并启动 VPSS Group，配置输入图像属性，并使能各个 VPSS Channel 输出不同尺寸或格式的图像。需要注意的是，修改 VPSS Channel 输出尺寸通常只是缩放；如果要裁剪画面区域，需要通过专门的 Crop 或 Zoom 接口设置。sample_comm_vi_bind_vpss() 通过 SYS 绑定机制把 VI Pipe/Channel 连接到 VPSS Group，并不是 CPU 逐帧拷贝数据。在离线模式下，VI 通常先把完整帧写入 VB/DDR，再由 VPSS 读取处理；图像从已使能的 VPSS Channel 输出给 VENC。

6、sample_venc_normal_start_encode() 根据编码分辨率、编码类型及用户选择生成H.265和H.264编码参数，包括码率控制、GOP、帧率、Profile和码流缓冲区大小，再通过 sample_comm_venc_start() 将参数传给VENC驱动，由驱动配置编码硬件、DMA、中断及相关缓冲区并启动VENC Channel。随后，sample_comm_vpss_bind_venc() 通过 SYS 绑定机制把 VPSS Channel 连接到 VENC Channel。VPSS 输出一帧 YUV 图像后，VENC 直接通过帧描述和物理地址读取图像并进行硬件编码，不需要 CPU 逐帧复制。编码后的 H.264/H.265 码流进入 VENC 码流缓冲区，应用程序再通过 ss_mpi_venc_get_stream() 取出码流，用于保存文件或网络发送。编码完成后，原始 YUV 帧会被释放；普通模式下 VB 块返回 VB 池，Wrap 模式下则循环复用 Wrap 缓冲区。

一、核心概念

电子防抖（EIS）的本质是"裁剪窗口反向补偿"，不是让 Sensor 或摄像头停止抖动。

当摄像头发生转动时，固定目标会在原始画面中向反方向移动。程序根据 IMU 姿态改变裁剪窗口的位置，使目标在最终输出画面中的位置尽量保持稳定。DMP 只负责估计姿态；真正让画面稳定的是 VI/VPSS 的裁剪执行器。

当前验证链路：

Sensor/VI 输入：2560x1440
EIS 输出窗口：1920x1080
水平余量：640 px，裁剪 x 的范围为 0~640
垂直余量：360 px，裁剪 y 的范围为 0~360
中心窗口：rect=(320,180,1920,1080)

例如，摄像头向右转时，固定目标在原图中向左移。若目标从原图 x=1280 移到 x=1235，而裁剪窗口还在 x=320，则目标在输出中的位置为 1235-320=915，已经偏离输出中心 960。把窗口同时左移到 x=275 后，目标位置变为 1235-275=960，目标重新回到中心。

二、当前视频链路与裁剪位置

当前工程最终选择 VI Pipe Post Crop 作为 EIS 执行器：它在 VI 输出进入 VPSS 前从 2560x1440 中选出 1920x1080，因此下游 VPSS 收到的是裁剪后的 1920x1080 图像。

SC4336P Sensor
    |
    | 2560x1440 原始输入
    v
VI Pipe 0
    |
    |-- ss_mpi_vi_set_pipe_post_crop()
    |   // 在 2560x1440 的输入坐标域设置 EIS 裁剪窗口。
    |   // 当前实时防抖实际调用的硬件接口。
    v
VPSS Group 0
    |
    |-- VPSS Chn0 -> VENC0
    |   // Chn0 也会收到 VI 后裁剪的图像。
    |
    `-- VPSS Chn1 -> VENC1 -> RTSP /test.264
        // 当前 RTSP 视频由 VENC1 的数据发送。

为什么不用 VPSS Chn Crop：VPSS Chn Crop 使用的是该 Channel 配置后的输出坐标域。若 Chn1 已是 1920x1080，rect=(320,180,1920,1080) 不能表示"从 2560x1440 原图中取中心窗口"，容易越界或导致参数错误。

VPSS Group Crop 位于通道缩放之前，理论上可在 Group 的 2560x1440 输入域裁剪，但会影响全部 Channel，并受 Online Wrap、VB 缓冲、通道尺寸和启动时序等组合约束。它不是绝对不能使用，只是当前配置下曾出现非法参数，不适合作为第一条稳定的 EIS 路径。

VI Post Crop 的代价是：该 Pipe 后的所有 VPSS 通道都失去原始 2560x1440 图像。后续若要求大码流保留原图、小码流单独防抖，需要独立的处理支路或重新设计 VPSS/硬件资源分配。

三、MPU6050 原始陀螺仪验证

MPU6050 连接到 I2C2，设备地址为 0x68。先读取 WHO_AM_I(0x75)，返回 0x68 才能确认地址、供电、SDA/SCL 与基本 I2C 通信正常。

在调 DMP 前，应先关闭 DMP 和 FIFO，只验证原始陀螺仪：

I2C2 /dev/i2c-2
    |
    |-- 读 0x75
    |   // WHO_AM_I，预期为 0x68。
    |
    |-- 写 0x6A = 0x00
    |   // USER_CTRL，关闭 DMP/FIFO 路径。
    |
    |-- 写 0x6B = 0x01
    |   // PWR_MGMT_1，解除休眠并选择工作时钟。
    |
    |-- 写 0x1B = 0x00
    |   // GYRO_CONFIG，设置陀螺仪量程；此值对应 +/-250 dps。
    |
    |-- 写 0x1A = 0x03
    |   // CONFIG，设置数字低通滤波器 DLPF。
    |
    |-- 写 0x19 = 0x04
    |   // SMPLRT_DIV，设置底层采样分频。
    |
    `-- 连续读 0x43~0x48
        // 得到 gyro_x / gyro_y / gyro_z 三个 16 位原始值。

静止状态下三个轴的平均值是零速偏置（零偏），不是移动量。原始陀螺仪读数减去零偏后得到校正角速度；角速度单位为 dps。若没有 DMP，需要按实际 dt 对角速度积分才能得到角度。

四、Linux I2C 访问路径

官方 DMP 代码原本主要面向 MCU。Linux 适配层打开 /dev/i2c-2，构造 struct i2c_msg，通过 ioctl(fd, I2C_RDWR, ...) 访问 MPU6050。

mpu_init() / dmp_read_fifo()
    |
    |-- mpu6050_i2c_read() / mpu6050_i2c_write()
    |   // Linux 适配层构造 I2C 消息。
    v
ioctl(fd, I2C_RDWR, ...)
    |
    v
i2c-dev -> i2c_transfer() -> adapter->algo->master_xfer()
    |
    v
bsp_i2c_xfer()
    |
    `-- Hi3516 硬件 I2C 控制器 -> MPU6050(0x68)

读取普通寄存器时，通常由两个 i2c_msg 构成一次事务：第一个消息写寄存器地址，第二个消息使用重复起始条件读取数据。例如读取 0x43 开始的 6 字节陀螺仪数据。

五、DMP 固件加载与 FIFO 运行

DMP 是 MPU6050 内部的数字运动处理器。加载固件后，它可融合陀螺仪与加速度计，输出四元数、校正后的陀螺仪等数据到 FIFO。它不负责视频裁剪。

当前没有磁力计，因此 yaw 是相对航向角，长期可能漂移；短时间手抖补偿通常仍可使用。pitch/roll 会受到加速度计辅助，但摄像头快速加减速时也可能被线加速度干扰。

dmp_load_motion_driver_firmware()
    |
    v
mpu_load_firmware()
    |
    |-- 写 0x6D：BANK_SEL
    |   // 选择 DMP SRAM 的 Bank。
    |
    |-- 写 0x6E：MEM_START_ADDR
    |   // 选择该 Bank 内的起始偏移。
    |
    |-- 写 0x6F：MEM_R_W
    |   // 连续写入固件字节，内部地址自动递增。
    |
    |-- 读回 0x6F
    |   // 校验固件是否正确写入 DMP SRAM。
    |
    `-- 写 0x70 / 0x71
        // 设置 DMP 程序入口地址。

sample_eis_imu_dmp_start()
    |
    |-- mpu_init()
    |   // 初始化 MPU6050：复位、唤醒与基础采样配置。
    |
    |-- mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)
    |   // 使能三轴陀螺仪和三轴加速度计。
    |
    |-- dmp_load_motion_driver_firmware()
    |   // 加载并校验 DMP 固件。
    |
    |-- dmp_set_orientation()
    |   // 设置 MPU6050 实际安装方向与逻辑坐标轴映射。
    |
    |-- dmp_enable_feature()
    |   // 选择四元数、校正陀螺仪等 DMP 输出功能。
    |
    |-- dmp_set_fifo_rate(100)
    |   // 设置 DMP 写 FIFO 的输出频率，当前为 100 Hz。
    |
    `-- mpu_set_dmp_state(1)
        |
        |-- 0x6A = 0x0C
        |   // FIFO_RESET + DMP_RESET，清除残留状态和旧 FIFO 数据。
        |
        |-- 0x6A = 0xC0
        |   // FIFO_EN + DMP_EN，开启 FIFO 与 DMP。
        |
        |-- 0x23 = 0x00
        |   // 关闭原始传感器直接写 FIFO 的通路，FIFO 由 DMP 输出接管。
        |
        `-- 0x38 配置 DMP 中断
            // 有新 FIFO 数据时可通知主机读取。

运行时读 FIFO 不需要再设置 0x6D/0x6E。Bank 仅用于 DMP SRAM；FIFO 是固定寄存器窗口：

dmp_read_fifo()
    |
    |-- 读 0x72 / 0x73：FIFO_COUNT_H/L
    |   // 查询 FIFO 内积压的数据字节数。
    |
    `-- 读 0x74：FIFO_R_W
        // 连续读出一个完整 DMP 包并解析四元数、陀螺仪、加速度计等。

六、IMU 环形缓冲与视频 PTS 匹配

IMU 线程约 100 Hz 写入姿态，视频线程约 30 Hz 读取并匹配。两者通过同进程、互斥锁保护的环形缓冲共享数据。

一条 IMU 数据至少包含：

pts_us                 // MPP 时间戳
pitch_deg/yaw_deg/roll_deg
enable_stab
sequence               // 递增序号，便于日志和调试

环形缓冲保存 512 条数据，在 100 Hz 下约保留 5.12 秒。满后从头覆盖最旧数据；视频线程只复制找到的最近项，随后立即解锁，避免阻塞 IMU 写入。

sample_eis_imu_start()
    |
    |-- imu_timestamp_buffer_init()
    |   // 初始化 512 项环形缓冲和 pthread_mutex。
    |
    |-- sample_eis_imu_dmp_start()
    |   // 启动 MPU6050 DMP。
    |
    `-- pthread_create(sample_eis_imu_thread)
        // 创建持续读取 FIFO 的线程。

sample_eis_imu_thread()
    |
    |-- dmp_read_fifo()
    |   // 读取一包 DMP FIFO 数据。
    |
    |-- sample_eis_imu_quat_to_euler()
    |   // 四元数转换为 pitch / yaw / roll。
    |
    |-- ss_mpi_sys_get_cur_pts()
    |   // 获取 Hi3516 MPP 统一时间基。
    |
    `-- imu_timestamp_buffer_push()
        // 写入姿态、PTS、序号；下标循环递增。

sample_easyvenc_comm_save_frame_to_file(index=1)
    |
    |-- stream->pack[0].pts
    |   // 取得当前 VENC1 完整编码帧的 PTS。
    |
    `-- sample_rtsp_log_imu_match(video_pts)
        |
        |-- sample_eis_imu_find_nearest()
        |   // 查找 abs(imu_pts-video_pts) 最小的样本，并复制到本地变量。
        |
        |-- abs(delta_us) <= 20000 us ?
        |   // 当前匹配有效阈值是 20 ms；最近不代表一定足够近。
        |
        |-- 是 -> sample_rtsp_eis_process_match()
        |   // 使用匹配姿态计算 EIS。
        |
        `-- 否 -> sample_rtsp_eis_process_miss()
            // 前 3 帧保持上次裁剪；之后每帧向中心回退最多 16 px。

当前 IMU PTS 是 dmp_read_fifo() 返回后读取的，代表"主机读到数据的时间"，不完全等于"传感器采样时间"。后续可引入：

effective_imu_pts = read_pts - EIS_IMU_READ_DELAY_US;

EIS_IMU_READ_DELAY_US 只用于修正写入环形缓冲的 IMU 时间戳，不改变 DMP 采样率、不改变 MPP 系统时间。

七、实时 EIS 状态流程

SYS/VI/VPSS 初始化完成
    |
    |-- sample_rtsp_start_vpss_crop_test()
    |   // 建立 EIS 裁剪配置，并先设置中心 VI Post Crop。
    |
    |-- sample_eis_imu_start()
    |   // 启动 DMP 与 IMU FIFO 读取线程。
    |
    |-- sample_venc_normal_start_encode()
    |   // 启动 VENC0/VENC1 和取流线程。
    |
    `-- VENC1 每得到一个完整编码帧
        |
        |-- video_pts = stream->pack[0].pts
        |
        |-- 找到最近 IMU，并检查 delta_us
        |
        |-- 匹配失败？
        |   |
        |   `-- sample_rtsp_eis_process_miss()
        |       // 短暂失败保持窗口；持续失败渐进回中心。
        |
        `-- 匹配成功 -> sample_rtsp_eis_process_match()
            |
            |-- 前 240 个有效视频匹配帧
            |   // DMP settle，约 8 秒，只等待姿态稳定。
            |
            |-- 后续 30 个有效匹配帧
            |   // 建立 base_pitch/base_yaw 基准姿态。
            |   // yaw 用 sin/cos 均值，避免 +/-180 度跳变。
            |
            |-- diff_pitch = pitch - base_pitch
            |-- diff_yaw = normalize(yaw - base_yaw)
            |-- eis_angle = max(abs(diff_pitch), abs(diff_yaw))
            |
            |-- eis_angle < 8 度
            |   |
            |   |-- 角度转像素
            |   |   // dx=fx*tan(diff_yaw)，dy=fy*tan(diff_pitch)。
            |   |
            |   |-- 计算 crop_x/crop_y
            |   |   // 使用 yaw/pitch 的实际安装方向符号。
            |   |
            |   `-- sample_rtsp_eis_apply_crop()
            |       // 裁剪坐标限幅、2 像素对齐后调用
            |       // ss_mpi_vi_set_pipe_post_crop()。
            |
            |-- 8 度 <= eis_angle <= 10 度
            |   // 保持上一次裁剪窗口；这是迟滞区，不是静止死区。
            |
            `-- eis_angle > 10 度
                |
                |-- sample_rtsp_eis_reset_baseline()
                |   // 关闭补偿并准备重新标定基准。
                |
                `-- sample_rtsp_eis_recenter()
                    // 窗口向中心移动最多 16 px，避免突然跳变。

注意：VENC1 当前帧被取到时已经在流水线末端，因此该帧的 IMU 匹配结果只能影响"后续帧"的 VI 裁剪，不能修改已经编码完成的当前帧。roll 当前只记录，不参与裁剪；要补偿 roll 需要图像旋转和重采样，简单移动矩形窗口无法做到。

八、调参笔记

1. FOV（优先调）

EIS_FOV_H_DEG 与 EIS_FOV_V_DEG 最好由镜头厂商、摄像头模组厂商或整机供应商提供。需要的是"实际镜头 + 实际 Sensor + 实际 2560x1440 工作模式"下的水平/垂直 FOV，不能直接使用宣传页的对角 FOV，也不能使用该镜头配另一颗 Sensor 时的参数。

当前初始值：

EIS_FOV_H_DEG = 90.0
EIS_FOV_V_DEG = 60.0

针孔模型：

fx = input_width  / (2 * tan(HFOV / 2))
fy = input_height / (2 * tan(VFOV / 2))
dx = fx * tan(diff_yaw)
dy = fy * tan(diff_pitch)

对于 2560x1440、HFOV=90 度、VFOV=60 度：fx=1280 px，fy 约为 1247 px；yaw 偏移 2 度约对应 45 px 的水平裁剪位移。

若厂商不能提供准确 FOV，使用转台标定：让摄像头围绕光心转动已知小角度（如 2 度或 5 度），测量画面中央远处目标在 2560x1440 原图中的像素位移：

fx 约等于 水平像素位移 / tan(实际转角)
HFOV = 2 * atan(输入宽度 / (2 * fx))

小角度 EIS 中，实测 fx/fy 通常比标称 FOV 更可靠，因为它同时包含了实际镜头、Sensor 工作区域和畸变的影响。

1. 方向符号

   EIS_YAW_SIGN = -1.0

   EIS_PITCH_SIGN = 1.0

符号必须通过实际画面校准。摄像头向右转时，记录 DMP yaw 增加还是减小，并观察目标偏移方向；若补偿方向错误，优先翻转对应 SIGN，而不是改 FOV。

1. 大幅运动阈值

当前：

EIS_STAB_ENABLE_ANGLE_DEG  = 8.0
EIS_STAB_DISABLE_ANGLE_DEG = 10.0

小于 8 度：正常补偿
8~10 度：保持上一裁剪窗口
大于 10 度：视为主动转向，重置基准并渐进回中心

阈值应结合余量调节。当前纵向余量只有 +/-180 px，按照 VFOV=60 度，pitch 接近 8 度时已经接近垂直边界，所以 8/10 度是保守的初始值。若输出窗口更大、余量更小，应降低阈值。

1. 静止抖动：软件滤波与小死区（建议后续增加，当前未实现）

8/10 度不是用于消除静止时的窗口跳动。若窗口在 318、320、322 附近来回跳，应增加软件低通和独立死区。建议起始值：

EIS_FILTER_ALPHA          = 0.35
EIS_ANGLE_DEAD_ZONE_DEG   = 0.15
EIS_CROP_DEAD_ZONE_PX     = 4

一阶低通示意：

filtered_yaw += alpha * (diff_yaw - filtered_yaw)
filtered_pitch += alpha * (diff_pitch - filtered_pitch)

alpha 小则更稳但反应更慢；alpha 大则响应更快但噪声更多。小于角度死区时将该轴偏移视为 0；新旧 crop 坐标差小于像素死区时不调用 VI 裁剪接口。DLPF 也能降低传感器噪声，但会引入传感器侧延迟；不要一开始同时把 DLPF 和软件低通调得过重。

1. 时间同步与预测（建议后续增加）

打印并观察：

now_pts - video_pts
imu_pts - video_pts

若目标仍明显跟在摄像头抖动后面移动，说明控制滞后，应增加 EIS_CONTROL_LEAD_US；若目标提前移动、转向时反向过冲，应减小它。建议从 0 us 开始，以 5~10 ms 为步长调节。

预测最好保存 IMU 的 gyro_x/gyro_y/gyro_z，使用角速度预测裁剪真正生效时的姿态。仅用 VENC PTS 匹配适合当前同步验证，但 VENC 位于视频流水线末端，最终控制仍需要考虑流水线延迟。

1. FIFO、I2C 与匹配阈值

当前 DMP FIFO 为 100 Hz，视频约 30 Hz，匹配有效阈值为 20 ms。若频繁出现匹配失败，不应直接无限放大阈值，应依次检查：

- dmp_read_fifo() 是否频繁失败；
- FIFO 是否积压或溢出；
- I2C 是否出现超时；
- IMU PTS 是否需要 EIS_IMU_READ_DELAY_US 修正；
- IMU 与视频是否使用同一 MPP 时间基。

阈值过大时，系统会把明显不同步的姿态用于裁剪，容易出现拖影、反向抖动或转向过冲。

九、推荐调试顺序

1. 固定中心裁剪，确认 RTSP 长时间稳定出图。
2. 手动移动中心/左/右/上/下裁剪窗口，确认余量和边界正确。
3. 验证 MPU6050 WHO_AM_I、原始陀螺仪、零偏和轴方向。
4. 验证 DMP 固件加载、FIFO 读取、四元数和姿态角稳定性。
5. 验证 IMU PTS 与 VENC1 PTS 匹配误差。
6. 校准 yaw/pitch 符号，再标定 FOV 或直接标定 fx/fy。
7. 开启小角度实时裁剪，先观察补偿方向和边界。
8. 增加滤波、角度死区和像素死区，消除静止窗口抖动。
9. 调整 EIS_IMU_READ_DELAY_US 与 EIS_CONTROL_LEAD_US，减少滞后和过冲。
10. 最后评估长时间 yaw 漂移、主动转向、大幅振动和 FIFO 异常恢复。