FC2カウンター FPGAの部屋 ステレオカメラによる画像解析
fc2ブログ

FPGAやCPLDの話題やFPGA用のツールの話題などです。 マニアックです。 日記も書きます。

FPGAの部屋

FPGAの部屋の有用と思われるコンテンツのまとめサイトを作りました。Xilinx ISEの初心者の方には、FPGAリテラシーおよびチュートリアルのページをお勧めいたします。

ステレオカメラによる距離測定テスト12(焦点距離の計算)

ステレオカメラによる距離測定テスト11(距離と左右視差の測定)”の続き。

前回は距離と左右カメラの視差のピクセル数を測定した。今回はその測定データからカメラの焦点距離を計算した。

(2016/02/17:追記) 光学系も素人なので、勉強しながらやっていきたいと思いますので、この辺りはトライ・アンド・エラーしています。そして厳しいご指摘のコメントをお待ちしていおります。できれば詳しくやり方をコメント欄に書いて頂くか?参考にできるURLを教えて頂けると助かります。

ステレオカメラによる交通環境認識と歩行者の検出”の12ページの”視差と物体までの距離の関係”の図を下に引用する。
StereoCamTest_79_160215.png

私が測定した距離は焦点距離 + Z になっているので、焦点距離を計算で求めてみよう。測定した距離をLとすると、
F + Z = L

F + BF/D = (FD + FB) / D = F(D + B) / D = L

F = LD / (D + B)

この式に従って、各測定した距離の焦点距離を計算してみた。
StereoCamTest_80_160215.png

距離が短い方はだいぶ誤差が出ている気がする。カメラの軸が曲がっているのだろうか?焦点距離は 540 mm 位なのだろうか?
2.2 m よりも長い距離の焦点距離がどうなるか?が気になる。

次は、自動的に視差を求めるアプリケーションを作ってみたいと思う。
  1. 2016年02月16日 04:40 |
  2. ステレオカメラによる画像解析
  3. | トラックバック:0
  4. | コメント:7

ステレオカメラによる距離測定テスト11(距離と左右視差の測定)

ステレオカメラによる距離測定テスト10(左右カメラの視差の測定)”の続き。

左右画像を重ね合わせるアプリケーションが出来たので、カメラから物体までの距離と左右の視差による左右カメラ画像のピクセル位置のズレのデータを取ってみた。

そのデータを下に示す。
距離 (m)左右視差
(pixels)
上下視差
(Lines)
0.5304-11
0.6249-14
0.7213-14
0.8183-14
0.9161-14
1.0149-14
1.2119-20
1.499-20
1.590-20
1.681-20
1.869-20
2.061-20
2.257-20

なお、上下視差は測った時期によって、ばらついているかもしれない。
左右カメラ間の距離は 160 mm だった。
  1. 2016年02月15日 05:09 |
  2. ステレオカメラによる画像解析
  3. | トラックバック:0
  4. | コメント:0

ステレオカメラによる距離測定テスト10(左右カメラの視差の測定)

ステレオカメラによる距離測定テスト9(アプリケーションの作製2)”の続き。

左右カメラの視差の測定を行うアプリケーションを作っていたのだが、取りあえず完成した。左目カメラ画像に右目カメラ画像の一部を表示するアプリケーションだ。右目カメラ画像の一部を表示するエリアは、左目カメラ画像の輝度を 1/2 にして、そこに輝度を 1/2 にした右目カメラ画像を足し合わせる。デフォルトでは、右目カメラ画像の縦方向真ん中で、横方向左端の 160 ピクセル X 120 行を切り取って、左目カメラ画像に表示する。
アプリケーションのファイル名はRL_alpha_blend.c だ。オプションは5つあって、

c : 右目カメラ画像の横方向の幅を指定する(初期値: 160 ピクセル)
r : 右目カメラ画像の縦方向の幅を指定する(初期値: 120 行)
d : 右目カメラ画像の横方向の切り取り位置(初期値: 0 ピクセル)
s : 右目カメラ画像の縦方向の切り取り位置(初期値: 0 ピクセル)

となっている。

右目カメラ画像を出力するために、ZYBO_0_2 で ./udmabuf_insmod, ./cam_disp_vdma を起動して、HDMI 端子に右目カメラ画像を出力した。

左目カメラ画像を出すためにには、StereoCamTest で ./udmabuf_insmod, ./StereoCam_Alt_Disp を実行した。
左目カメラ画像を示す。
StereoCamTest_74_160214.jpg

左目カメラ画像用ZYBO に転送されてきた右目カメラ画像を示す。
StereoCamTest_75_160214.jpg

右目カメラ画像用、左目カメラ画像用ZYBO の様子を示す。
StereoCamTest_73_160214.jpg

次に、StereoCamTest の左目カメラ画像用ZYBO で、./RL_alpha_blend でアプリケーションを立ち上げた。
StereoCamTest_70_160214.png

左目カメラ画像に右目カメラ画像の一部が表示されているのが分かる。ピンクの枠で囲った部分が右目カメラ画像の一部を傘な合わせた部分だ。
StereoCamTest_76_160214.jpg

右目カメラ画像の一部を RL_alpha_blead アプリケーション上でコマンドを入力して位置を合わせる。コマンドを紹介する。

j : 左へ 1 ピクセル移動
k : 右へ 1 ピクセル移動
h : 左へ 10 ピクセル移動
l (Lの小文字): 右へ 10 ピクセル移動
g : 左へ 30 ピクセル移動
; : 右へ 30 ピクセル移動
i : 上へ 1 ピクセル移動
u : 上へ 10 ピクセル移動
m : 下へ 1 ピクセル移動
n : 下へ 10 ピクセル移動


右目カメラ画像を移動させる途中の画面を示す。
StereoCamTest_71_160214.png

StereoCamTest_77_160214.jpg

最初に比べて、右目カメラ画像の一部が移動しているのがわかると思う。

右目カメラ画像の一部を左目カメラ画像と一致された状態を示す。
StereoCamTest_72_160214.png

StereoCamTest_78_160214.jpg

右目カメラ画像の一部を 43 ピクセル移動させると左目カメラ画像に一致した。なお、ZYBO からの一致させたオブジェクトまでの距離は 2.3 m だった。

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

//
// RL_alpha_blend.c  by marsee
// 2016/02/06
//
// 800 pixels x 600 lines x 4 bytes x 3 pictures + 1024 pixels x 768 lines x 4 bytes x 3 pictures +
//   800 pixels x 600 lines x 4 bytes(alpha blend area for Right camera and Left camera)  = 17117184
//
// When you press the j key, right camera image is moved one pixel to the left.
// When you press the k key, right camera image is moved one pixel to the right.
// When you press the h key, right camera image is moved ten pixels to the left.
// When you press the l key, right camera image is moved ten pixels to the right.
// When you press the i key, right camera image is moved one pixel to the up.
// When you press the m key, right camera image is moved one pixel to the down.
//
// Usage : RL_alpha_blend [-c 80] [-r 60] [-h]
// -c and -r indicates the area to be cut out of the left edge of the right-eye camera image.
//

#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 XGA_3_PICTURES          (XGA_ALL_DISP_ADDRESS * NUMBER_OF_WRITE_FRAMES) 

#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)
#define SVGA_3_PICTURES         (SVGA_ALL_DISP_ADDRESS * NUMBER_OF_WRITE_FRAMES)

#define DEFAULT_RECTANGLE_PIXELS    160
#define DEFAULT_RECTANGLE_LINES     120

int half_bright(int pixel);
int alpha_blend(int pixel1, int pixel2);

int main(int argc, char *argv[]){
    int column_width = DEFAULT_RECTANGLE_PIXELS;
    int row_width = DEFAULT_RECTANGLE_LINES;
    int opt, c;
    int help_flag = 0;
    int fdf, fdp, fd6;
    volatile unsigned *frame_buffer;
    volatile unsigned *bmdc_axi_lites0;
    unsigned char  attr[1024];
    unsigned long  phys_addr;
    volatile unsigned *Leye_addr, *alpha_addr;
    int i, j;
    volatile unsigned *Reye_start_addr, *Reye_stride;
    volatile unsigned *abdisp_start_addr, *abdisp_addr;
    volatile unsigned *s, *t;
    unsigned int   buf_size;
    int offset_row=0, offset_column=0;
    int reye_img_offset_row=0, reye_img_offset_column=0;
    
    while ((opt=getopt(argc, argv, "c:r:d:s:h")) != -1){
        switch (opt){
            case 'c':
                column_width = atoi(optarg);
                break;
            case 'r':
                row_width = atoi(optarg);
                break;
            case 'd':
                reye_img_offset_column = atoi(optarg);
                break;
            case 's':
                reye_img_offset_row = atoi(optarg);
                break;
            case 'h':
                help_flag = 1;
                break;
        }
    }

    printf("column_width = %d, row_width = %dn", column_width, row_width);
    printf("reye_img_offset_column = %d, reye_img_offset_row = %dn", reye_img_offset_column, reye_img_offset_row);
    
    if (help_flag == 1){ // help
        printf("Usage : RL_alpha_blend [-c 160] [-r 120] [-d 0] [-s 0] [-h]n");
        printf("-c: column_width, -r: row_widthn");
        printf("-d: column of right eys's camere image offset, -s: row of right eys's camere image offsetn");
        printf("-c and -r indicates the area to be cut out of the left edge of the right-eye camera image.n");
        printf("n");
        printf("Key bindn");
        printf("j: left 1 pixelnk: right 1 pixelnh: left 10 pixelsnl: right 10 pixelsn");
        printf("g: left 30 pixelsn';': right 30 pixelsn");
        printf("i: up 1 pixelnu: up 10 pixelsnm: down 1 pixelnn: down 10 pixelsn");
        return(0);
    }

    // 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 errorn");
        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 errorn");
        exit(-1);
    }

    // udmabuf0
    fdf = open("/dev/udmabuf0", O_RDWR | O_SYNC); // frame_buffer, The chache is disabled. 
    if (fdf == -1){
        fprintf(stderr, "/dev/udmabuf0 open errorn");
        exit(-1);
    }
    frame_buffer = (volatile unsigned *)mmap(NULL, SVGA_3_PICTURES+XGA_3_PICTURES+SVGA_ALL_DISP_ADDRESS, PROT_READ|PROT_WRITE, MAP_SHARED, fdf, 0);
    if (!frame_buffer){
        fprintf(stderr, "frame_buffer mmap errorn");
        exit(-1);
    }

    // phys_addr of udmabuf0
    fdp = open("/sys/devices/virtual/udmabuf/udmabuf0/phys_addr", O_RDONLY);
    if (fdp == -1){
        fprintf(stderr, "/sys/devices/virtual/udmabuf/udmabuf0/phys_addr open errorn");
        exit(-1);
    }
    read(fdp, attr, 1024);
    sscanf(attr, "%lx", &phys_addr);  
    close(fdp);
    printf("phys_addr = %xn", (int)phys_addr);

    // buf_size of udmabuf0
    fdp = open("/sys/devices/virtual/udmabuf/udmabuf0/size", O_RDONLY);
    if (fdp == -1){
        fprintf(stderr, "/sys/devices/virtual/udmabuf/udmabuf0/size open errorn");
        exit(-1);
    }
    read(fdp, attr, 1024);
    sscanf(attr, "%d", &buf_size);  
    close(fdp);
    printf("buf_size = %dn", (int)buf_size);
    
    // Display the left-eye camera image
    Leye_addr = frame_buffer;
    alpha_addr = (volatile unsigned *)((unsigned)frame_buffer+SVGA_3_PICTURES+XGA_3_PICTURES);

    s = alpha_addr;
    t = Leye_addr;
    for (i=0; i<SVGA_VERTICAL_LINES; i++){
        for (j=0; j<SVGA_HORIZONTAL_PIXELS; j++){
            *s =  *t;
            s++;
            t++;
        }
    }
    bmdc_axi_lites0[0] = (unsigned)phys_addr+SVGA_3_PICTURES+XGA_3_PICTURES;
    
    // Display the right eye camera image, taken in a rectangular
    Reye_start_addr = (volatile unsigned *)((unsigned)frame_buffer + SVGA_3_PICTURES + (SVGA_HORIZONTAL_PIXELS * (SVGA_VERTICAL_LINES - row_width)/2 * PIXEL_NUM_OF_BYTES)+0x8+reye_img_offset_column+(SVGA_HORIZONTAL_PIXELS * reye_img_offset_row * PIXEL_NUM_OF_BYTES)); // left edge + offset
    Reye_stride = (volatile unsigned *)((SVGA_HORIZONTAL_PIXELS - column_width)*PIXEL_NUM_OF_BYTES);

    abdisp_start_addr = (volatile unsigned *)((unsigned)frame_buffer + SVGA_3_PICTURES + XGA_3_PICTURES+ (SVGA_HORIZONTAL_PIXELS * (SVGA_VERTICAL_LINES - row_width)/2 * PIXEL_NUM_OF_BYTES));

    abdisp_addr = abdisp_start_addr;
    s = abdisp_addr;
    t = Reye_start_addr;
    
    for (i=0; i<row_width; i++){
        for (j=0; j<column_width; j++){
            *s = alpha_blend(*s, *t);
            s++;
            t++;
        }
        s = (volatile unsigned *)((unsigned)s+ (unsigned)Reye_stride);
        t = (volatile unsigned *)((unsigned)t + (unsigned)Reye_stride);
    }
    
    printf("offset_row = %d pixels, offset_column = %d linesn", offset_row, offset_column);
    
    // j, k, h, l, i, m , q key
    while(c != 'q'){
        c = getc(stdin);
        switch ((char)c) {
            case 'j' :  // left 1 pixel
                abdisp_addr--;
                offset_row--;
                break;
            case 'k' :  // right 1 pixel
                abdisp_addr++;
                offset_row++;
                break;
            case 'h' :  // left 10 pixels
                abdisp_addr -= 10;
                offset_row -= 10;
                break;
            case 'l' :  // right 10 pixels
                abdisp_addr += 10;
                offset_row += 10;
                break;
            case 'g' : // left 30 pixels
                abdisp_addr -= 30;
                offset_row -= 30;
                break;          
            case ';' :  // right 30 pixels
                abdisp_addr += 30;
                offset_row += 30;
                break;
            case 'i' :  // up 1 pixel
                abdisp_addr -= SVGA_HORIZONTAL_PIXELS;
                offset_column--;
                break;
            case 'u' :  // up 10 pixel
                abdisp_addr -= (SVGA_HORIZONTAL_PIXELS*10);
                offset_column -= 10;
                break;
            case 'm' :  // down 1 pixel
                abdisp_addr += SVGA_HORIZONTAL_PIXELS;
                offset_column++;
                break;
            case 'n' :  // down 10 pixel
                abdisp_addr += (SVGA_HORIZONTAL_PIXELS*10);
                offset_column += 10;
                break;
        }

        // Display the left-eye camera image
        s = alpha_addr;
        t = Leye_addr;
        for (i=0; i<SVGA_VERTICAL_LINES; i++){
            for (j=0; j<SVGA_HORIZONTAL_PIXELS; j++){
                *s =  *t;
                s++;
                t++;
            }
        }

        // Display the right eye camera image, taken in a rectangular
        s = abdisp_addr;
        t = Reye_start_addr;
        for (i=0; i<row_width; i++){
            for (j=0; j<column_width; j++){
                *s = alpha_blend(*s, *t);
                s++;
                t++;
            }
            s = (volatile unsigned *)((unsigned)s+ (unsigned)Reye_stride);
            t = (volatile unsigned *)((unsigned)t + (unsigned)Reye_stride);
        }
        if (c != 'n')
            printf("offset_row = %d pixels, offset_column = %d linesn", offset_row, offset_column);
    }
    munmap((void *)bmdc_axi_lites0, 0x10000);
    munmap((void *)frame_buffer, (SVGA_3_PICTURES+XGA_3_PICTURES+SVGA_ALL_DISP_ADDRESS));
    close(fd6);
    close(fdf);
    
    return(0);
}

int half_bright(int pixel){
    int hpixel = 0;

    hpixel = (pixel & 0xff)/2;
    hpixel |= (pixel & 0xff00)/2;
    hpixel |= (pixel & 0xff0000)/2;

    return(hpixel);
}

int alpha_blend(int pixel1, int pixel2){
    int r, g, b;

    b = (pixel1 & 0xff)/2 + (pixel2 & 0xff)/2;
    g = ((pixel1 & 0xff00)/2 + (pixel2 & 0xff00)/2) & 0xff00;
    r = ((pixel1 & 0xff0000)/2 + (pixel2 & 0xff0000)/2) & 0xff0000;
    return(b+g+r);
}

  1. 2016年02月14日 06:06 |
  2. ステレオカメラによる画像解析
  3. | トラックバック:0
  4. | コメント:0

ステレオカメラによる距離測定テスト9(アプリケーションの作製2)

”ステレオカメラによる距離測定テスト8(アプリケーションの作製)”の続き。

前回は、StereoCam_Alt_Disp を作製して、自分の左目カメラ画像とHDMI ポート経由で送られてきた右目カメラ画像を選択して表示することができるようになった。
今回は、左目カメラ画像に真ん中のある領域を切り出した右目カメラ画像を重ねて表示してみることにした。
まずは、左目カメラ画像をSVGA画面にすべて表示する。次に、真ん中のある領域を切り出した右目カメラ画像を左端の上下方向は真ん中に表示する。
その表示する時に、表示する位置の左目カメラ画像のピクセルを1/2 にしたものと、切り出した右目カメラ画像の当該ピクセルに 1/2 を掛けたものを足し算して、表示しようと思う。切り出した右目カメラ画像はキー操作で動くようにする。キーバインドを下に示す。

j キーで 1 ピクセル左へ移動
k キーで 1 ピクセル右へ移動
h キーで 10 ピクセル左へ移動
l (Lの小文字)キーで 10 ピクセル右へ移動
i キーで 1 ピクセル上へ移動
m キーで 1 ピクセル下に移動


ファイル名はRL_alpha_blend.c とした。
gcc RL_alpha_blend.c -o RL_alpha_blend
でコンパイルを行った。
./RL_alpha_blend
でアプリケーションを起動したが、Segmentation fault だった。デバックする必要があるが、GDBだとコマンド打つのが面倒なので、GDBのフロントエンドをインストールすることにした。
StereoCamTest_66_160209.png
  1. 2016年02月09日 05:29 |
  2. ステレオカメラによる画像解析
  3. | トラックバック:0
  4. | コメント:0

ステレオカメラによる距離測定テスト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の写真を示す。
StereoCamTest_49_160128.jpg

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

左目用のZYBO は
.udmabuf_insmod
を起動してから、
./StereoCam_Alt_Disp
を起動した。
StereoCamTest_61_160203.png
上図の右上が右目用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 とリターンキーで、アプリケーションを終了する。

という処理を行う。

左目カメラ画像を示す。
StereoCamTest_62_160203.jpg

右目カメラ画像を示す。
StereoCamTest_63_160203.jpg

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

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

暗くなっているので、エッジが見にくくなっている。こういう場合は、以前作成したアンシャープマスキング・フィルタ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, 0xf00x1);        // Changed regster map to IFP page 1
    cam_i2c_write(caminf_axi_iic, 0xba, 0x970x20);    // 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);
}

  1. 2016年02月03日 04:50 |
  2. ステレオカメラによる画像解析
  3. | トラックバック:0
  4. | コメント:0

ステレオカメラによる距離測定テスト7(BOOT.bin, devicetree.dtb)

ステレオカメラによる距離測定テスト6(ステレオカメラのテスト)”の続き。

ベアメタル・アプリケーションでStereo Camera のテストが上手く行ったので、次は、Linux のアプリケーションとして実装したい。ということで、今回はBOOT.bin と devicetree.dtb の作製を行った。

最初にFSBL を作製した。
StereoCamTest_53_160130.png

次に、u-boot.elf を追加して、Boot Image (BOOT.bin) を作製した。
StereoCamTest_54_160130.png

BOOT.bin が作製された。
StereoCamTest_55_160130.png

次に、devicetree.dtb を作る。VirtualBox 上のUbuntu 14.04 LTS 上で作業する。

~/ZYBO/Digilent_Linux_Tutrial/drivers_StereoCamTest ディレクトリ上で作業した。

Vivado and zybo linux勉強会資料3”の zynq-zybo.dts に uio のエントリを追加した。
StereoCamTest_56_160130.png

        caminf_axi_iic@0x41600000 {
            compatible = "generic-uio";
            reg = < 0x41600000 0x10000>;
        };
        caminf_axi_vdma_0@43010000 {
            compatible = "generic-uio";
            reg = < 0x43010000 0x10000 >;
        };
        caminf_axis_switch_0@0x43c10000 {
            compatible = "generic-uio";
            reg = < 0x43c10000 0x10000 >;
        };
        caminf_axis_switch_1@0x43c20000 {
            compatible = "generic-uio";
            reg = < 0x43c20000 0x10000 >;
        };
        caminf_lap_filter_axis_0@0x43c30000 {
            compatible = "generic-uio";
            reg = < 0x43c30000 0x10000>;
        };    
        caminf_mt9d111_inf_axis_0@0x43C40000 {
            compatible = "generic-uio";
            reg = < 0x43C40000 0x10000>;
        };
        bitmap_disp_cntrler_axi_master_0@0x43c00000 {
            compatible = "generic-uio";
            reg = < 0x43c00000 0x10000>;
        };
        dviin_axi_vdma_0@0x43000000 {
            compatible = "generic-uio";
            reg = < 0x43000000 0x10000>;
        };


../scripts/dtc/dtc -I dts -O dtb -o devicetree.dtb zynq-zybo.dts
を実行して、zynq-zybo.dts をコンパイルして、devicetree.dtb を生成した。
StereoCamTest_57_160130.png

ZYBO に挿入するMicroSD カードに、作製したBOOT.bin と devicetree.dtb をコピー&ペーストした。 uEnv.txt と uImage は以前のままとした。
StereoCamTest_58_160130.png

MicroSD カードをZYBO に入れて、SDカード・ブートに設定変更し、電源ONを行った。

Linux が起動して、/sys/devices/amba.0 を見ると、デバイスツリーに書いたデバイスが見えた。成功。。。
StereoCamTest_59_160130.png

/sys/devices/amba.0/41600000.caminf_axi_iic/uio ディレクトリに入って ls すると uio0 になっているのが見えた。デバイスツリーの順番の通りに uio の番号が設定されていた。
StereoCamTest_60_160130.png
  1. 2016年01月30日 20:57 |
  2. ステレオカメラによる画像解析
  3. | トラックバック:0
  4. | コメント:0

ステレオカメラによる距離測定テスト6(ステレオカメラのテスト)

ステレオカメラによる距離測定テスト5(インプリメント成功)”の続き。

かなり期間が空いてしまったが、前回はインプリメントが終了した。今回はSDKを立ち上げて、ベアメタル・アプリケーションを作製して自分のカメラと、ZYBO_0_2 からHDMI ポート経由で送られてきた画像が表示できるか?を確認する。

最初に、Vivado 2015.4で、ハードウェアをエクスポートし、SDKを立ち上げた。

次に、disp_test という名前のApplication Project を作製し、disp_test.c を書いた。
StereoCamTest_48_160128.png

もう片方のZYBO はZYBO_0_2 を起動して、カメラ画像をVGA ポートからSVGA 解像度で出力し、HDMI ポートからはSVGA のフレームバッファをXGA 解像度で送る。SDKでアプリケーションを起動するZYBOは、HDMI ポートからのXGA 解像度のSVGA 画像をDDR3 SDRAM 上のフレームバッファに転送するのと、自分のカメラの画像を異なるフレームバッファに転送する。そのどちらかのフレームバッファをVGA ポートに出力するという機能がある。
現在は、切り替えるフレームバッファは、どちらのフレームバッファのアドレスをビットマップ・ディスプレイ・コントローラのベース・アドレスとして、与えるかをコメント・アウトで切り替えて、再ビルドすることで、ディスプレイの表示を切り替える。

まず、やっている環境はこんな感じだ。カメラから椅子の一番近いところまでの距離は約 47 cm だった。
StereoCamTest_49_160128.jpg

下の画像で、左の小さいディスプレイは、左目に相当するカメラ(左目カメラ)で撮った画像で、右の大きなディスプレイは右目に相当するカメラで撮った画像を示す。右のディスプレイの画像は、ZYBO_0_2 が自分で取ったカメラ画像が表示されている(HDMI で転送した画像ではない)
StereoCamTest_50_160128.jpg

左目カメラだけ画像を示す。つまり、自分のカメラで取った画像だ。
StereoCamTest_51_160128.jpg

次に、HMDI 経由で送られてきたZYBO_0_2 の右目カメラが取った画像を示す。やはり、黒いドットが出ているのが分かる。
StereoCamTest_52_160128.jpg

最後に、disp_test.c を貼っておく。2種類の方法でAXI VDMA の設定を行っている。

/* * disp_test.c * *  Created on: 2016/01/20 *      Author: marsee */

#include <stdio.h>
#include <stdlib.h>
#include "xaxivdma.h"
#include "xil_io.h"
#include "xparameters.h"
#include "sleep.h"

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

#define PIXEL_NUM_OF_BYTES    4

#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)

#define DVI_INPUT_FRAME_BUFFER_ADDRESS 0x10000000
#define CAMERA_FRAME_BUFFER_ADDRESS 0x10900000

static XAxiVdma_DmaSetup Vdma_dvi_WriteCfg;
//static XAxiVdma_DmaSetup Vdma_cam_WriteCfg;

void cam_i2c_init(volatile unsigned *mt9d111_i2c_axi_lites) {
    mt9d111_i2c_axi_lites[64] = 0x2// reset tx fifo ,address is 0x100, i2c_control_reg
    mt9d111_i2c_axi_lites[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 *mt9d111_i2c_axi_lites, unsigned int device_addr, unsigned int write_addr, unsigned int write_data){
    mt9d111_i2c_axi_lites[66] = 0x100 | (device_addr & 0xfe);   // Slave IIC Write Address, address is 0x108, i2c_tx_fifo
    mt9d111_i2c_axi_lites[66] = write_addr;
    mt9d111_i2c_axi_lites[66] = (write_data >> 8)|0xff;         // first data
    mt9d111_i2c_axi_lites[66] = 0x200 | (write_data & 0xff);        // second data
    cam_i2x_write_sync();
}

int main(){
    // AXI VDMA Initialization sequence
    XAxiVdma_Config *XAxiVdma_dvi_Config;
    XAxiVdma XAxiVdma_dvi;
    int XAxiVdma_dvi_Status;
    //XAxiVdma_Config *XAxiVdma_cam_Config;
    //XAxiVdma XAxiVdma_cam;
    //int XAxiVdma_cam_Status;

    // AXI VDMA for dvi_input Initialization sequence
    XAxiVdma_dvi_Config = XAxiVdma_LookupConfig(XPAR_DVI_INPUT_AXI_VDMA_0_DEVICE_ID); // Look up the hardware configuration for a device instance
    if (XAxiVdma_dvi_Config == NULL){
        fprintf(stderr, "No AXI VDMA for dvi_input found\n");
        return(-1);
    }

    XAxiVdma_dvi_Status = XAxiVdma_CfgInitialize(&XAxiVdma_dvi, XAxiVdma_dvi_Config, XAxiVdma_dvi_Config->BaseAddress); // Initialize the driver with hardware configuration
    if (XAxiVdma_dvi_Status != XST_SUCCESS){
        fprintf(stderr, "XAxiVdma_CfgInitialize() for dvi_input failed\n");
        return(-1);
    }

    XAxiVdma_Reset(&XAxiVdma_dvi, XAXIVDMA_WRITE);
    while(XAxiVdma_ResetNotDone(&XAxiVdma_dvi, XAXIVDMA_WRITE)) ;

    XAxiVdma_dvi_Status = XAxiVdma_SetFrmStore(&XAxiVdma_dvi, NUMBER_OF_WRITE_FRAMES, XAXIVDMA_WRITE); // Set the number of frame store buffers to use.

    Vdma_dvi_WriteCfg.VertSizeInput = XGA_VERTICAL_LINES;
    Vdma_dvi_WriteCfg.HoriSizeInput = XGA_HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES;
    Vdma_dvi_WriteCfg.Stride = XGA_HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES; // Indicates the number of address bytes between the first pixels of each video line.
    Vdma_dvi_WriteCfg.FrameDelay = 0// Indicates the minimum number of frame buffers the Genlock slave is to be behind the locked master. This field is only used if the channel is enabled for Genlock Slave operations. This field has no meaning in other Genlock modes.
    Vdma_dvi_WriteCfg.EnableCircularBuf = 1// Indicates frame buffer Circular mode or frame buffer Park mode.  1 = Circular Mode Engine continuously circles through frame buffers.
    Vdma_dvi_WriteCfg.EnableSync = 0// Enables Genlock or Dynamic Genlock Synchronization. 0 = Genlock or Dynamic Genlock Synchronization disabled.
    Vdma_dvi_WriteCfg.PointNum = 0// No Gen-Lock
    Vdma_dvi_WriteCfg.EnableFrameCounter = 0// Endless transfers
    Vdma_dvi_WriteCfg.FixedFrameStoreAddr = 0// We are not doing parking

    XAxiVdma_dvi_Status = XAxiVdma_DmaConfig(&XAxiVdma_dvi, XAXIVDMA_WRITE, &Vdma_dvi_WriteCfg);
    if (XAxiVdma_dvi_Status != XST_SUCCESS){
        fprintf(stderr, "XAxiVdma_DmaConfig() failed\n");
        return(-1);
    }

    // Frame buffer(AXI4 VDMA of dvi_input) address set
    unsigned int frame_addr = (unsigned int)DVI_INPUT_FRAME_BUFFER_ADDRESS;
    int i;
    for (i=0; i<NUMBER_OF_WRITE_FRAMES; i++){
        Vdma_dvi_WriteCfg.FrameStoreStartAddr[i] = frame_addr;
        frame_addr += XGA_HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES * XGA_VERTICAL_LINES;
    }

    XAxiVdma_dvi_Status = XAxiVdma_DmaSetBufferAddr(&XAxiVdma_dvi, XAXIVDMA_WRITE, Vdma_dvi_WriteCfg.FrameStoreStartAddr);
    if (XAxiVdma_dvi_Status != XST_SUCCESS){
        fprintf(stderr, "XAxiVdma_DmaSetBufferAddr() failed\n");
        return(-1);
    }

    // VDMA start
    XAxiVdma_dvi_Status = XAxiVdma_DmaStart(&XAxiVdma_dvi, XAXIVDMA_WRITE);
    if (XAxiVdma_dvi_Status != XST_SUCCESS){
        fprintf(stderr, "XAxiVdma_DmaStart() for dvi_input failed\n");
        return(-1);
    }


    // AXI VDMA for Camera module setup
    // axis_switch_1, 1to2 ,Select M00_AXIS
    // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
    Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x40), 0x0);
    Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x44), 0x80000000); // disable
    Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR), 0x2); // Commit registers

    // axis_switch_0, 2to1, Select S00_AXIS
    // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
    Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR+0x40), 0x0);
    Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR), 0x2); // Commit registers

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

    // mt9d111_inf_axis_0, axi_iic_0
    volatile unsigned int *mt9d111_axi_lites;
    volatile unsigned int *mt9d111_i2c_axi_lites;

    mt9d111_axi_lites = (volatile unsigned *)XPAR_CAMERA_INTERFACE_MT9D111_INF_AXIS_0_BASEADDR;
    mt9d111_i2c_axi_lites = (volatile unsigned *)XPAR_CAMERA_INTERFACE_AXI_IIC_0_BASEADDR;

    mt9d111_axi_lites[0] = (volatile unsigned int)CAMERA_FRAME_BUFFER_ADDRESS; // Camera Interface start (Address is dummy)

    // CMOS Camera initialize, MT9D111
    cam_i2c_init(mt9d111_i2c_axi_lites);

    cam_i2c_write(mt9d111_i2c_axi_lites, 0xba, 0xf00x1);      // Changed regster map to IFP page 1
    cam_i2c_write(mt9d111_i2c_axi_lites, 0xba, 0x970x20);        // RGB Mode, RGB565

    mt9d111_axi_lites[1] = 0// One_shot_mode is disabled

    // bitmap_disp_cntrler_axi_master_0 : VGA port Display
    volatile unsigned int *bmdc0_axi_lites;
    bmdc0_axi_lites = (volatile unsigned *)XPAR_BITMAP_DISP_CNTRLER_AXI_MASTER_0_BASEADDR;
    //bmdc0_axi_lites[0] = (volatile unsigned int)CAMERA_FRAME_BUFFER_ADDRESS; // Bitmap Display Controller 0 start, Camera Image
    bmdc0_axi_lites[0] = (volatile unsigned int)(DVI_INPUT_FRAME_BUFFER_ADDRESS + 0xc); // Bitmap Display Controller 0 start, HDMI Image

    return(0);
}

  1. 2016年01月29日 03:32 |
  2. ステレオカメラによる画像解析
  3. | トラックバック:0
  4. | コメント:0