並列ステレオカメラによる距離の測定7（ZYBO 0 のハードウェア6）完成2

”並列ステレオカメラによる距離の測定6（ZYBO 0 のハードウェア5）完成”の続き。

前回はZYBO でUbuntu を起動して、そこからカメラ出力かラプラシアンフィルタ出力かを選択できるようにしようということで終わったが、ベアメタル・アプリとしてスイッチでカメラ出力とラプラシアンフィルタ出力を切り替えても良いのじゃないか？ということで、スライドSWの 0 番めが 0 だったらカメラ出力、1 だったらラプラシアンフィルタ出力にすることにした。

まずは、ブロックデザインで、AXI　GPIO をAdd IP した。

axi_gpio をダブル・クリックして、設定を行った。
All Inputs のチェックボックスをチェックして、GPIO Width を 1 に設定した。


Run Connection Automatin をクリックして、AXI4 Lite Slave バスとGPIO のポートを自動的に接続した。GPIO ポートは sw という名前に変更した。


Address Editor の設定を示す。 axi_gpio_0 は 0x41200000 にマップされた。


ブロックデザインをセーブした。


Flow Navigator -> Synthesis -> Run Synthesis をクリックして、論理合成を行った。


論理合成が成功した。
Synthesised Desgin を開いて、Layout メニューから I/O Planning を選択した。
Package ビューが開いた。
sw_tri_i[0] をG15 に割り当てて、IO Std はLVCMOS33 に設定した。


Flow Navigator -> Program and Debug -> Generate Bitstream をクリックして、インプリメント、ビットストリームの生成を行った。
ビットストリームの生成まで成功した。


Project Summary を示す。 LUT が 1% 増えている。


ハードウェアをエクスポートして、SDKを立ち上げよう。

File メニューから Exprot -> Export Hardware... を選択した。

Export Hardware ダイアログが表示される。Include bitstream にチェックを入れて、OKボタンをクリックした。

File メニューから Launch SDKを選択した。

SDKが立ち上がった。

SDK の File メニューからNew -> Application Project を選択する。

Project name に cam_lap_disp と入力した。Next > ボタンをクリックした。OS Platform は取りあえず、ベアメタル・アプリケーションとするので、デフォルトの standalone に設定した。
Board Support Package は Create New のラジオボタンをチェックして、BSPを新しく生成する。
BSPを新しく生成すると gpio のベアメタル・ドライバを参照することができる。


SDK使ったベアメタルアプリケーションのXilinx IPのドライバは、Vivadoでハードウェアをエクスポートした後に作ったBSPのps7_cortex9_? -> libsrc -> Xilinx IP名にある。
今回は gpio_v4_0 -> src にドライバのソースがある。


gpio のドライバを使用して、gpio を制御するが、制御方法は”ZedBoard Linux のフレームバッファにカメラ画像を表示８（カメラ画像表示成功）”を参考にした。

cam_lap_disp.c を新しく生成して、Cソースコードを書いて、オート・ビルドした。


Xilinx Tools メニューからProgram FPGA を選択して、ZYBO のコンフィギュレーションを行った。

cam_lap_disp.elf を右クリックし、右クリックメニューから Run AS -> Launch on Hardware (GDB) を選択して、ソフトウェアを起動すると、スライドSWの 0 番めが 0 の時がカメラ出力、1 の時がラプラシアンフィルタ出力された。成功だ。

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

// cam_lap_disp.c
// 2015/11/25 by marsee
//
// Refered to Xilinx\SDK\2015.1\data\embeddedsw\XilinxProcessorIPLib\drivers\axivdma_v5_1\doc\html\api
// Refered to https://github.com/elitezhe/Atyls-VDMA-one-in-one-out/blob/master/SDK/colorbar/src/helloworld.c
// Refered to http://www.xilinx.com/support/documentation/ip_documentation/axi_vdma/v6_2/pg020_axi_vdma.pdf
// Refered to http://forums.xilinx.com/t5/Embedded-Processor-System-Design/Axi-VDMA-on-Digilent-Atlys/td-p/297019/page/2
//
// if sw0 is 0 level then normal camera out else laplacian filter out
//

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

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

#define HORIZONTAL_PIXELS    800
#define VERTICAL_LINES        600
#define PIXEL_NUM_OF_BYTES    4

#define FRAME_BUFFER_ADDRESS 0x10000000

static XAxiVdma_DmaSetup Vdma0_WriteCfg;

void camera_out(XLap_filter_axis Xlap_fil_axis);
void lap_fil_out(XLap_filter_axis Xlap_fil_axis);

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(){
    // malloc frame buffer
    // unsigned int *frame_buffer = (unsigned int *)malloc(HORIZONTAL_PIXELS * VERTICAL_LINES * PIXEL_NUM_OF_BYTES * NUMBER_OF_WRITE_FRAMES);

    // AXI VDMA Initialization sequence
    XAxiVdma_Config *XAxiVdma0_Config;
    XAxiVdma XAxiVdma0;
    int XAxiVdma0_Status;
    static XGpio GPIOInstance_Ptr;
    int XGpio_Status;
    XLap_filter_axis Xlap_fil_axis;
    XLap_filter_axis_Config *Xlap_fil_axisPtr;
    int sw0_status, sw0_status_old;

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

    XAxiVdma0_Status = XAxiVdma_CfgInitialize(&XAxiVdma0, XAxiVdma0_Config, XAxiVdma0_Config->BaseAddress); // Initialize the driver with hardware configuration
    if (XAxiVdma0_Status != XST_SUCCESS){
        fprintf(stderr, "XAxiVdma_CfgInitialize() failed\n");
        return(-1);
    }

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

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

    Vdma0_WriteCfg.VertSizeInput = VERTICAL_LINES;
    Vdma0_WriteCfg.HoriSizeInput = HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES;
    Vdma0_WriteCfg.Stride = HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES; // Indicates the number of address bytes between the first pixels of each video line.
    Vdma0_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.
    Vdma0_WriteCfg.EnableCircularBuf = 1; // Indicates frame buffer Circular mode or frame buffer Park mode.  1 = Circular Mode Engine continuously circles through frame buffers.
    Vdma0_WriteCfg.EnableSync = 0; // Enables Genlock or Dynamic Genlock Synchronization. 0 = Genlock or Dynamic Genlock Synchronization disabled.
    Vdma0_WriteCfg.PointNum = 0; // No Gen-Lock
    Vdma0_WriteCfg.EnableFrameCounter = 0; // Endless transfers
    Vdma0_WriteCfg.FixedFrameStoreAddr = 0; // We are not doing parking

    XAxiVdma0_Status = XAxiVdma_DmaConfig(&XAxiVdma0, XAXIVDMA_WRITE, &Vdma0_WriteCfg);
    if (XAxiVdma0_Status != XST_SUCCESS){
        fprintf(stderr, "XAxiVdma_DmaConfig() failed\n");
        return(-1);
    }

    // Frame buffer address set
    unsigned int frame_addr = (unsigned int)FRAME_BUFFER_ADDRESS;
    int i;
    for (i=0; i<NUMBER_OF_WRITE_FRAMES; i++){
        Vdma0_WriteCfg.FrameStoreStartAddr[i] = frame_addr;
        frame_addr += HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES * VERTICAL_LINES;
    }

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

    // 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

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

    // mt9d111_inf_axis_0, axi_iic_0, bitmap_disp_cntrler_axi_master_0
    volatile unsigned int *bmdc0_axi_lites;
    volatile unsigned int *bmdc1_axi_lites;
    volatile unsigned int *mt9d111_axi_lites;
    volatile unsigned int *mt9d111_i2c_axi_lites;

    bmdc0_axi_lites = (volatile unsigned *)XPAR_BITMAP_DISP_CNTRLER_AXI_MASTER_0_BASEADDR;
    bmdc1_axi_lites = (volatile unsigned *)XPAR_BITMAP_DISP_CNTRLER_AXI_MASTER_1_BASEADDR;
    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;

    bmdc0_axi_lites[0] = (volatile unsigned int)FRAME_BUFFER_ADDRESS; // Bitmap Display Controller 0 start
    bmdc1_axi_lites[0] = (volatile unsigned int)FRAME_BUFFER_ADDRESS; // Bitmap Display Controller 1 start
    mt9d111_axi_lites[0] = (volatile unsigned int)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, 0xf0, 0x1);      // Changed regster map to IFP page 1
    cam_i2c_write(mt9d111_i2c_axi_lites, 0xba, 0x97, 0x20);        // RGB Mode, RGB565

    mt9d111_axi_lites[1] = 0; // One_shot_mode is disabled

    // AXI GPIO Initialization
    XGpio_Status = XGpio_Initialize(&GPIOInstance_Ptr,XPAR_AXI_GPIO_0_DEVICE_ID);
    if(XST_SUCCESS != XGpio_Status)
        print("GPIO INIT FAILED\n\r");
    // AXI GPIO Set the Direction(input setting)
    XGpio_SetDataDirection(&GPIOInstance_Ptr, 1, 1);

    // Look Up the device configuration
    Xlap_fil_axisPtr = XLap_filter_axis_LookupConfig(0);
    if (!Xlap_fil_axisPtr){
        fprintf(stderr, "XLap_filter_axis configuration failed.\n");
        return(-1);
    }

    // Initialize the Device
    int Xlap_status = XLap_filter_axis_CfgInitialize(&Xlap_fil_axis, Xlap_fil_axisPtr);
    if (Xlap_status != XST_SUCCESS){
        fprintf(stderr, "Could not Initialize XLap_filter_axis\n");
        return(-1);
    }

    sw0_status_old = XGpio_DiscreteRead(&GPIOInstance_Ptr, 1) & 1;
    usleep(5000); // wait 5ms

    while(1){
        sw0_status = XGpio_DiscreteRead(&GPIOInstance_Ptr, 1) & 1;
        if (sw0_status_old != sw0_status){ // sw0 status changed
            if (sw0_status == 0) // camera output
                camera_out(Xlap_fil_axis);
            else // laplacian filter output
                lap_fil_out(Xlap_fil_axis);
            sw0_status_old = sw0_status;
        }
        usleep(5000); // wait 5ms
    }

    return(0);
}

void camera_out(XLap_filter_axis Xlap_fil_axis){
    // 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

    // laplacian filter AXIS AutoRestart disable
    XLap_filter_axis_DisableAutoRestart(&Xlap_fil_axis);

    // 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
}

void lap_fil_out(XLap_filter_axis Xlap_fil_axis){
    // axis_switch_1, 1to2 ,Select M01_AXIS
    // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
    Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x40), 0x80000000); // disable
    Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x44), 0);
    Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR), 0x2); // Commit registers

    // laplacian filter AXIS Start
    XLap_filter_axis_Start(&Xlap_fil_axis);
    XLap_filter_axis_EnableAutoRestart(&Xlap_fil_axis);

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