FC2カウンター FPGAの部屋 ステレオカメラによる距離測定テスト6(ステレオカメラのテスト)

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

FPGAの部屋

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

ステレオカメラによる距離測定テスト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

コメント

コメントの投稿


管理者にだけ表示を許可する

トラックバック URL
http://marsee101.blog19.fc2.com/tb.php/3380-4f396ac9
この記事にトラックバックする(FC2ブログユーザー)