ステレオカメラによる距離測定テスト8（アプリケーションの作製）

”ステレオカメラによる距離測定テスト7（BOOT.bin, devicetree.dtb）”の続き。

前回は、BOOT.bin とdevicetree.dtb を作製して、ZYBO にMicroSDカードを挿入してUbuntu 14.04 LTS を立ち上げた。
今回は、自分のカメラ画像とそのラプラシアンフィルタ処理画像、それにZYBO_0_2 からHDMI ポート経由で転送されてきたカメラ画像を表示するアプリケーションを作製する。

最初に、udmabuf デバイス・ドライバをロードするスクリプト udmabuf_insmod を作製した。

sudo insmod udmabuf.ko udmabuf0=15197184
sudo chmod 666 /dev/udmabuf0

15197184 の意味は、SVGA画面（800 x 600）を 3 画面とXGA画面（1024 x 768）を 3 画面の1ピクセル = 4 バイト分のフレームバッファということである。

自分のカメラ画像とそのラプラシアンフィルタ処理画像、それにZYBO_0_2 からHDMI ポート経由で転送されてきたカメラ画像を表示するアプリケーションのStereoCam_Alt_Disp.c を作製し、gcc でコンパイルした。

右目用、左目用のZYBO をTera Term でコネクトした。

右目用、左目用のZYBOの写真を示す。


右目用のZYBO_0_2 は cam_disp_vdma アプリケーションを起動した。（cam_disp_vdma.c については、”デバイスドライバ udmabuf を使用する2”を参照のこと）

左目用のZYBO は
.udmabuf_insmod
を起動してから、
./StereoCam_Alt_Disp
を起動した。

上図の右上が右目用ZYBO_0_2 のシリアル・インターフェースに接続したTera Term、右下が右目用ZYBO_0_2 にSSH で接続したTera Term、左上が左目用ZYBO のシリアル・インターフェースに接続したTera Term、左下が左目用ZYBO にSSH で接続したTera Term を示す。

アプリケーションのStereoCam_Alt_Disp は起動後、自分のカメラ画像を表示するが、その後はキー待ちをしていて

1 とリターンキーを押すと自分のカメラ画像を表示する。
2 とリターンキーを押すと右目用ZYBO からHDMI ポート経由で転送されてきた画像を表示する。
l （Lの小文字）とリターンキーを押すと、自分のカメラ画像にラプラシアンフィルタ処理を施す。
c とリターンキーを押すと、カメラ画像に戻る。
q とリターンキーで、アプリケーションを終了する。

という処理を行う。

左目カメラ画像を示す。


右目カメラ画像を示す。


左目ラプラシアンフィルタ処理画像を示す。


右目ラプラシアンフィルタ処理画像を示す。


暗くなっているので、エッジが見にくくなっている。こういう場合は、以前作成したアンシャープマスキング・フィルタIP を使って、エッジを強調すれば良いのじゃないかな？と思っている。

最後に、StereoCam_Alt_Disp.c を貼っておく。

//
// StereoCam_Alt_Disp.c
// 2016/02/01 by marsee 
//

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>
#include <sys/mman.h>
#include <fcntl.h>

#define PIXEL_NUM_OF_BYTES    4
#define NUMBER_OF_WRITE_FRAMES    3 // Note: If not at least 3 or more, the image is not displayed in succession.

#define XGA_HORIZONTAL_PIXELS    1024
#define XGA_VERTICAL_LINES        768
#define XGA_ALL_DISP_ADDRESS    (XGA_HORIZONTAL_PIXELS*XGA_VERTICAL_LINES*PIXEL_NUM_OF_BYTES)

#define SVGA_HORIZONTAL_PIXELS    800
#define SVGA_VERTICAL_LINES        600
#define SVGA_ALL_DISP_ADDRESS    (SVGA_HORIZONTAL_PIXELS*SVGA_VERTICAL_LINES*PIXEL_NUM_OF_BYTES)

void cam_i2c_init(volatile unsigned *caminf_axi_iic) {
    caminf_axi_iic[64] = 0x2; // reset tx fifo ,address is 0x100, i2c_control_reg
    caminf_axi_iic[64] = 0x1; // enable i2c
}

void cam_i2x_write_sync(void) {
    // unsigned c;

    // c = *cam_i2c_rx_fifo;
    // while ((c & 0x84) != 0x80)
        // c = *cam_i2c_rx_fifo; // No Bus Busy and TX_FIFO_Empty = 1
    usleep(1000);
}

void cam_i2c_write(volatile unsigned *caminf_axi_iic, unsigned int device_addr, unsigned int write_addr, unsigned int write_data){
    caminf_axi_iic[66] = 0x100 | (device_addr & 0xfe);    // Slave IIC Write Address, address is 0x108, i2c_tx_fifo
    caminf_axi_iic[66] = write_addr;
    caminf_axi_iic[66] = (write_data >> 8)|0xff;            // first data
    caminf_axi_iic[66] = 0x200 | (write_data & 0xff);        // second data
    cam_i2x_write_sync();
}

int main()
{
    int fd0, fd1, fd2, fd3, fd4, fd5, fd6, fd7, fd9, fd10;
    volatile unsigned *bmdc_axi_lites0;
    volatile unsigned *caminf_axi_vdma_0, *dviin_axi_vdma_0;
    volatile unsigned *caminf_axis_switch_0, *caminf_axis_switch_1;
    volatile unsigned *caminf_mt9d111_inf_axis_0;
    volatile unsigned *caminf_axi_iic;
    volatile unsigned *caminf_lap_filter_axis_0;
    volatile unsigned *frame_buffer;
    unsigned char  attr[1024];
    unsigned long  phys_addr;
    char c;
    int laps_cntrl;

    // Bitmap Display Controller 0 AXI4 Lite Slave (UIO6)
    fd6 = open("/dev/uio6", O_RDWR); // bitmap_display_controller 0 axi4 lite
    if (fd6 < 1){
        fprintf(stderr, "/dev/uio6 (bitmap_disp_cntrler_axi_master_0) open error\n");
        exit(-1);
    }
    bmdc_axi_lites0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd6, 0);
    if (!bmdc_axi_lites0){
        fprintf(stderr, "bmdc_axi_lites0 mmap error\n");
        exit(-1);
    }
    
    // caminf_axi_vdma_0 (UIO1)
    fd1 = open("/dev/uio1", O_RDWR); // caminf_axi_vdma_0 interface AXI4 Lite Slave
    if (fd1 < 1){
        fprintf(stderr, "/dev/uio1 (caminf_axi_vdma_0) open error\n");
        exit(-1);
    }
    caminf_axi_vdma_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd1, 0);
    if (!caminf_axi_vdma_0){
        fprintf(stderr, "caminf_axi_vdma_0 mmap error\n");
        exit(-1);
    }
    
    // dviin_axi_vdma_0 (UIO7)
    fd7 = open("/dev/uio7", O_RDWR); // dviin_axi_vdma_0 interface AXI4 Lite Slave
    if (fd7 < 1){
        fprintf(stderr, "/dev/uio7 (dviin_axi_vdma_0) open error\n");
        exit(-1);
    }
    dviin_axi_vdma_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd7, 0);
    if (!dviin_axi_vdma_0){
        fprintf(stderr, "dviin_axi_vdma_0 mmap error\n");
        exit(-1);
    }
    
    // mt9d111 i2c AXI4 Lite Slave (UIO0)
    fd0 = open("/dev/uio0", O_RDWR); // mt9d111 i2c AXI4 Lite Slave
    if (fd0 < 1){
        fprintf(stderr, "/dev/uio0 (caminf_axi_iic) open error\n");
        exit(-1);
    }
    caminf_axi_iic = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd0, 0);
    if (!caminf_axi_iic){
        fprintf(stderr, "caminf_axi_iic mmap error\n");
        exit(-1);
    }

    // mt9d111 inf axis AXI4 Lite Slave (UIO5)
    fd5 = open("/dev/uio5", O_RDWR); // mt9d111 inf axis AXI4 Lite Slave
    if (fd5 < 1){
        fprintf(stderr, "/dev/uio5 (caminf_mt9d111_inf_axis_0) open error\n");
        exit(-1);
    }
    caminf_mt9d111_inf_axis_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd5, 0);
    if (!caminf_mt9d111_inf_axis_0){
        fprintf(stderr, "caminf_mt9d111_inf_axis_0 mmap error\n");
        exit(-1);
    }

    // caminf_axis_switch_0 (UIO2)
    fd2 = open("/dev/uio2", O_RDWR); // caminf_axis_switch_0 interface AXI4 Lite Slave
    if (fd2 < 1){
        fprintf(stderr, "/dev/uio2 (caminf_axis_switch_0) open error\n");
        exit(-1);
    }
    caminf_axis_switch_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd2, 0);
    if (!caminf_axis_switch_0){
        fprintf(stderr, "caminf_axis_switch_0 mmap error\n");
        exit(-1);
    }
    
    // caminf_axis_switch_1 (UIO3)
    fd3 = open("/dev/uio3", O_RDWR); // caminf_axis_switch_1 interface AXI4 Lite Slave
    if (fd3 < 1){
        fprintf(stderr, "/dev/uio3 (caminf_axis_switch_1) open error\n");
        exit(-1);
    }
    caminf_axis_switch_1 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd3, 0);
    if (!caminf_axis_switch_1){
        fprintf(stderr, "caminf_axis_switch_1 mmap error\n");
        exit(-1);
    }
    
    // caminf_lap_filter_axis_0 (UIO4)
    fd4 = open("/dev/uio4", O_RDWR); // caminf_lap_filter_axis_0 interface AXI4 Lite Slave
    if (fd4 < 1){
        fprintf(stderr, "/dev/uio4 (caminf_lap_filter_axis_0) open error\n");
        exit(-1);
    }
    caminf_lap_filter_axis_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd4, 0);
    if (!caminf_lap_filter_axis_0){
        fprintf(stderr, "caminf_lap_filter_axis_0 mmap error\n");
        exit(-1);
    }
    
    // udmabuf0
    fd9 = open("/dev/udmabuf0", O_RDWR | O_SYNC); // frame_buffer, The chache is disabled. 
    if (fd9 == -1){
        fprintf(stderr, "/dev/udmabuf0 open error\n");
        exit(-1);
    }
    frame_buffer = (volatile unsigned *)mmap(NULL, (XGA_ALL_DISP_ADDRESS*NUMBER_OF_WRITE_FRAMES)+(SVGA_ALL_DISP_ADDRESS*NUMBER_OF_WRITE_FRAMES), PROT_READ|PROT_WRITE, MAP_SHARED, fd9, 0);
    if (!frame_buffer){
        fprintf(stderr, "frame_buffer mmap error\n");
        exit(-1);
    }

    // caminf_axis_switch_1, 1to2 ,Select M00_AXIS
    // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
    caminf_axis_switch_1[16] = 0x0; // 0x40 = 0
    caminf_axis_switch_1[17] = 0x80000000; // 0x44 = 0x80000000, disable
    caminf_axis_switch_1[0] = 0x2; // Comit registers
    
    // caminf_axis_switch_0, 2to1, Select S00_AXIS
    // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
    caminf_axis_switch_0[16] = 0x0; // 0x40 = 0;
    caminf_axis_switch_0[0] = 0x2; // Comit registers
    
    // phys_addr of udmabuf0
    fd10 = open("/sys/devices/virtual/udmabuf/udmabuf0/phys_addr", O_RDONLY);
    if (fd10 == -1){
        fprintf(stderr, "/sys/devices/virtual/udmabuf/udmabuf0/phys_addr open error\n");
        exit(-1);
    }
    read(fd10, attr, 1024);
    sscanf(attr, "%lx", &phys_addr);  
    close(fd10);
    printf("phys_addr = %x\n", (unsigned)phys_addr);
    
    // AXI VDMA Initialization sequence (caminf_axi_vdma_0)
    caminf_axi_vdma_0[12] = 0x4; // S2MM_VDMACR (S2MM VDMA Control Register  Offset 30h) is 0x4 
    while ((caminf_axi_vdma_0[12] & 0x4) == 0x4) ; // Reset is progress
    caminf_axi_vdma_0[12] = 0x4; // S2MM_VDMACR (S2MM VDMA Control Register  Offset 30h) is 0x4 
    while ((caminf_axi_vdma_0[12] & 0x4) == 0x4) ; // Reset is progress
    caminf_axi_vdma_0[18] = NUMBER_OF_WRITE_FRAMES; // S2MM_FRMSTORE (0x48) register
    caminf_axi_vdma_0[12] = 0x00010002; // S2MM_VDMACR(IRQFrameCount = 0x1, Circular_Park = 1)
    caminf_axi_vdma_0[41] = SVGA_HORIZONTAL_PIXELS*PIXEL_NUM_OF_BYTES; // S2MM Horizontal Size Register（S2MM_HSIZE）0xc80 = 3200dec = 800 x 4
    caminf_axi_vdma_0[42] = SVGA_HORIZONTAL_PIXELS*PIXEL_NUM_OF_BYTES; // S2MM Frame Delay and Stride Register（S2MM_FRMDLY_STRIDE）0xc80 = 3200dec = 800 x 4
    caminf_axi_vdma_0[43] = (unsigned)phys_addr; // S2MM Start Address (1 to 16) Start Address 1
    caminf_axi_vdma_0[44] = (unsigned)phys_addr+SVGA_ALL_DISP_ADDRESS; // S2MM Start Address (1 to 16) Start Address 2
    caminf_axi_vdma_0[45] = (unsigned)phys_addr+2*SVGA_ALL_DISP_ADDRESS; // S2MM Start Address (1 to 16) Start Address 3
    caminf_axi_vdma_0[12] = 0x00010003; // S2MM_VDMACR(IRQFrameCount = 0x1, Circular_Park = 1, Run/stop = 1)
    while((caminf_axi_vdma_0[13] & 0x1) == 0x1) ; // Halt? (S2MM_VDMASR 0x34)
    caminf_axi_vdma_0[40] = SVGA_VERTICAL_LINES; // S2MM Vertical Size (S2MM_VSIZE  Offset 0xA0) 0x258 = 600dec

    // AXI VDMA Initialization sequence (dviin_axi_vdma_0)
    dviin_axi_vdma_0[12] = 0x4; // S2MM_VDMACR (S2MM VDMA Control Register  Offset 30h) is 0x4 
    while ((dviin_axi_vdma_0[12] & 0x4) == 0x4) ; // Reset is progress
    dviin_axi_vdma_0[12] = 0x4; // S2MM_VDMACR (S2MM VDMA Control Register  Offset 30h) is 0x4 
    while ((dviin_axi_vdma_0[12] & 0x4) == 0x4) ; // Reset is progress
    dviin_axi_vdma_0[18] = NUMBER_OF_WRITE_FRAMES; // S2MM_FRMSTORE (0x48) register
    dviin_axi_vdma_0[12] = 0x00010002; // S2MM_VDMACR(IRQFrameCount = 0x1, Circular_Park = 1)
    dviin_axi_vdma_0[41] = XGA_HORIZONTAL_PIXELS*PIXEL_NUM_OF_BYTES; // S2MM Horizontal Size Register（S2MM_HSIZE）0xc80 = 3200dec = 800 x 4
    dviin_axi_vdma_0[42] = XGA_HORIZONTAL_PIXELS*PIXEL_NUM_OF_BYTES; // S2MM Frame Delay and Stride Register（S2MM_FRMDLY_STRIDE）0xc80 = 3200dec = 800 x 4
    dviin_axi_vdma_0[43] = (unsigned)phys_addr+3*SVGA_ALL_DISP_ADDRESS; // S2MM Start Address (1 to 16) Start Address 1
    dviin_axi_vdma_0[44] = (unsigned)phys_addr+3*SVGA_ALL_DISP_ADDRESS+XGA_ALL_DISP_ADDRESS; // S2MM Start Address (1 to 16) Start Address 2
    dviin_axi_vdma_0[45] = (unsigned)phys_addr+3*SVGA_ALL_DISP_ADDRESS+2*XGA_ALL_DISP_ADDRESS; // S2MM Start Address (1 to 16) Start Address 3
    dviin_axi_vdma_0[12] = 0x00010003; // S2MM_VDMACR(IRQFrameCount = 0x1, Circular_Park = 1, Run/stop = 1)
    while((dviin_axi_vdma_0[13] & 0x1) == 0x1) ; // Halt? (S2MM_VDMASR 0x34)
    dviin_axi_vdma_0[40] = XGA_VERTICAL_LINES; // S2MM Vertical Size (S2MM_VSIZE  Offset 0xA0) 0x258 = 600dec

    // CMOS Camera initialize, MT9D111
    cam_i2c_init(caminf_axi_iic);
    
    cam_i2c_write(caminf_axi_iic, 0xba, 0xf0, 0x1);        // Changed regster map to IFP page 1
    cam_i2c_write(caminf_axi_iic, 0xba, 0x97, 0x20);    // RGB Mode, RGB565

    caminf_mt9d111_inf_axis_0[1] = 0;
    
    // Camera Base Address Setting
    caminf_mt9d111_inf_axis_0[0] = (unsigned)phys_addr+3*SVGA_ALL_DISP_ADDRESS;; // Camera Interface start (Address is dummy)

    // bitmap display controller settings
    bmdc_axi_lites0[0] = (unsigned)phys_addr; // Bitmap Display Controller 0 start, My Camera Image
    c = getc(stdin);
    while(c != 'q'){
        switch ((char)c) {
            case '1' :
                bmdc_axi_lites0[0] = (unsigned)phys_addr; // Bitmap Display Controller 0 start, My Camera Image
                break;
            case '2' :
                bmdc_axi_lites0[0] = (unsigned)phys_addr+3*SVGA_ALL_DISP_ADDRESS+0xc; // Another one ZYBO Camera Image
                break;
            case 'l' : // laplacian filter
                // caminf_axis_switch_1, 1to2 ,Select M01_AXIS
                // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
                caminf_axis_switch_1[16] = 0x80000000; // 0x40 = 0x80000000; disable
                caminf_axis_switch_1[17] = 0; // 0x44 = 0;
                caminf_axis_switch_1[0] = 0x2; // 0x0 = 2; Commit registers
    
                // laplacian filter AXIS Start
                laps_cntrl = caminf_lap_filter_axis_0[0] & 0x80; // Auto Restart bit
                caminf_lap_filter_axis_0[0] = laps_cntrl | 0x01; // Start bit set
                caminf_lap_filter_axis_0[0] = 0x80; // Auto Restart bit set
    
                // caminf_axis_switch_0, 2to1, Select S01_AXIS
                // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
                caminf_axis_switch_0[16] = 0x1; // 0x40 = 0x1;
                caminf_axis_switch_0[0] = 0x2; // 0x0 = 2; Commit registers
                break;
            case 'c' : // camera image
                // caminf_axis_switch_1, 1to2 ,Select M01_AXIS
                // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
                caminf_axis_switch_1[16] = 0; // 0x44 = 0;
                caminf_axis_switch_1[17] = 0x80000000; // 0x40 = 0x80000000; disable
                caminf_axis_switch_1[0] = 0x2; // 0x0 = 2; Commit registers
    
                // laplacian filter AXIS Start
                caminf_lap_filter_axis_0[0] = 0x00; // Auto Restart Disable
    
                // caminf_axis_switch_0, 2to1, Select S01_AXIS
                // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
                caminf_axis_switch_0[16] = 0x0; // 0x40 = 0x0;
                caminf_axis_switch_0[0] = 0x2; // 0x0 = 2; Commit registers
                break;
        }
        c = getc(stdin);
    }

    munmap((void *)bmdc_axi_lites0, 0x10000);
    munmap((void *)caminf_axi_vdma_0, 0x10000);
    munmap((void *)dviin_axi_vdma_0, 0x10000);
    munmap((void *)caminf_axi_iic, 0x10000);
    munmap((void *)caminf_mt9d111_inf_axis_0, 0x10000);
    munmap((void *)caminf_axis_switch_0, 0x10000);
    munmap((void *)caminf_axis_switch_1, 0x10000);
    munmap((void *)caminf_lap_filter_axis_0, 0x10000);
    munmap((void *)frame_buffer, (XGA_ALL_DISP_ADDRESS*NUMBER_OF_WRITE_FRAMES)+(SVGA_ALL_DISP_ADDRESS*NUMBER_OF_WRITE_FRAMES));
    
    close(fd0);
    close(fd1);
    close(fd2);
    close(fd3);
    close(fd4);
    close(fd5);
    close(fd6);
    close(fd7);
    close(fd9);
    close(fd10);
    
    return(0);
}