ガボール・フィルタ (Gabor Filter) による白線検出11（その他の道路写真での検証）

”ガボール・フィルタ (Gabor Filter) による白線検出10（hls::LineBuffer と hls::Window を使用7）”の続き。

前回で、ガボール・フィルタは完成した（パラメータを調整する必要はあると思う）が、まだ1枚の道路の写真しか試していないので、もう少し違った写真で試してみることにした。

road_2 にガボール・フィルタをかけてみた。


左白線検出の固定小数点演算のガボール・フィルタ画像を左に、浮動小数点演算のガボール・フィルタ画像を右に示す。
　
この写真では、左白線の角度が予定と異なるので、検出が甘いというか、色が薄い。やはり、Zybot から撮影した画像でデータを取る必要がありそうだ。

次に、右白線検出の固定小数点演算のガボール・フィルタ画像を左に、浮動小数点演算のガボール・フィルタ画像を右に示す。
　
右白線は角度的に問題ない。


次に、road_3 でやってみた。


左白線検出の固定小数点演算のガボール・フィルタ画像を左に、浮動小数点演算のガボール・フィルタ画像を右に示す。
　
左白線は検出されているが、陰になったところで薄くなってしまった。

右白線検出の固定小数点演算のガボール・フィルタ画像を左に、浮動小数点演算のガボール・フィルタ画像を右に示す。
　
右白線の検出は問題ないようだ。


road_4 にガボール・フィルタをかけてみた。


左白線検出の固定小数点演算のガボール・フィルタ画像を左に、浮動小数点演算のガボール・フィルタ画像を右に示す。
　
問題なさそうだ。

右白線検出の固定小数点演算のガボール・フィルタ画像を左に、浮動小数点演算のガボール・フィルタ画像を右に示す。
　
こちらも、問題なさそうだ。


road_8 は田んぼのあぜ道のような白線が無い道で、これにガボール・フィルタをかけてみた。


左白線検出の固定小数点演算のガボール・フィルタ画像を左に、浮動小数点演算のガボール・フィルタ画像を右に示す。
　
うっすらとは道の形がわかるが、やはり無理そうだ。

右白線検出の固定小数点演算のガボール・フィルタ画像を左に、浮動小数点演算のガボール・フィルタ画像を右に示す。
　
やはり左と同様に、うっすらとは道の形がわかるが、やはり無理そうだ。

結論としては、Zybot で撮影した画像でガボール・フィルタのカーネルのパラメータを決定すべきだということが分かった。

ガボール・フィルタ (Gabor Filter) による白線検出10（hls::LineBuffer と hls::Window を使用7）

”ガボール・フィルタ (Gabor Filter) による白線検出9（hls::LineBuffer と hls::Window を使用6）”の続き。

前回は左白線検出用重みと右白線検出用重みを一緒の配列にしてみた。つまり重みを 3 次元配列にした。新しいGabor_filter_lh.h 、 Gabor_filter_lh.cpp 、 Gabor_filter_lh_tb.cpp を貼ったが、今回は、そのC ソースコードを使って、C シミュレーション、C コードの合成、C/RTL 協調シミュレーション、IP 化を行った。

まずは、右白線検出の C シミュレーションを行った。


前と同様の結果だった。ガボール・フィルタの出力画像も問題ない。

次に、左白線検出の C シミュレーションを行った。


こちらも、前と同様の結果だった。ガボール・フィルタの出力画像も問題ない。

次に C コードの合成を行った。今回の合成結果を左に、前回の合成結果を右に示す。
　

DSP48E の使用量は92 % で変化がないが、FF の使用量は15 % から 13 % に、LUT の使用量は 27 % から 22 % にそれぞれ今回の方が減少しているので、DSP48E の使用量が心配だが、これで行くことにする。

C/RTL 協調シミュレーションを行った。


Latency は 307239 クロックだった。これは、ほとんど 1 クロックで 1 ピクセルの処理ができていることになる。

C/RTL 協調シミュレーション波形を示す。


ins_TVALID, ins_TREADY , out_TVALID , out_TREADY がずっと 1 で、連続的に処理できていることがわかる。

最後に、IP 化を行った。

ガボール・フィルタ (Gabor Filter) による白線検出9（hls::LineBuffer と hls::Window を使用6）

”ガボール・フィルタ (Gabor Filter) による白線検出8（hls::LineBuffer と hls::Window を使用5）”の続き。

前回は、左白線検出と右白線検出ができる両方の白線検出用Gabor Filterを作成した。1ビットのRorL という信号を追加して、右白線検出か、左白線検出かを選べるようにした。C シミュレーション、C コードの合成を行った。
前回では、DSP48E を 92 % も使用していた。安易に左白線用の演算と右白線用の演算を if 文で切り替えてしまったからかもしれない？よって、今回は、左白線検出用重みと右白線検出用重みを一緒の配列にしてみた。つまり重みを 3 次元配列にした。新しいGabor_filter_lh.h 、 Gabor_filter_lh.cpp 、 Gabor_filter_lh_tb.cpp を貼っておく。


まずは、Gabor_filter_lh.h を示す。

// Gabor_filter_lh.h
// 2016/07/24
// 2016/07/25 : 右白線検出用のGabor Filterの重みを追加
// 2016/07/27 : 右白線検出用配列と左白線検出用配列を統合
//

#ifndef __Gabor_filter_lh_H__
#define __Gabor_filter_lh_H__

//#define HORIZONTAL_PIXEL_WIDTH    800
//#define VERTICAL_PIXEL_WIDTH    600

#define HORIZONTAL_PIXEL_WIDTH    640
#define VERTICAL_PIXEL_WIDTH    480

//#define HORIZONTAL_PIXEL_WIDTH    64
//#define VERTICAL_PIXEL_WIDTH    48

#define ALL_PIXEL_VALUE    (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)

#define ARRAY_SIZE    9
#define RIGHT_WEIGHT    1
#define LEFT_WEIGHT        0

const int gabor_weight[2][ARRAY_SIZE][ARRAY_SIZE] = { // 左白線検出用＋右白線検出用
    {
        {0,-2,-9,-17,-19,-13,-4,0,1},
        {-5,-15,-28,-30,-9,16,22,14,5},
        {-8,-12,5,57,112,115,69,24,4},
        {5,35,108,186,190,105,17,-16,-12},
        {20,59,101,82,-18,-109,-113,-61,-19},
        {10,9,-33,-125,-203,-186,-100,-29,-2},
        {-5,-27,-71,-111,-100,-43,5,16,9},
        {-5,-13,-19,-9,18,36,30,15,4},
        {-1,0,5,14,20,17,8,2,0}
    },
    {
        {0,3,11,10,-4,-15,-10,-2,1},
        {-4,-1,23,53,37,-11,-29,-14,-2},
        {-11,-26,-5,82,136,70,-15,-29,-10},
        {-8,-51,-93,-13,154,186,70,-11,-15},
        {7,-30,-133,-174,-18,154,136,37,-4},
        {16,18,-57,-183,-174,-13,82,53,10},
        {10,31,24,-57,-133,-93,-5,23,11},
        {1,13,31,18,-30,-51,-26,-1,3},
        {-1,1,10,16,7,-8,-11,-4,0}        
    }
};
const float gabor_fweight[2][ARRAY_SIZE][ARRAY_SIZE] = { // 左白線検出用＋右白線検出用(float)
    {
        {-0.000387,-0.009416,-0.034769,-0.067138,-0.075984,-0.04953,-0.015067,0.001684,0.003258},
        {-0.017724,-0.057864,-0.111235,-0.116438,-0.035878,0.061351,0.087704,0.054213,0.019079},
        {-0.031458,-0.048283,0.018458,0.22454,0.438323,0.448468,0.269024,0.09288,0.014896},
        {0.018804,0.137564,0.420198,0.727096,0.740714,0.410208,0.065614,-0.0622,-0.046793},
        {0.078413,0.231233,0.393865,0.318885,-0.069756,-0.42599,-0.439616,-0.23833,-0.07577},
        {0.039486,0.034327,-0.12741,-0.490047,-0.792569,-0.726876,-0.392411,-0.114968,-0.009538},
        {-0.020254,-0.10421,-0.278203,-0.434837,-0.39232,-0.167537,0.020893,0.064293,0.035078},
        {-0.019134,-0.050825,-0.073897,-0.03345,0.068666,0.138881,0.118933,0.057811,0.016539},
        {-0.002471,0.000988,0.020414,0.055516,0.078527,0.065056,0.031089,0.007002,-0.000496}
    },
    {
        {-0.000442,0.013535,0.042156,0.041006,-0.016289,-0.059269,-0.039775,-0.006967,0.002935},
        {-0.01529,-0.004044,0.090353,0.205079,0.145379,-0.042085,-0.111597,-0.054579,-0.006967},
        {-0.041345,-0.102069,-0.019676,0.320331,0.529862,0.273733,-0.057749,-0.111597,-0.039775},
        {-0.033072,-0.201134,-0.361867,-0.050836,0.603151,0.72707,0.273733,-0.042085,-0.059269},
        {0.026968,-0.117249,-0.519669,-0.68136,-0.069756,0.603151,0.529862,0.145379,-0.016289},
        {0.063308,0.069678,-0.220769,-0.713083,-0.68136,-0.050836,0.320331,0.205079,0.041006},
        {0.037404,0.119202,0.095611,-0.220769,-0.519669,-0.361867,-0.019676,0.090353,0.042156},
        {0.004415,0.051325,0.119202,0.069678,-0.117249,-0.201134,-0.102069,-0.004044,0.013535},
        {-0.003687,0.004415,0.037404,0.063308,0.026968,-0.033072,-0.041345,-0.01529,-0.000442}
    }
};

#endif

次に、Gabor_fiter_lh.cpp を示す。

// Gabor_fiter_lh.cpp
// 2016/07/23 by marsee
// 2016/07/25 : 右白線検出用のGabor Filterを追加して、右左の白線を指定するRorL 引数を追加
// 2016/07/27 : 右白線検出用配列と左白線検出用配列を統合
//

#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>

#include "Gabor_filter_lh.h"

int conv_rgb2y(int rgb);

int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins,
        hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL){
#pragma HLS INTERFACE axis port=ins
#pragma HLS INTERFACE axis port=outs
#pragma HLS INTERFACE s_axilite port=return

    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> gabor;

    hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
    hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;

    int gray_pix, val, i, j, x, y;

    do {    // user が 1になった時にフレームがスタートする
        ins >> pix;
    } while(pix.user == 0);

    for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
        for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
            if (!(x==0 && y==0))    // 最初の入力はすでに入力されている
                ins >> pix;    // AXI4-Stream からの入力

            mbuf.shift_left();    // mbuf の列を1ビット左シフト
            for(i=ARRAY_SIZE-2; i>=0; --i){
                mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
            }
            gray_pix = conv_rgb2y(pix.data);
            mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);

            // LineBuffer の更新
            linebuf.shift_down(x);
            linebuf.insert_bottom(gray_pix, x);

            // Gabor filter の演算
            for (j=0, val=0; j<ARRAY_SIZE-1; j++){
                for (i=0; i<ARRAY_SIZE-1; i++){
                    val += gabor_weight[(int)RorL][j][i] * mbuf(ARRAY_SIZE-1-j,i);
                }
            }
            val = val/256; // 256倍してあるので、1/256して戻す
            if (val<0)
                //val = -val; // 絶対値
                val = 0; // マイナスの値を0に丸める
            else if (val>255)
                val = 255;

            // Gabor filter・データの書き込み
            gabor.data = (val<<16)+(val<<8)+val;
            // 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
            if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
                gabor.data = 0;

            if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
                gabor.user = 1;
            else
                gabor.user = 0;

            if (x == (HORIZONTAL_PIXEL_WIDTH-1))    // 行の最後で TLAST をアサートする
                gabor.last = 1;
            else
                gabor.last = 0;

            outs << gabor;    // AXI4-Stream へ出力
         }
     }
     return(0);
}

// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y =  0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
//　2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y(int rgb){
    int r, g, b, y_f;
    int y;

    b = rgb & 0xff;
    g = (rgb>>8) & 0xff;
    r = (rgb>>16) & 0xff;

    y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
    y = y_f >> 8; // 256で割る

    return(y);
}

Gabor_filter_lh_tb.cpp を示す。

// Gabor_filter_lh_tb.cpp
// 2016/07/24 by marsee
// 2016/07/25 : 右白線検出用のGabor Filterを追加して、右左の白線を指定するRorL 引数を追加
//

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>

#include "Gabor_filter_lh.h"
#include "bmp_header.h"

int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL);

int conv_rgb2y_soft(int rgb);
int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL);

#define CLOCK_PERIOD 10
#define RIGHT_OR_LEFT   LEFT_WEIGHT
#define BMP_FILE_NAME   "road_4.bmp"

int main()
{
    using namespace std;

    hls::stream<ap_axis<32,1,1,1> > ins;
    hls::stream<ap_axis<32,1,1,1> > ins_soft;
    hls::stream<ap_axis<32,1,1,1> > outs;
    hls::stream<ap_axis<32,1,1,1> > outs_soft;
    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> vals;
    ap_axis<32,1,1,1> vals_soft;

    int m_seq = 1; // M系列の値
    int i;
    int xor_shift;

    BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
    BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
    FILE *fbmpr, *fbmpw, *fbmpwf;
    int *rd_bmp, *hw_gabor, *sw_gabor;
    int blue, green, red;
    ap_uint<1> r_l;

    if ((fbmpr = fopen(BMP_FILE_NAME, "rb")) == NULL){ // test.bmp をオープン
        fprintf(stderr, "Can't open test.bmp by binary read mode\n");
        exit(1);
    }
    // bmpヘッダの読み出し
    fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
    fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
    fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
    fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);

    // ピクセルを入れるメモリをアロケートする
    if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate rd_bmp memory\n");
        exit(1);
    }
    if ((hw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate hw_gabor memory\n");
        exit(1);
    }
    if ((sw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate hw_gabor memory\n");
        exit(1);
    }

    // rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = fgetc(fbmpr);
            green = fgetc(fbmpr);
            red = fgetc(fbmpr);
            rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
        }
    }
    fclose(fbmpr);

    // ins に入力データを用意する
    for(int i=0; i<5; i++){    //　dummy data
        pix.user = 0;
        pix.data = i;
        ins << pix;
    }

    for(int j=0; j < bmpihr.biHeight; j++){
        for(i=0; i < bmpihr.biWidth; i++){
            pix.data = (ap_int<32>)rd_bmp[(j*bmpihr.biWidth)+i];

            if (j==0 && i==0)    // 最初のデータの時に TUSER を 1 にする
                pix.user = 1;
            else
                pix.user = 0;

            if (i == bmpihr.biWidth-1) // 行の最後でTLASTをアサートする
                pix.last = 1;
            else
                pix.last = 0;

            ins << pix;
            ins_soft << pix;
        }
    }

    r_l = (ap_uint<1>)RIGHT_OR_LEFT;
    Gabor_filter_lh(ins, outs, r_l);
    Gabor_filter_lh_soft(ins_soft, outs_soft, r_l);

    // ハードウェアとソフトウェアのラプラシアン・フィルタの値のチェック
    cout << endl;
    cout << "outs" << endl;
    for(int j=0; j < bmpihr.biHeight; j++){
        for(i=0; i < bmpihr.biWidth; i++){
            outs >> vals;
            outs_soft >> vals_soft;
            ap_int<32> val = vals.data;
            ap_int<32> val_soft = vals_soft.data;

            hw_gabor[(j*bmpihr.biWidth)+i] = (int)val;
            sw_gabor[(j*bmpihr.biWidth)+i] = (int)val_soft;

            if ((double)pow((double)(val&0xff)-(val_soft&0xff),(double)2) > 4){ // 2乗誤差が4よりも大きい
                printf("ERROR HW and SW results mismatch i = %ld, j = %ld, HW = %08x, SW = %08x\n", i, j, (int)val, (int)val_soft);
                //return(1);
            }
            //if (vals.last)
                //cout << "AXI-Stream is end" << endl;
        }
    }
    cout << "Success HW and SW results match" << endl;
    cout << endl;

    // ハードウェアのラプラシアンフィルタの結果を temp_gabor.bmp へ出力する
    if ((fbmpw=fopen("temp_gabor.bmp", "wb")) == NULL){
        fprintf(stderr, "Can't open temp_gabor.bmp by binary write mode\n");
        exit(1);
    }
    // BMPファイルヘッダの書き込み
    fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
    fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
    fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
    fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
    fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
    fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);
    // RGB データの書き込み、逆順にする
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
            green = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
            red = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;

            fputc(blue, fbmpw);
            fputc(green, fbmpw);
            fputc(red, fbmpw);
        }
    }
    fclose(fbmpw);

    // ソフトウェアのラプラシアンフィルタの結果を temp_gabor_float.bmp へ出力する
    if ((fbmpwf=fopen("temp_gabor_float.bmp", "wb")) == NULL){
        fprintf(stderr, "Can't open temp_gabor_float.bmp by binary write mode\n");
        exit(1);
    }
    // BMPファイルヘッダの書き込み
    fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpwf);
    fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpwf);
    fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpwf);
    fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpwf);
    fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpwf);
    fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpwf);
    // RGB データの書き込み、逆順にする
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
            green = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
            red = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;

            fputc(blue, fbmpwf);
            fputc(green, fbmpwf);
            fputc(red, fbmpwf);
        }
    }
    fclose(fbmpwf);

    free(rd_bmp);
    free(hw_gabor);

    return 0;
}

int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL){
    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> gabor;

    hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
    hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;

    int gray_pix, val, i, j, x, y;
    float valf;

    do {    // user が 1になった時にフレームがスタートする
        ins >> pix;
    } while(pix.user == 0);

    for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
        for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
            if (!(x==0 && y==0))    // 最初の入力はすでに入力されている
                ins >> pix;    // AXI4-Stream からの入力

            mbuf.shift_left();    // mbuf の列を1ビット左シフト
            for(i=ARRAY_SIZE-2; i>=0; --i){
                mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
            }
            gray_pix = conv_rgb2y_soft(pix.data);
            mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);

            // LineBuffer の更新
            linebuf.shift_down(x);
            linebuf.insert_bottom(gray_pix, x);

            // Gabor filter の演算
            for (j=0, valf=0; j<ARRAY_SIZE-1; j++){
                for (i=0; i<ARRAY_SIZE-1; i++){
                    valf += gabor_fweight[(int)RorL][j][i] * (float)mbuf(ARRAY_SIZE-1-j,i);
                }
            }

            val = (int)valf;
            if (val<0)
                //val = -val; // 絶対値
                val = 0; // マイナスの値を0に丸める
            else if (val>255)
                val = 255;

            // Gabor filter・データの書き込み
            gabor.data = (val<<16)+(val<<8)+val;
            // 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
            if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
                gabor.data = 0;

            if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
                gabor.user = 1;
            else
                gabor.user = 0;

            if (x == (HORIZONTAL_PIXEL_WIDTH-1))    // 行の最後で TLAST をアサートする
                gabor.last = 1;
            else
                gabor.last = 0;

            outs << gabor;    // AXI4-Stream へ出力
         }
     }
     return(0);
}

// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y =  0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
//　2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y_soft(int rgb){
    int r, g, b, y_f;
    int y;

    b = rgb & 0xff;
    g = (rgb>>8) & 0xff;
    r = (rgb>>16) & 0xff;

    y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
    y = y_f >> 8; // 256で割る

    return(y);

ガボール・フィルタ (Gabor Filter) による白線検出8（hls::LineBuffer と hls::Window を使用5）

”ガボール・フィルタ (Gabor Filter) による白線検出7（hls::LineBuffer と hls::Window を使用4）”の続き。

前回は、左白線検出と右白線検出ができる両方の白線検出用Gabor Filterを作成した。1ビットのRorL という信号を追加して、右白線検出か、左白線検出かを選べるようにした。新しいGabor_filter_lh.h 、 Gabor_filter_lh.cpp 、 Gabor_filter_lh_tb.cpp を貼った。
今回は、そのC ソースコードを使って、C シミュレーション、C コードの合成を行った。

まずは、プロジェクトを示す。


C シミュレーションを行った。


エラーは無しだった。つまり、固定小数点演算と浮動小数点演算の差が 2 より大きい箇所は無いということが分かる。
Gabor Filter 処理後の写真を示す。左が固定小数点演算を使用した Gabor Filter で、右が浮動小数点演算を使用した Gabor Filter だ。


2 つの写真に肉眼で違いは見られない。右の白線が強調されているのがわかる。

次に、C コードの合成を行った。


Detail -> Loop 2 を見ると、Latency は307206 だった。これは大体 1 クロックで 1 ピクセルを処理できていることになる。
次に使用リソースだが、DSP48E が 92 % とほとんど消費してしまっている。これは、C ソースコードを見ると分かるが、安易に左白線用の演算と右白線用の演算を if 文で切り替えてしまったからかもしれない？
次は、左白線検出用重みと右白線検出用重みを一緒の配列にしてみよう。

ガボール・フィルタ (Gabor Filter) による白線検出7（hls::LineBuffer と hls::Window を使用4）

”ガボール・フィルタ (Gabor Filter) による白線検出6（hls::LineBuffer と hls::Window を使用3）”の続き。

前回は、hls::LineBuffer と hls::Window を使用して実装した左白線検出用のGabor FilterのC/RTL 協調シミュレーションを行った。今回は、左白線検出と右白線検出ができる両方の白線検出用Gabor Filterを作成する。
1ビットのRorL という信号を追加して、右白線検出か、左白線検出かを選べるようにした。新しいGabor_filter_lh.h 、 Gabor_filter_lh.cpp 、 Gabor_filter_lh_tb.cpp を貼っておく。

まずは、Gabor_filter_lh.h を示す。

// Gabor_filter_lh.h
// 2016/07/24
// 2016/07/25 : 右白線検出用のGabor Filterの重みを追加
//

#ifndef __Gabor_filter_lh_H__
#define __Gabor_filter_lh_H__

//#define HORIZONTAL_PIXEL_WIDTH    800
//#define VERTICAL_PIXEL_WIDTH    600

#define HORIZONTAL_PIXEL_WIDTH    640
#define VERTICAL_PIXEL_WIDTH    480

//#define HORIZONTAL_PIXEL_WIDTH    64
//#define VERTICAL_PIXEL_WIDTH    48

#define ALL_PIXEL_VALUE    (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)

#define ARRAY_SIZE    9
#define RIGHT_WEIGHT    1
#define LEFT_WEIGHT        0

const int gabor_weight_left[ARRAY_SIZE][ARRAY_SIZE] = { // 左側の白線検出用
        {0,-2,-9,-17,-19,-13,-4,0,1},
        {-5,-15,-28,-30,-9,16,22,14,5},
        {-8,-12,5,57,112,115,69,24,4},
        {5,35,108,186,190,105,17,-16,-12},
        {20,59,101,82,-18,-109,-113,-61,-19},
        {10,9,-33,-125,-203,-186,-100,-29,-2},
        {-5,-27,-71,-111,-100,-43,5,16,9},
        {-5,-13,-19,-9,18,36,30,15,4},
        {-1,0,5,14,20,17,8,2,0}
};
const float gabor_fweight_left[ARRAY_SIZE][ARRAY_SIZE] = { // 左側の白線検出用(float)
        {-0.000387,-0.009416,-0.034769,-0.067138,-0.075984,-0.04953,-0.015067,0.001684,0.003258},
        {-0.017724,-0.057864,-0.111235,-0.116438,-0.035878,0.061351,0.087704,0.054213,0.019079},
        {-0.031458,-0.048283,0.018458,0.22454,0.438323,0.448468,0.269024,0.09288,0.014896},
        {0.018804,0.137564,0.420198,0.727096,0.740714,0.410208,0.065614,-0.0622,-0.046793},
        {0.078413,0.231233,0.393865,0.318885,-0.069756,-0.42599,-0.439616,-0.23833,-0.07577},
        {0.039486,0.034327,-0.12741,-0.490047,-0.792569,-0.726876,-0.392411,-0.114968,-0.009538},
        {-0.020254,-0.10421,-0.278203,-0.434837,-0.39232,-0.167537,0.020893,0.064293,0.035078},
        {-0.019134,-0.050825,-0.073897,-0.03345,0.068666,0.138881,0.118933,0.057811,0.016539},
        {-0.002471,0.000988,0.020414,0.055516,0.078527,0.065056,0.031089,0.007002,-0.000496}
};

const int gabor_weight_right[ARRAY_SIZE][ARRAY_SIZE] = { // 右側の白線検出用
        {0,3,11,10,-4,-15,-10,-2,1},
        {-4,-1,23,53,37,-11,-29,-14,-2},
        {-11,-26,-5,82,136,70,-15,-29,-10},
        {-8,-51,-93,-13,154,186,70,-11,-15},
        {7,-30,-133,-174,-18,154,136,37,-4},
        {16,18,-57,-183,-174,-13,82,53,10},
        {10,31,24,-57,-133,-93,-5,23,11},
        {1,13,31,18,-30,-51,-26,-1,3},
        {-1,1,10,16,7,-8,-11,-4,0}
};
const float gabor_fweight_right[ARRAY_SIZE][ARRAY_SIZE] = { // 右側の白線検出用(float)
        {-0.000442,0.013535,0.042156,0.041006,-0.016289,-0.059269,-0.039775,-0.006967,0.002935},
        {-0.01529,-0.004044,0.090353,0.205079,0.145379,-0.042085,-0.111597,-0.054579,-0.006967},
        {-0.041345,-0.102069,-0.019676,0.320331,0.529862,0.273733,-0.057749,-0.111597,-0.039775},
        {-0.033072,-0.201134,-0.361867,-0.050836,0.603151,0.72707,0.273733,-0.042085,-0.059269},
        {0.026968,-0.117249,-0.519669,-0.68136,-0.069756,0.603151,0.529862,0.145379,-0.016289},
        {0.063308,0.069678,-0.220769,-0.713083,-0.68136,-0.050836,0.320331,0.205079,0.041006},
        {0.037404,0.119202,0.095611,-0.220769,-0.519669,-0.361867,-0.019676,0.090353,0.042156},
        {0.004415,0.051325,0.119202,0.069678,-0.117249,-0.201134,-0.102069,-0.004044,0.013535},
        {-0.003687,0.004415,0.037404,0.063308,0.026968,-0.033072,-0.041345,-0.01529,-0.000442}
};

#endif

次に、Gabor_fiter_lh.cpp を示す。

// Gabor_fiter_lh.cpp
// 2016/07/23 by marsee
// 2016/07/25 : 右白線検出用のGabor Filterを追加して、右左の白線を指定するRorL 引数を追加
//

#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>

#include "Gabor_filter_lh.h"

int conv_rgb2y(int rgb);

int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins,
        hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL){
#pragma HLS INTERFACE axis port=ins
#pragma HLS INTERFACE axis port=outs
#pragma HLS INTERFACE s_axilite port=return

    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> gabor;

    hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
    hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;

    int gray_pix, val, i, j, x, y;

    do {    // user が 1になった時にフレームがスタートする
        ins >> pix;
    } while(pix.user == 0);

    for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
        for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
            if (!(x==0 && y==0))    // 最初の入力はすでに入力されている
                ins >> pix;    // AXI4-Stream からの入力

            mbuf.shift_left();    // mbuf の列を1ビット左シフト
            for(i=ARRAY_SIZE-2; i>=0; --i){
                mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
            }
            gray_pix = conv_rgb2y(pix.data);
            mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);

            // LineBuffer の更新
            linebuf.shift_down(x);
            linebuf.insert_bottom(gray_pix, x);

            // Gabor filter の演算
            for (j=0, val=0; j<ARRAY_SIZE-1; j++){
                for (i=0; i<ARRAY_SIZE-1; i++){
                    if (RorL == LEFT_WEIGHT) // 左白線検出用重みと右白線検出用重みの選択
                        val += gabor_weight_left[j][i] * mbuf(ARRAY_SIZE-1-j,i);
                    else
                        val += gabor_weight_right[j][i] * mbuf(ARRAY_SIZE-1-j,i);
                }
            }
            val = val/256; // 256倍してあるので、1/256して戻す
            if (val<0)
                //val = -val; // 絶対値
                val = 0; // マイナスの値を0に丸める
            else if (val>255)
                val = 255;

            // Gabor filter・データの書き込み
            gabor.data = (val<<16)+(val<<8)+val;
            // 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
            if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
                gabor.data = 0;

            if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
                gabor.user = 1;
            else
                gabor.user = 0;

            if (x == (HORIZONTAL_PIXEL_WIDTH-1))    // 行の最後で TLAST をアサートする
                gabor.last = 1;
            else
                gabor.last = 0;

            outs << gabor;    // AXI4-Stream へ出力
         }
     }
     return(0);
}

// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y =  0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
//　2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y(int rgb){
    int r, g, b, y_f;
    int y;

    b = rgb & 0xff;
    g = (rgb>>8) & 0xff;
    r = (rgb>>16) & 0xff;

    y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
    y = y_f >> 8; // 256で割る

    return(y);
}

Gabor_filter_lh_tb.cpp を示す。

// Gabor_filter_lh_tb.cpp
// 2016/07/24 by marsee
// 2016/07/25 : 右白線検出用のGabor Filterを追加して、右左の白線を指定するRorL 引数を追加
//

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>

#include "Gabor_filter_lh.h"
#include "bmp_header.h"

int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL);

int conv_rgb2y_soft(int rgb);
int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL);

#define CLOCK_PERIOD 10
#define RIGHT_OR_LEFT   RIGHT_WEIGHT

int main()
{
    using namespace std;

    hls::stream<ap_axis<32,1,1,1> > ins;
    hls::stream<ap_axis<32,1,1,1> > ins_soft;
    hls::stream<ap_axis<32,1,1,1> > outs;
    hls::stream<ap_axis<32,1,1,1> > outs_soft;
    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> vals;
    ap_axis<32,1,1,1> vals_soft;

    int m_seq = 1; // M系列の値
    int i;
    int xor_shift;

    BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
    BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
    FILE *fbmpr, *fbmpw, *fbmpwf;
    int *rd_bmp, *hw_gabor, *sw_gabor;
    int blue, green, red;
    ap_uint<1> r_l;

    if ((fbmpr = fopen("road_1.bmp", "rb")) == NULL){ // test.bmp をオープン
        fprintf(stderr, "Can't open test.bmp by binary read mode\n");
        exit(1);
    }
    // bmpヘッダの読み出し
    fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
    fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
    fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
    fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);

    // ピクセルを入れるメモリをアロケートする
    if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate rd_bmp memory\n");
        exit(1);
    }
    if ((hw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate hw_gabor memory\n");
        exit(1);
    }
    if ((sw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate hw_gabor memory\n");
        exit(1);
    }

    // rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = fgetc(fbmpr);
            green = fgetc(fbmpr);
            red = fgetc(fbmpr);
            rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
        }
    }
    fclose(fbmpr);

    // ins に入力データを用意する
    for(int i=0; i<5; i++){    //　dummy data
        pix.user = 0;
        pix.data = i;
        ins << pix;
    }

    for(int j=0; j < bmpihr.biHeight; j++){
        for(i=0; i < bmpihr.biWidth; i++){
            pix.data = (ap_int<32>)rd_bmp[(j*bmpihr.biWidth)+i];

            if (j==0 && i==0)    // 最初のデータの時に TUSER を 1 にする
                pix.user = 1;
            else
                pix.user = 0;

            if (i == bmpihr.biWidth-1) // 行の最後でTLASTをアサートする
                pix.last = 1;
            else
                pix.last = 0;

            ins << pix;
            ins_soft << pix;
        }
    }

    r_l = (ap_uint<1>)RIGHT_OR_LEFT;
    Gabor_filter_lh(ins, outs, r_l);
    Gabor_filter_lh_soft(ins_soft, outs_soft, r_l);

    // ハードウェアとソフトウェアのラプラシアン・フィルタの値のチェック
    cout << endl;
    cout << "outs" << endl;
    for(int j=0; j < bmpihr.biHeight; j++){
        for(i=0; i < bmpihr.biWidth; i++){
            outs >> vals;
            outs_soft >> vals_soft;
            ap_int<32> val = vals.data;
            ap_int<32> val_soft = vals_soft.data;

            hw_gabor[(j*bmpihr.biWidth)+i] = (int)val;
            sw_gabor[(j*bmpihr.biWidth)+i] = (int)val_soft;

            if ((double)pow((double)(val&0xff)-(val_soft&0xff),(double)2) > 4){ // 2乗誤差が4よりも大きい
                printf("ERROR HW and SW results mismatch i = %ld, j = %ld, HW = %08x, SW = %08x\n", i, j, (int)val, (int)val_soft);
                //return(1);
            }
            //if (vals.last)
                //cout << "AXI-Stream is end" << endl;
        }
    }
    cout << "Success HW and SW results match" << endl;
    cout << endl;

    // ハードウェアのラプラシアンフィルタの結果を temp_gabor.bmp へ出力する
    if ((fbmpw=fopen("temp_gabor.bmp", "wb")) == NULL){
        fprintf(stderr, "Can't open temp_gabor.bmp by binary write mode\n");
        exit(1);
    }
    // BMPファイルヘッダの書き込み
    fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
    fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
    fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
    fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
    fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
    fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);
    // RGB データの書き込み、逆順にする
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
            green = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
            red = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;

            fputc(blue, fbmpw);
            fputc(green, fbmpw);
            fputc(red, fbmpw);
        }
    }
    fclose(fbmpw);

    // ソフトウェアのラプラシアンフィルタの結果を temp_gabor_float.bmp へ出力する
    if ((fbmpwf=fopen("temp_gabor_float.bmp", "wb")) == NULL){
        fprintf(stderr, "Can't open temp_gabor_float.bmp by binary write mode\n");
        exit(1);
    }
    // BMPファイルヘッダの書き込み
    fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpwf);
    fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpwf);
    fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpwf);
    fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpwf);
    fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpwf);
    fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpwf);
    // RGB データの書き込み、逆順にする
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
            green = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
            red = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;

            fputc(blue, fbmpwf);
            fputc(green, fbmpwf);
            fputc(red, fbmpwf);
        }
    }
    fclose(fbmpwf);

    free(rd_bmp);
    free(hw_gabor);

    return 0;
}

int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<1> & RorL){
    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> gabor;

    hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
    hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;

    int gray_pix, val, i, j, x, y;
    float valf;

    do {    // user が 1になった時にフレームがスタートする
        ins >> pix;
    } while(pix.user == 0);

    for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
        for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
            if (!(x==0 && y==0))    // 最初の入力はすでに入力されている
                ins >> pix;    // AXI4-Stream からの入力

            mbuf.shift_left();    // mbuf の列を1ビット左シフト
            for(i=ARRAY_SIZE-2; i>=0; --i){
                mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
            }
            gray_pix = conv_rgb2y_soft(pix.data);
            mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);

            // LineBuffer の更新
            linebuf.shift_down(x);
            linebuf.insert_bottom(gray_pix, x);

            // Gabor filter の演算
            for (j=0, valf=0; j<ARRAY_SIZE-1; j++){
                for (i=0; i<ARRAY_SIZE-1; i++){
                    if (RorL == LEFT_WEIGHT) // 左白線検出用重みと右白線検出用重みの選択
                        valf += gabor_fweight_left[j][i] * (float)mbuf(ARRAY_SIZE-1-j,i);
                    else
                        valf += gabor_fweight_right[j][i] * (float)mbuf(ARRAY_SIZE-1-j,i);
                }
            }

            val = (int)valf;
            if (val<0)
                //val = -val; // 絶対値
                val = 0; // マイナスの値を0に丸める
            else if (val>255)
                val = 255;

            // Gabor filter・データの書き込み
            gabor.data = (val<<16)+(val<<8)+val;
            // 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
            if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
                gabor.data = 0;

            if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
                gabor.user = 1;
            else
                gabor.user = 0;

            if (x == (HORIZONTAL_PIXEL_WIDTH-1))    // 行の最後で TLAST をアサートする
                gabor.last = 1;
            else
                gabor.last = 0;

            outs << gabor;    // AXI4-Stream へ出力
         }
     }
     return(0);
}

// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y =  0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
//　2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y_soft(int rgb){
    int r, g, b, y_f;
    int y;

    b = rgb & 0xff;
    g = (rgb>>8) & 0xff;
    r = (rgb>>16) & 0xff;

    y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
    y = y_f >> 8; // 256で割る

    return(y);
}

ガボール・フィルタ (Gabor Filter) による白線検出6（hls::LineBuffer と hls::Window を使用3）

”ガボール・フィルタ (Gabor Filter) による白線検出5（hls::LineBuffer と hls::Window を使用2）”の続き。

前回は、hls::LineBuffer と hls::Window を使用した Gabor Filter の C シミュレーションとC コードの合成を行ったので、今回は、C/RTL 協調シミュレーションを行った。

C/RTL 協調シミュレーションの結果を示す。


307239 クロックかかっていた。640 x 480 ピクセルの画像は、307200 ピクセルなので、39 クロックのみ余計にかかっているだけで、ほぼ 1 クロックに 1 ピクセルの処理ができている。

C/RTL 協調シミュレーション波形を示す。


入力ストリームの ins_TVALID, ins_TREADY と出力ストリームの outs_TVALID, outs_TREADY がずーと 1 のままなのがわかる。
つまり、連続的に休みなく Gabor Filter の処理出力ができているということを示している。

ガボール・フィルタ (Gabor Filter) による白線検出5（hls::LineBuffer と hls::Window を使用2）

”ガボール・フィルタ (Gabor Filter) による白線検出4（hls::LineBuffer と hls::Window を使用1）”の続き。

前回は、hls::LineBuffer と hls::Window を使用してGabor Filter を実装し、そのC ソースコードを貼り付けた。今回はそのGabor Filter を評価した。

ZYBO 用のVivado HLS 2016.2 プロジェクトを作成した。


これで、C シミュレーションを行った。最初にGabor Filter のカーネルを乗算した後で、1/256 倍して、その値がマイナスだったときは絶対値を取るようにした。そのときの出力画像を示す。


ソフトウェアで実装したときのGabor Filter の画像と違っている。


値がマイナスだったときは、0 に丸めるようにコードを変更して、もう一度、C シミュレーションを行った。結果を見るとこちらが正解のようだ。
出力されたハードウェアのGabor Filterをかけた画像のtemp_gabor.bmp とテストベンチで作成された浮動小数点演算を用いたtemp_gabor_float.bmp を示す。左がtemp_gabor.bmp だ。


2つの画像は見た目では違いが判らない。

C シミュレーションの様子を示す。


ハードウェア化する整数演算の結果と浮動小数点演算された結果の二乗誤差が 4 以上の時に表示される。ほとんどの二乗誤差は 4 以下で、表示されているのは二乗誤差が 9 だった。つまり、3 離れている。

次にC コードの合成を行った。



Loop2 のLatency が 307209 クロックだった。今回の写真の解像度が 640 x 480 = 307200 なので、1クロックごとに1 ピクセルの画像を処理できている。

リソース的にもZYBO に実装可能のようであるが、まだ、右車線用が残っている。コードを追加して、今回の左車線用と一緒にする予定だ。

ガボール・フィルタ (Gabor Filter) による白線検出4（hls::LineBuffer と hls::Window を使用1）

”ガボール・フィルタ (Gabor Filter) による白線検出3（HLSビデオライブラリ）”の続き。

前回は、Vivado HLS のOpenCV機能でHLS_16UC3 と HLS_16UC1 が見つからなかった。今回は、hls::LineBuffer と hls::Window を使用してGabor Filter を実装してみた。今回は、C ソースコードを貼り付けた。

”ガボール・フィルタ (Gabor Filter) による白線検出3（HLSビデオライブラリ）”の表で一番下の256倍された整数のGabor Filter をVivado HLS でハードウェア化するのだが、それと浮動小数点の結果をテストベンチで比較して、二乗誤差が 4 よりも大きいときは表示させている。

それでは、現在の C ソースコードを示す。なお、現在は左白線検出用のみのパラメータだが、右白線検出用パラメータも一緒にしようと思っている。
それでは、Gabor_filter_lh.h から示す。

// Gabor_filter_lh.h
// 2016/07/24

#ifndef __Gabor_filter_lh_H__
#define __Gabor_filter_lh_H__

//#define HORIZONTAL_PIXEL_WIDTH    800
//#define VERTICAL_PIXEL_WIDTH    600

#define HORIZONTAL_PIXEL_WIDTH    640
#define VERTICAL_PIXEL_WIDTH    480

//#define HORIZONTAL_PIXEL_WIDTH    64
//#define VERTICAL_PIXEL_WIDTH    48

#define ALL_PIXEL_VALUE    (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)

#define ARRAY_SIZE    9

const int gabor_weight[ARRAY_SIZE][ARRAY_SIZE] = { // 左側の白線検出用
        {0,-2,-9,-17,-19,-13,-4,0,1},
        {-5,-15,-28,-30,-9,16,22,14,5},
        {-8,-12,5,57,112,115,69,24,4},
        {5,35,108,186,190,105,17,-16,-12},
        {20,59,101,82,-18,-109,-113,-61,-19},
        {10,9,-33,-125,-203,-186,-100,-29,-2},
        {-5,-27,-71,-111,-100,-43,5,16,9},
        {-5,-13,-19,-9,18,36,30,15,4},
        {-1,0,5,14,20,17,8,2,0}
};
const float gabor_fweight[ARRAY_SIZE][ARRAY_SIZE] = { // 左側の白線検出用(float)
        {-0.000387,-0.009416,-0.034769,-0.067138,-0.075984,-0.04953,-0.015067,0.001684,0.003258},
        {-0.017724,-0.057864,-0.111235,-0.116438,-0.035878,0.061351,0.087704,0.054213,0.019079},
        {-0.031458,-0.048283,0.018458,0.22454,0.438323,0.448468,0.269024,0.09288,0.014896},
        {0.018804,0.137564,0.420198,0.727096,0.740714,0.410208,0.065614,-0.0622,-0.046793},
        {0.078413,0.231233,0.393865,0.318885,-0.069756,-0.42599,-0.439616,-0.23833,-0.07577},
        {0.039486,0.034327,-0.12741,-0.490047,-0.792569,-0.726876,-0.392411,-0.114968,-0.009538},
        {-0.020254,-0.10421,-0.278203,-0.434837,-0.39232,-0.167537,0.020893,0.064293,0.035078},
        {-0.019134,-0.050825,-0.073897,-0.03345,0.068666,0.138881,0.118933,0.057811,0.016539},
        {-0.002471,0.000988,0.020414,0.055516,0.078527,0.065056,0.031089,0.007002,-0.000496}
};

#endif

次に、Gabor_fiter_lh.cpp を示す。

// Gabor_fiter_lh.cpp
// 2016/07/23 by marsee
//

#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>

#include "Gabor_filter_lh.h"

int conv_rgb2y(int rgb);

int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins,
        hls::stream<ap_axis<32,1,1,1> >& outs){
#pragma HLS INTERFACE axis port=ins
#pragma HLS INTERFACE axis port=outs
#pragma HLS INTERFACE s_axilite port=return

    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> gabor;

    hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
    hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;

    int gray_pix, val, i, j, x, y;

    do {    // user が 1になった時にフレームがスタートする
        ins >> pix;
    } while(pix.user == 0);

    for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
        for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
            if (!(x==0 && y==0))    // 最初の入力はすでに入力されている
                ins >> pix;    // AXI4-Stream からの入力

            mbuf.shift_left();    // mbuf の列を1ビット左シフト
            for(i=ARRAY_SIZE-2; i>=0; --i){
                mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
            }
            gray_pix = conv_rgb2y(pix.data);
            mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);

            // LineBuffer の更新
            linebuf.shift_down(x);
            linebuf.insert_bottom(gray_pix, x);

            // Gabor filter の演算
            for (j=0, val=0; j<ARRAY_SIZE-1; j++){
                for (i=0; i<ARRAY_SIZE-1; i++){
                    val += gabor_weight[j][i] * mbuf(ARRAY_SIZE-1-j,i);
                }
            }
            val = val/256; // 256倍してあるので、1/256して戻す
            if (val<0)
                //val = -val; // 絶対値
                val = 0; // マイナスの値を0に丸める
            else if (val>255)
                val = 255;

            // Gabor filter・データの書き込み
            gabor.data = (val<<16)+(val<<8)+val;
            // 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
            if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
                gabor.data = 0;

            if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
                gabor.user = 1;
            else
                gabor.user = 0;

            if (x == (HORIZONTAL_PIXEL_WIDTH-1))    // 行の最後で TLAST をアサートする
                gabor.last = 1;
            else
                gabor.last = 0;

            outs << gabor;    // AXI4-Stream へ出力
         }
     }
     return(0);
}

// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y =  0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
//　2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y(int rgb){
    int r, g, b, y_f;
    int y;

    b = rgb & 0xff;
    g = (rgb>>8) & 0xff;
    r = (rgb>>16) & 0xff;

    y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
    y = y_f >> 8; // 256で割る

    return(y);
}

次にテストベンチのGabor_filter_lh_tb.cpp を示す。

// Gabor_filter_lh_tb.cpp
// 2016/07/24 by marsee
//

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>

#include "Gabor_filter_lh.h"
#include "bmp_header.h"

int Gabor_filter_lh(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs);

int conv_rgb2y_soft(int rgb);
int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs);

#define CLOCK_PERIOD 10

int main()
{
    using namespace std;

    hls::stream<ap_axis<32,1,1,1> > ins;
    hls::stream<ap_axis<32,1,1,1> > ins_soft;
    hls::stream<ap_axis<32,1,1,1> > outs;
    hls::stream<ap_axis<32,1,1,1> > outs_soft;
    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> vals;
    ap_axis<32,1,1,1> vals_soft;

    int m_seq = 1; // M系列の値
    int i;
    int xor_shift;

    BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
    BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
    FILE *fbmpr, *fbmpw, *fbmpwf;
    int *rd_bmp, *hw_gabor, *sw_gabor;
    int blue, green, red;

    if ((fbmpr = fopen("road_1.bmp", "rb")) == NULL){ // test.bmp をオープン
        fprintf(stderr, "Can't open test.bmp by binary read mode\n");
        exit(1);
    }
    // bmpヘッダの読み出し
    fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
    fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
    fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
    fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);

    // ピクセルを入れるメモリをアロケートする
    if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate rd_bmp memory\n");
        exit(1);
    }
    if ((hw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate hw_gabor memory\n");
        exit(1);
    }
    if ((sw_gabor =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate hw_gabor memory\n");
        exit(1);
    }

    // rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = fgetc(fbmpr);
            green = fgetc(fbmpr);
            red = fgetc(fbmpr);
            rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
        }
    }
    fclose(fbmpr);

    // ins に入力データを用意する
    for(int i=0; i<5; i++){    //　dummy data
        pix.user = 0;
        pix.data = i;
        ins << pix;
    }

    for(int j=0; j < bmpihr.biHeight; j++){
        for(i=0; i < bmpihr.biWidth; i++){
            pix.data = (ap_int<32>)rd_bmp[(j*bmpihr.biWidth)+i];

            if (j==0 && i==0)    // 最初のデータの時に TUSER を 1 にする
                pix.user = 1;
            else
                pix.user = 0;

            if (i == bmpihr.biWidth-1) // 行の最後でTLASTをアサートする
                pix.last = 1;
            else
                pix.last = 0;

            ins << pix;
            ins_soft << pix;
        }
    }

    Gabor_filter_lh(ins, outs);
    Gabor_filter_lh_soft(ins_soft, outs_soft);

    // ハードウェアとソフトウェアのラプラシアン・フィルタの値のチェック
    cout << endl;
    cout << "outs" << endl;
    for(int j=0; j < bmpihr.biHeight; j++){
        for(i=0; i < bmpihr.biWidth; i++){
            outs >> vals;
            outs_soft >> vals_soft;
            ap_int<32> val = vals.data;
            ap_int<32> val_soft = vals_soft.data;

            hw_gabor[(j*bmpihr.biWidth)+i] = (int)val;
            sw_gabor[(j*bmpihr.biWidth)+i] = (int)val_soft;

            if ((double)pow((double)(val&0xff)-(val_soft&0xff),(double)2) > 4){ // 2乗誤差が4よりも大きい
                printf("ERROR HW and SW results mismatch i = %ld, j = %ld, HW = %08x, SW = %08x\n", i, j, (int)val, (int)val_soft);
                //return(1);
            }
            //if (vals.last)
                //cout << "AXI-Stream is end" << endl;
        }
    }
    cout << "Success HW and SW results match" << endl;
    cout << endl;

    // ハードウェアのラプラシアンフィルタの結果を temp_gabor.bmp へ出力する
    if ((fbmpw=fopen("temp_gabor.bmp", "wb")) == NULL){
        fprintf(stderr, "Can't open temp_gabor.bmp by binary write mode\n");
        exit(1);
    }
    // BMPファイルヘッダの書き込み
    fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
    fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
    fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
    fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
    fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
    fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);
    // RGB データの書き込み、逆順にする
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
            green = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
            red = (hw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;

            fputc(blue, fbmpw);
            fputc(green, fbmpw);
            fputc(red, fbmpw);
        }
    }
    fclose(fbmpw);

    // ソフトウェアのラプラシアンフィルタの結果を temp_gabor_float.bmp へ出力する
    if ((fbmpwf=fopen("temp_gabor_float.bmp", "wb")) == NULL){
        fprintf(stderr, "Can't open temp_gabor_float.bmp by binary write mode\n");
        exit(1);
    }
    // BMPファイルヘッダの書き込み
    fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpwf);
    fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpwf);
    fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpwf);
    fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpwf);
    fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpwf);
    fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpwf);
    // RGB データの書き込み、逆順にする
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
            green = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
            red = (sw_gabor[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;

            fputc(blue, fbmpwf);
            fputc(green, fbmpwf);
            fputc(red, fbmpwf);
        }
    }
    fclose(fbmpwf);

    free(rd_bmp);
    free(hw_gabor);

    return 0;
}

int Gabor_filter_lh_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs){
    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> gabor;

    hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
    hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;

    int gray_pix, val, i, j, x, y;
    float valf;

    do {    // user が 1になった時にフレームがスタートする
        ins >> pix;
    } while(pix.user == 0);

    for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
        for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
            if (!(x==0 && y==0))    // 最初の入力はすでに入力されている
                ins >> pix;    // AXI4-Stream からの入力

            mbuf.shift_left();    // mbuf の列を1ビット左シフト
            for(i=ARRAY_SIZE-2; i>=0; --i){
                mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
            }
            gray_pix = conv_rgb2y_soft(pix.data);
            mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);

            // LineBuffer の更新
            linebuf.shift_down(x);
            linebuf.insert_bottom(gray_pix, x);

            // Gabor filter の演算
            for (j=0, valf=0; j<ARRAY_SIZE-1; j++){
                for (i=0; i<ARRAY_SIZE-1; i++){
                    valf += gabor_fweight[j][i] * (float)mbuf(ARRAY_SIZE-1-j,i);
                }
            }

            val = (int)valf;
            if (val<0)
                //val = -val; // 絶対値
                val = 0; // マイナスの値を0に丸める
            else if (val>255)
                val = 255;

            // Gabor filter・データの書き込み
            gabor.data = (val<<16)+(val<<8)+val;
            // 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
            if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
                gabor.data = 0;

            if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
                gabor.user = 1;
            else
                gabor.user = 0;

            if (x == (HORIZONTAL_PIXEL_WIDTH-1))    // 行の最後で TLAST をアサートする
                gabor.last = 1;
            else
                gabor.last = 0;

            outs << gabor;    // AXI4-Stream へ出力
         }
     }
     return(0);
}

// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y =  0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
//　2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y_soft(int rgb){
    int r, g, b, y_f;
    int y;

    b = rgb & 0xff;
    g = (rgb>>8) & 0xff;
    r = (rgb>>16) & 0xff;

    y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
    y = y_f >> 8; // 256で割る

    return(y);
}

Zybotのモーター・モニタのパラメータ

Zybot の後ろにキャスターを付けたのだが、回らない固定のキャスターだったので、左右モーターのPWM 値を変えてもなかなか即l度が変化しなかった。自在に回るキャスターか、ボール・キャスターを付ける必要があるようだ。失敗した。

Zybot で使用しているモーターは、Digilent 社から購入した「DC Motor/Gearbox (1:53 Gear Ratio): Custom 6V Motor Designed for Digilent Robot Kits」だが、モーター・モニタのパラメータを取得した。

PWM 値は左、右モーターともに 25 % で、車輪は浮かせて回している。車輪に目印をつけて、20回転するに何秒かかったのかを測定した。目分量でストップウオッチ計測なので、誤差はあると思う。モーター・モニタのデータは、測定の前後で計測した。
なお、車輪直径は 7 cm だった。

左モーター
（測定データ）10.8 sec / 20 回転、308 Hz , 298 Hz （平均 303 Hz）

20 回転 / 10.8 sec ≒ 1.85 回転/sec
303 Hz / 1.85 回転 / sec ≒ 164 パルス / 回転

右モーター
（測定データ）10.0 sec / 20 回転、324 Hz , 315 Hz （平均 320 Hz）　右モーターの方が左モーターより回転数が多い。

20 回転 / 10.0 sec ≒ 2.00 回転/sec
320 Hz / 2.00 回転 / sec = 160 パルス / 回転

右モーターと左モーターの平均を取って、162 パルス / 回転　とする。
車輪直径は 7 cm なので、車輪の円周は 22 cm
162 Hz の時に、22 cm /sec = 0.22 m / sec = 0.792 km / h となる。
303 Hz では、303 / 162 * 22 cm ≒ 41.1 cm / sec
320 Hz では、320 / 162 * 22 cm ≒ 43.5 cm / sec
これで車速を測定することができるようになった。

ガボール・フィルタ (Gabor Filter) による白線検出3（HLSビデオライブラリ）

”ガボール・フィルタ (Gabor Filter) による白線検出2”の続き。

前回は、カーネルサイズを 9 にしてガボール・フィルタをかけて、カーネルのパラメータをを取得した。今回は、取得したカーネルのパラメータを使って、Vivado HLS のHLS ビデオライブラリを使用して、ハードウェア化を試みた。

まずは、カーネルのパラメータを加工して、int で扱える形にする。
カーネルのパラメータを256倍して、四捨五入を行った。
左白線検出用パラメータを示す。


この値は256倍してあるので、カーネルを画像データにかけた後で、1/256倍する必要がある。

次に右白線検出用パラメータを示す。


このパラメータをカーネルとして、hls::Filter2D() で2次元フィルタをかけるわけなのだが、今回は、1/256 することもあり、少なくとも16ビットSingedで演算したい。
画像の型情報のHLS_8UC3 と HLS_8UC1 があるのはわかっているのだが、HLS_16UC3 と HLS_16UC1 があるのかどうかの情報が全くない。。。

試しに、HLS_16UC3 と HLS_16UC1 を使ってやってみたのだが、エラーになってしまう。


もうすこしやってみてダメだったら、自分で hls::LineBuffer と hls::Window を使用して、ガボール・フィルタを実装しようと思う。

Zybot のモーター・コントロール・アプリケーションの開発目標の覚書

Zybot のモーター・コントロール・アプリケーションの開発目標の覚書

１．　静止状態からPWM 値が 80 % にすると、たぶん電圧が降下してZYBO がリセットされてしまうようなので、段々とPWM 値を上げていくように変更する。

２．　モーター・モニタの値を監視して、その値になるようにPWM 値を調整するモードを作る。例えば、モーター・モニタの値が左右 190 Hz になるようにPWM 値を調整するとか。

３．　左右のモーターのPWM 値の違いで、どの程度曲がるか？のパラメータを確認したい。

４．　アプリケーション・ソフトをC++ のクラスで書き直したい。

ガボール・フィルタ (Gabor Filter) による白線検出2

”ガボール・フィルタ (Gabor Filter) による白線検出1”の続き。

前回は、カーネルサイズ 21 のガボール・フィルタで道路の白線を検出してみたが、今回はカーネルサイズ21だとハードウェアが大きいので、カーネルサイズを 9 にしてみた。更に、カーネルのパラメータを取得した。

カーネルサイズを 9 にするには、ソフトウェアの kernel_size を 9 にすれば良さそうだ。そして、カーネルのパラメータをコードを追加して取得する。


パラメータは sigma = 4, Lambda = 80, Psi = 94 で左の白線用のTheta = 68 で、右の白線用のTheta = 135 を使用した。




C シミュレーションを行い、各パラメータを設定して、カーネルのパラメータを取得できた。

ガボール・フィルタ (Gabor Filter) による白線検出1

以前は、OpenCV のHoughLine 変換を使って、道路の白線の検出をしていたが、今回はガボール・フィルタ (Gabor Filter) を使用して白線を検出してみようと思う。

ガボール・フィルタはこのサイトによるとガウス関数と正弦波の積だそうだ。
画像処理　(2) ガボール・フィルタに詳しく書いてある。
ガボール・フィルタのサンプルコードはこのサイトに載っていたので、そっくりそのままVivado HLS のテストベンチとして動作させたら動いた。
画像は自分で撮影した道路の写真にガボール・フィルタをかけてみた。
まずはVivado HLS のプロジェクトを示す。現在、C シミュレーションを行っている最中だ。


ガボール・フィルタを使うと左側の白線と右側の白線の区別ができる。
Theta を47度くらいにして左側の白線だけ浮き上がらせている。


次は、Theta を143度くらいにして、右側の白線だけを浮き上がらせている。


車のカメラから見たときに白線は決まった角度にあると思うので、ガボール・フィルタで白線の角度でフィルタを行うと、目的の白線だけ抽出できるという目論見である。教えてくれたT先生ありがとうございます。
大体白線がありそうな場所、角度のガボール・フィルタでサーチして白線の位置を検出することになると思う。画像の縦2点で検出して角度を検出できそうだ。
道路がどっちに曲がっているのか？を検出できそうなところが良いと思う。

Vivado HLS で DMA Read IP を作る（絶対アドレス指定版）

DMA Write IP に続いてDMA Read IP を作ってみた。絶対アドレスを設定する版を作った。
DMA Read IP は、DMA Write IP のactive_frame を受けて、その1つ前を表示するモードと3つのフレームバッファの内容をDMA Read するフリーラン・モードを設けてある。
mode = 0 の時は、DMA Write IP のactive_frame を受けて、その1つ前を表示するモードで、mode = 1 の時がフリーラン・モードとなる。

それでは、C ソースコードの DMA_Read_addr.cpp を示す。なお、AXI4-Stream のフォーマットはAXI VDMA に合わせてある。

// DMA_Read_addr.cpp
// 2016/07/13 by marsee
//
// frame_buffer0, frame_buffer1, frame_buffer2 には3つのフレームバッファのアドレスを入れる
// mode = 0 : DMA Write IP の active_frame を見て、その1つ前のフレームをDMA Readするモード(DMA_WRITE_MODE)
// mode = 1 : フリーラン モード(FREE_RUN_MODE)

#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>

#include "DMA_Read.h"

int DMA_Read_addr(volatile int *in, hls::stream<ap_axis<32,1,1,1> >& outs,
        unsigned int frame_buffer0, unsigned int frame_buffer1,
        unsigned int frame_buffer2, ap_uint<2> & active_frame,
        ap_uint<1> mode){
#pragma HLS INTERFACE s_axilite port=mode
#pragma HLS INTERFACE ap_none port=active_frame
#pragma HLS INTERFACE s_axilite port=frame_buffer0
#pragma HLS INTERFACE s_axilite port=frame_buffer1
#pragma HLS INTERFACE s_axilite port=frame_buffer2
#pragma HLS INTERFACE m_axi depth=5000000 port=in offset=off
#pragma HLS INTERFACE axis port=outs
#pragma HLS INTERFACE s_axilite port=return

    ap_axis<32,1,1,1> pix;
    unsigned int dma_index, n;

    for (int i=0; i<MAX_FRAME_NUMBER; i++){
        if (mode == DMA_WRITE_MODE)
            n = (unsigned int)active_frame;
        else
            n = (unsigned int)i;

        switch (n){ // 1つ前のフレームバッファを読みだす
            case 0 :
                dma_index = frame_buffer2/sizeof(int);
                break;
            case 1 :
                dma_index = frame_buffer0/sizeof(int);
                break;
            case 2 :
                dma_index = frame_buffer1/sizeof(int);
                break;
            default :
                dma_index = frame_buffer0/sizeof(int);
                break;
        } 

        for (int y=0; y<VERTICAL_PIXEL_WIDTH; y++){
            for (int x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
                pix.data = in[dma_index+(y*HORIZONTAL_PIXEL_WIDTH)+x];

                if (y==0 && x==0)
                    pix.user = 1;
                else
                    pix.user = 0;

                if (x == (HORIZONTAL_PIXEL_WIDTH-1))
                    pix.last = 1;
                else
                    pix.last = 0;

                outs << pix;
            }
        }
    }

    return 0;
}

次に、DMA_Read.h を示す。
<

span class="src_singlelinecomment">// DMA_Read.h
// 2016/07/14 by marsee
//

//#define HORIZONTAL_PIXEL_WIDTH    800
//#define VERTICAL_PIXEL_WIDTH    600

#define HORIZONTAL_PIXEL_WIDTH    64
#define VERTICAL_PIXEL_WIDTH    48

#define ALL_PIXEL_VALUE    (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)

#define MAX_FRAME_NUMBER    3

#define DMA_WRITE_MODE    0
#define FREE_RUN_MODE    1

最後にテストベンチのDMA_Read_addr_tb.cpp を示す。
’A’という文字のBMPファイルをRead して3つのフレームバッファに同じ’A’画像を書き込んで、mode = 0 の時にDMA_Read_addr() を実行し、mode = 1 でもう一度、DMA_Read_addr() を実行する。mode = 1の時のAXI4-Stream の出力をdma_result0.bmp ～ dma_result2.bmp の3つの画像にする。

// DMA_Read_addr_tb.cpp
// 2016/07/15 by marsee
//

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <ap_axi_sdata.h>

#include "DMA_Read.h"
#include "bmp_header.h"

int DMA_Read_addr(volatile int *in, hls::stream<ap_axis<32,1,1,1> >& outs,
        unsigned int frame_buffer0, unsigned int frame_buffer1,
        unsigned int frame_buffer2,  ap_uint<2> & active_frame,
        ap_uint<1> mode);

int main()
{
    using namespace std;

    hls::stream<ap_axis<32,1,1,1> > outs_dummy;
    hls::stream<ap_axis<32,1,1,1> > outs;
    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> vals;

    BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
    BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
    FILE *fbmpr, *fbmpw;
    int *rd_bmp, *hw_lapd;
    int blue, green, red;
    ap_uint<2> active_frame = 0;
    int *frame_buffer;

    if ((fbmpr = fopen("test.bmp", "rb")) == NULL){ // test.bmp をオープン
        fprintf(stderr, "Can't open test.bmp by binary read mode\n");
        exit(1);
    }
    // bmpヘッダの読み出し
    fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
    fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
    fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
    fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);

    // ピクセルを入れるメモリをアロケートする
    if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate rd_bmp memory\n");
        exit(1);
    }

    int *buf;
    if ((buf =(int *)malloc(3 * sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate buf memory\n");
        exit(1);
    }

    // rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = fgetc(fbmpr);
            green = fgetc(fbmpr);
            red = fgetc(fbmpr);
            rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
        }
    }
    fclose(fbmpr);

    // frame buffer をアロケートする、3倍の領域を取ってそれを3つに分ける
    if ((frame_buffer =(int *)malloc(MAX_FRAME_NUMBER * sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate frame_buffer0 ~ 2\n");
        exit(1);
    }

    // 3 つのフレームバッファにそれぞれ'A' を入力する
    memcpy(frame_buffer, rd_bmp, bmpihr.biHeight * bmpihr.biWidth * sizeof(int));

    memcpy((int *)((unsigned int)frame_buffer + bmpihr.biHeight * bmpihr.biWidth * sizeof(int)),
        rd_bmp, bmpihr.biHeight * bmpihr.biWidth * sizeof(int));

    memcpy((int *)((unsigned int)frame_buffer + 2 * bmpihr.biHeight * bmpihr.biWidth * sizeof(int)),
        rd_bmp, bmpihr.biHeight * bmpihr.biWidth * sizeof(int));

    DMA_Read_addr((volatile int *)0, outs_dummy, (unsigned int)frame_buffer,
        (unsigned int)frame_buffer+(bmpihr.biWidth * bmpihr.biHeight * sizeof(int)),
        (unsigned int)frame_buffer+(2 * (bmpihr.biWidth * bmpihr.biHeight) * sizeof(int)),
        active_frame, DMA_WRITE_MODE);

    DMA_Read_addr((volatile int *)0, outs, (unsigned int)frame_buffer,
        (unsigned int)frame_buffer+(bmpihr.biWidth * bmpihr.biHeight * sizeof(int)),
        (unsigned int)frame_buffer+(2 * (bmpihr.biWidth * bmpihr.biHeight) * sizeof(int)),
        active_frame, FREE_RUN_MODE);

    // outs ストリームのデータを buf に入力する
    for (int k=0; k<3; k++){
        for(int j=0; j < bmpihr.biHeight; j++){
            for(int i=0; i < bmpihr.biWidth; i++){
                outs >> vals;
                ap_int<32> val = vals.data;
                buf[(k*bmpihr.biWidth*bmpihr.biHeight)+(j*bmpihr.biWidth)+i] = (int)val;
            }
        }
    }

    // DMAされたデータをBMPフィルに書き込む
    char output_file[] = "dma_result0.bmp";
    for (int i=0; i<MAX_FRAME_NUMBER; i++){
        switch (i){
            case 0:
                strcpy(output_file,"dma_result0.bmp");
                break;
            case 1:
                strcpy(output_file,"dma_result1.bmp");
                break;
            case 2:
                strcpy(output_file,"dma_result2.bmp");
                break;
        }
        if ((fbmpw=fopen(output_file, "wb")) == NULL){
            fprintf(stderr, "Can't open %s by binary write mode\n", output_file);
            exit(1);
        }
        // BMPファイルヘッダの書き込み
        fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
        fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
        fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
        fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
        fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
        fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);

        // RGB データの書き込み、逆順にする
        int offset = i * bmpihr.biWidth * bmpihr.biHeight;
        for (int y=0; y<bmpihr.biHeight; y++){
            for (int x=0; x<bmpihr.biWidth; x++){
                blue = buf[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
                green = (buf[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
                red = (buf[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;

                fputc(blue, fbmpw);
                fputc(green, fbmpw);
                fputc(red, fbmpw);
            }
        }
        fclose(fbmpw);
    }
    free(rd_bmp);
    free(frame_buffer);
    return 0;
}

これでC シミュレーションを実行した。


dma_result0.bmp ～ dma_result2.bmp の3つの画像はすべて正常だった。

C コードの合成を行った。


3つの画像のDMA で Latency が 9242 なので、1クロックごとに1ピクセルを処理できているようだ。
LUT は 10 % が使用されているので、リソース使用量は多いと言えると思う。

C/RTL 協調シミュレーションを行った。


9324 クロックだった。

C/RTL 協調シミュレーション波形を示す。


m_axi_in_r_ARLEN は 0f で 16 バースト転送となっていた。
dma_result0.bmp ～ dma_result2.bmp の3つの画像は真っ黒でDMA Write版と同じで、うまく行ってなかった。



outs_TVALID と outs_TREADY がほとんど 1 のままなので、連続して出力されていることがわかる。

最後に、AXI4 Lite Slave の最初の設定部分を示す。

Zybot の走行試験

Zybotの走行試験を繰り返していますが、静止状態からPWM出力を 80% にしてしまうと、ZYBO の反応が無くなってしまいます。どうやら、バッテリーの電圧が降下して、ZYBOがリセットされているような気がします。
急にPWM出力を急上昇してしまうとバッテリーにも良くないと思うので、2段階や3段階で段々とPWM出力を上げていくようなアプリケーションを作る必要があるようです。

右車輪と左車輪のPWM 値を変更して左右に曲がるのは確認できました。

それと、モーター・モニタの値を左右でそろえて（つまりモーターの回転数が同じ）、真っ直ぐ走るかどうかを確認したいですね。

Zybot が走った

”ZybotのZYBOをLinuxで動作させる1（BOOT.bin, devicetree.dtb）”でPWMモジュールIP やモーター・モニタIP を追加したハードウェアにしたBOOT.bin とデバイス・ツリーをSDカードに追加して、Linux を立ち上げることができた。
今回は、Ubuntu 上でPWMモジュールIP とモーター・モニタIP を制御するアプリケーション・ソフトウェアを作成して、Zybot を走らせてみることにする。

まずは、Ubuntu 14.04 LTS で、Zybot を動かすアプリケーションを作成した。
zybot_motor.cpp を示す。

// zybot_motor.cpp
// 2016/07/07 by marsee
//


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

#include "xpwm.h"
#include "xmotor_monitor.h"

void motor_settings(XPwm *motorLp, XPwm *motorRp){
    XPwm_DisableAutoRestart(motorLp);
    while(!XPwm_IsIdle(motorLp)) ;
    XPwm_Start(motorLp);
    XPwm_EnableAutoRestart(motorLp);
    
     XPwm_DisableAutoRestart(motorRp);
    while(!XPwm_IsIdle(motorRp)) ;
    XPwm_Start(motorRp);
    XPwm_EnableAutoRestart(motorRp);
}

void Stopped_Zybot(XPwm *motorLp, XPwm *motorRp){
    XPwm_Set_sw_late_V(motorLp, 0);
    XPwm_Set_sw_late_V(motorRp, 0);
}

int main(){
    XPwm motorL, motorR;
    XPwm *motorLp, *motorRp;
    XMotor_monitor mmL, mmR;
    XMotor_monitor *mmLp, *mmRp;
    int c = 0;
    int motorvalL, motorvalR;
    int mmonitorL, mmonitorR;
    
    motorLp = &motorL;
    motorRp = &motorR;
    mmLp = &mmL;
    mmRp = &mmR;
    
    // Initialization of motor
    if (XPwm_Initialize(motorLp, "pwm_0") != XST_SUCCESS){
        fprintf(stderr,"pwm_0 (Left) open error\n");
        exit(-1);
    }
    if (XPwm_Initialize(motorRp, "pwm_1") != XST_SUCCESS){
        fprintf(stderr,"pwm_1 (Right) open error\n");
        exit(-1);
    }
    
    // Initialization of motor monitor
    if (XMotor_monitor_Initialize(mmLp, "motor_monitor_0") != XST_SUCCESS){
        fprintf(stderr,"motor_monitor_0 (Left) open error\n");
        exit(-1);
    }
    if (XMotor_monitor_Initialize(mmRp, "motor_monitor_1") != XST_SUCCESS){
        fprintf(stderr,"motor_monitor_1 (Right) open error\n");
        exit(-1);
    }

    // The Motors is rotated in the forward direction.
    XPwm_Set_sw_late_V(&motorL, 0);
    XPwm_Set_dir_V(&motorL, 1);    

    XPwm_Set_sw_late_V(&motorR, 0);
     XPwm_Set_dir_V(&motorR, 0);

    motor_settings(motorLp, motorRp);
    
    // main loop
    printf("Zybot Motor Control Application Start...\n");    
    while(c != 'q'){
        c = getc(stdin);
        switch ((char)c) {
            case 'l':{ // Left motor value setting
                printf("Please input Left motor value = ");
                scanf("%d", &c);
                motorvalL = c;
                break;
            }
            case 'r':{ // Right motor value setting
                printf("Please input Right motor value = ");            
                scanf("%d", &c);
                motorvalR = c;
                break;
            }
            case 't':{ // To change the power of the right and left motor.
                printf("Left motor = %d, Right motor = %d\n", motorvalL, motorvalR);
                XPwm_Set_sw_late_V(&motorL, motorvalL);
                XPwm_Set_sw_late_V(&motorR, motorvalR);
                break;
            }                                
            case 'm':{ // Enter the difference between the motor value.
                printf("motor value = L %d, R %d\n", motorvalL, motorvalR);
                printf("Please input Motor output difference = ");
                scanf("%d", &c);
                motorvalL = motorvalL + c;
                motorvalR = motorvalR + c;
                if (motorvalL < 0)
                    motorvalL = 0;
                if (motorvalR < 0)
                     motorvalR = 0;
                if (motorvalL > 100)
                     motorvalL = 100;
                if (motorvalR > 100)
                     motorvalR = 100;
                XPwm_Set_sw_late_V(&motorL, motorvalL);    
                XPwm_Set_sw_late_V(&motorR, motorvalR);
                break;
            }
            case 'a':{ // The Motors is rotated in the forward or reverse direction.
                if (XPwm_Get_dir_V(&motorL)){ // forward
                    Stopped_Zybot(motorLp, motorRp);
                    sleep(1);
                    
                    XPwm_Set_sw_late_V(&motorL, motorvalL);    
                    XPwm_Set_sw_late_V(&motorR, motorvalR);
                    
                    XPwm_Set_dir_V(&motorL, 0); // reverse
                    XPwm_Set_dir_V(&motorR, 1);
                    
                }else{
                    Stopped_Zybot(motorLp, motorRp);
                    sleep(1);
                    
                    XPwm_Set_sw_late_V(&motorL, motorvalL);    
                    XPwm_Set_sw_late_V(&motorR, motorvalR);

                    XPwm_Set_dir_V(&motorL, 1); // forward
                    XPwm_Set_dir_V(&motorR, 0);
                }
                break;
            }
            case 'd':{ // Displayed the motor monitor value
                while (!XMotor_monitor_IsIdle(&mmL));
                XMotor_monitor_Start(&mmL);
                         
                while (!XMotor_monitor_IsIdle(&mmR));
                XMotor_monitor_Start(&mmR);
            
                while (!XMotor_monitor_IsIdle(&mmL));
                while (!XMotor_monitor_IsIdle(&mmR));
            
                unsigned int sa_countL = (unsigned int)XMotor_monitor_Get_sa_count_V(&mmL);        
                unsigned int sa_countR = (unsigned int)XMotor_monitor_Get_sa_count_V(&mmR);        

                unsigned int sb_levelL = (unsigned int)XMotor_monitor_Get_sb_level_V(&mmL);
                unsigned int sb_levelR = (unsigned int)XMotor_monitor_Get_sb_level_V(&mmR);  
     
                 unsigned int returnL = (unsigned int)XMotor_monitor_Get_return(&mmL);
                 unsigned int returnR = (unsigned int)XMotor_monitor_Get_return(&mmR);
                 
                 unsigned int sum_cntL = (unsigned int)XMotor_monitor_Get_sum_count(&mmL);
                 unsigned int sum_cntR = (unsigned int)XMotor_monitor_Get_sum_count(&mmR);
                 
                printf("sa_countL = %d, sb_levelL = %d, retrunL = %d, sum_cntL = %d\n", sa_countL, sb_levelL, returnL, sum_cntL);
                 printf("freqency = %dHz, peirod = %fms\n", (int)(1/((float)sa_countL/1000000)), (float)sa_countL/1000);

                printf("sa_countR = %d, sb_levelR = %d, retrunR = %d, sum_cntR = %d\n", sa_countR, sb_levelR, returnR, sum_cntR);
                 printf("freqency = %dHz, peirod = %fms\n", (int)(1/((float)sa_countR/1000000)), (float)sa_countR/1000);
                 break;
             }
             case 's':{ // Stop
                 Stopped_Zybot(motorLp, motorRp);
                 motorvalL = 0;
                 motorvalR = 0;
                 break;
             }
             case 'h':{ // help
                 printf("l: Left motor value setting\n");
                 printf("r: Right motor value setting\n");
                 printf("t: To change the power of the right and left motor\n");
                 printf("m: Enter the difference between the motor value. eg. 10, -10\n");
                 printf("a: The Motors is rotated in the forward or reverse direction. Altarnative\n");
                 printf("d: Displayed the motor monitor value\n");
                 printf("s: Stop\n");
                 printf("q: Quit\n");
                 break;
             }
         }
    }

     return 0;
}

次にMakefile を示す。

#Makefile
# Referred to http://www.ie.u-ryukyu.ac.jp/~e085739/c.makefile.tuts.html

PROGRAM = zybot_motor
OBJS = zybot_motor.o xpwm_linux.o xpwm.o xmotor_monitor_linux.o xmotor_monitor.o

CC = gcc
CFLAGS = -Wall -O2

.SUFFIXES: .c .o

.PHONY: all

all: zybot_motor

zybot_motor: $(OBJS)
    $(CC) -Wall -o $@ $(OBJS)
    
.c.o:
    $(CC) $(CFLAGS) -c $<

    
.PHONY: clean
clean:
    $(RM) $(PROGRAM) $(OBJS)
    

make コマンドでコンパイルを行った。その様子を下図に示す。


zybot_motor コマンドが作成された。

zybot_motor コマンドを使っている様子を示す。なお、これはまだ走らせてなくタイヤは空転状態だったので、かなり速く回っている。走らせているときは、PWM 値が 30 くらいだと、190 Hz 程度だった。


Zybot をうまく走らせることができた。結構速い。左右のタイヤは独立に固定しあるので、左右のタイヤの向きがいい加減になっているのをどうしたらよいかな？と思っている。

Zybot の今の様子を下に示す。まだ仮組で走行アプリケーションの具合を確かめるために走らせてみただけだ。あとで、レーザー加工機と3Dプリンタで車体を作ることにしようと思う。


なお、無線LANアダプタを付けてあるので、無線操縦になっている。

Vivado HLS で DMA Write IP を作る（オフセット・アドレス指定編）

”Vivado HLS で DMA Write IP を作る（絶対アドレス指定編）”の続き。

前回は、絶対アドレスを入れてDMA 転送を行うCソースコードを書いたが、C/RTL 協調シミュレーションで、結果のBMP ファイルがおかしくなってしまった。今回は、絶対アドレスでなくオフセット・アドレスを入れてDMA 転送を行うCソースコードを書いてみた。

オフセット・アドレスを指定してDMA を行う DMA_Write.cpp を示す。こちらは、AXI4 Master の設定を offset=slave にしてある。

// DMA_Write.cpp
// 2016/07/10 by marsee
//
// fb0_offset_addr, fb1_offset_addr, fb2_offset_addr にはベースアドレスからのオフセット・アドレスを入れる
//

#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>

#include "DMA_Write.h"

int DMA_Write(hls::stream<ap_axis<32,1,1,1> >& ins, volatile int *out,
        unsigned int fb0_offset_addr, unsigned int fb1_offset_addr,
        unsigned int fb2_offset_addr, volatile ap_uint<2> & active_frame){
#pragma HLS INTERFACE ap_vld port=active_frame
#pragma HLS INTERFACE s_axilite port=fb0_offset_addr
#pragma HLS INTERFACE s_axilite port=fb1_offset_addr
#pragma HLS INTERFACE s_axilite port=fb2_offset_addr
#pragma HLS INTERFACE m_axi depth=9216 port=out offset=slave
#pragma HLS INTERFACE axis port=ins
#pragma HLS INTERFACE s_axilite port=return

    ap_axis<32,1,1,1> pix;
    int dma_index;

    for (int i=0; i<MAX_FRAME_NUMBER; i++){
        switch (i){
            case 0 :
                dma_index = fb0_offset_addr/sizeof(int);
                break;
            case 1 :
                dma_index = fb1_offset_addr/sizeof(int);
                break;
            case 2 :
                dma_index = fb2_offset_addr/sizeof(int);
                break;
        } 
        active_frame = i;

        do { // user が 1になった時にフレームがスタートする
#pragma HLS LOOP_TRIPCOUNT min=1 max=1 avg=1
            ins >> pix;
        } while(pix.user == 0);

        for (int y=0; y<VERTICAL_PIXEL_WIDTH; y++){
            for (int x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
                if (!(x==0 && y==0))    // 最初の入力はすでに入力されている
                    ins >> pix;    // AXI4-Stream からの入力
                out[dma_index+(y*HORIZONTAL_PIXEL_WIDTH)+x] = pix.data;
            }
        }
    }
    return 0;
}

次に、DMA_Write.h を示す。
// DMA_Write.h
// 2016/07/10 by marsee
//

//#define HORIZONTAL_PIXEL_WIDTH 800
//#define VERTICAL_PIXEL_WIDTH 600

#define HORIZONTAL_PIXEL_WIDTH 64
#define VERTICAL_PIXEL_WIDTH 48

#define ALL_PIXEL_VALUE (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)

#define MAX_FRAME_NUMBER 3

DMA_Write_tb.cpp を示す。テストベンチは'A' という文字のBMPファイルを3つの領域にDMAするようになっている。DMAされた文字は3つのBMPファイルに落とされる。

// DMA_Write_tb.cpp
// 2016/07/10 by marsee
//

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <ap_axi_sdata.h>

#include "DMA_Write.h"
#include "bmp_header.h"

int DMA_Write(hls::stream<ap_axis<32,1,1,1> >& ins, volatile int *out,
        unsigned int fb0_offset_addr, unsigned int fb1_offset_addr,
        unsigned int fb2_offset_addr, volatile ap_uint<2> & active_frame);

int main()
{
    using namespace std;

    hls::stream<ap_axis<32,1,1,1> > ins;
    ap_axis<32,1,1,1> pix;

    BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
    BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
    FILE *fbmpr, *fbmpw;
    int *rd_bmp, *hw_lapd;
    int blue, green, red;
    ap_uint<2> active_frame;
    int *frame_buffer;

    if ((fbmpr = fopen("test.bmp", "rb")) == NULL){ // test.bmp をオープン
        fprintf(stderr, "Can't open test.bmp by binary read mode\n");
        exit(1);
    }
    // bmpヘッダの読み出し
    fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
    fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
    fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
    fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);

    // ピクセルを入れるメモリをアロケートする
    if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate rd_bmp memory\n");
        exit(1);
    }

    // rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = fgetc(fbmpr);
            green = fgetc(fbmpr);
            red = fgetc(fbmpr);
            rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
        }
    }
    fclose(fbmpr);

    // ins に入力データを用意する
    for(int i=0; i<5; i++){    //　dummy data
           pix.user = 0;
         pix.data = i;
        ins << pix;
    }

    for(int k=0; k<MAX_FRAME_NUMBER; k++){
        for(int j=0; j < bmpihr.biHeight; j++){
            for(int i=0; i < bmpihr.biWidth; i++){
                pix.data = (ap_int<32>)rd_bmp[(j*bmpihr.biWidth)+i];

                if (j==0 && i==0)    // 最初のデータの時に TUSER を 1 にする
                    pix.user = 1;
                else
                    pix.user = 0;

                if (i == bmpihr.biWidth-1) // 行の最後でTLASTをアサートする
                    pix.last = 1;
                else
                    pix.last = 0;

                ins << pix;
            }
        }
    }

    // frame buffer をアロケートする、3倍の領域を取ってそれを3つに分ける
    if ((frame_buffer =(int *)malloc(MAX_FRAME_NUMBER * sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate frame_buffer0 ~ 2\n");
        exit(1);
    }

    DMA_Write(ins, (volatile int *)frame_buffer, (unsigned int)0,
        (unsigned int)(bmpihr.biWidth * bmpihr.biHeight * sizeof(int)),
        (unsigned int)(2 * (bmpihr.biWidth * bmpihr.biHeight) * sizeof(int)),
        active_frame);
    
    // DMAされたデータをBMPフィルに書き込む
    char output_file[] = "dma_result0.bmp";
    for (int i=0; i<MAX_FRAME_NUMBER; i++){
        switch (i){
            case 0:
                strcpy(output_file,"dma_result0.bmp");
                break;
            case 1:
                strcpy(output_file,"dma_result1.bmp");
                break;
            case 2:
                strcpy(output_file,"dma_result2.bmp");
                break;
        }
        if ((fbmpw=fopen(output_file, "wb")) == NULL){
            fprintf(stderr, "Can't open %s by binary write mode\n", output_file);
            exit(1);
        }
        // BMPファイルヘッダの書き込み
        fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
        fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
        fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
        fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
        fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
        fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);

        // RGB データの書き込み、逆順にする
        int offset = i * bmpihr.biWidth * bmpihr.biHeight;
        for (int y=0; y<bmpihr.biHeight; y++){
            for (int x=0; x<bmpihr.biWidth; x++){
                blue = frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
                green = (frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
                red = (frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;

                fputc(blue, fbmpw);
                fputc(green, fbmpw);
                fputc(red, fbmpw);
            }
        }
        fclose(fbmpw);
    }       
    free(rd_bmp);
    free(frame_buffer);
    return 0;
}

Vivado HLS のプロジェクトを示す。


C シミュレーションを行った。
dma_result0.bmp ～ dma_result2.bmp ができていて、同じ 'A' という文字の画像だった。DMA できている。

次に Cコードの合成を行った。


絶対アドレス版のLatency は 9241 クロックだったが、オフセット・アドレス版は、9244 クロックで 3 クロック増えている。それは、Detial のLoop でLoop 1 のIteration Latency が 3081 クロックで絶対アドレス版に比べて 1 クロック増えていることが考えられる。
絶対アドレス版に比べて、FF とLUTの使用量が増えていた。

合成されたAXI4 Lite SlaveのVerilog HDL ファイルのDMA_Write_AXILiteS_s_axi.v のアドレスマップを下に示す。

//------------------------Address Info-------------------
// 0x00 : Control signals
//        bit 0  - ap_start (Read/Write/COH)
//        bit 1  - ap_done (Read/COR)
//        bit 2  - ap_idle (Read)
//        bit 3  - ap_ready (Read)
//        bit 7  - auto_restart (Read/Write)
//        others - reserved
// 0x04 : Global Interrupt Enable Register
//        bit 0  - Global Interrupt Enable (Read/Write)
//        others - reserved
// 0x08 : IP Interrupt Enable Register (Read/Write)
//        bit 0  - Channel 0 (ap_done)
//        bit 1  - Channel 1 (ap_ready)
//        others - reserved
// 0x0c : IP Interrupt Status Register (Read/TOW)
//        bit 0  - Channel 0 (ap_done)
//        bit 1  - Channel 1 (ap_ready)
//        others - reserved
// 0x10 : Data signal of ap_return
//        bit 31~0 - ap_return[31:0] (Read)
// 0x18 : Data signal of out_r
//        bit 31~0 - out_r[31:0] (Read/Write)
// 0x1c : reserved
// 0x20 : Data signal of fb0_offset_addr
//        bit 31~0 - fb0_offset_addr[31:0] (Read/Write)
// 0x24 : reserved
// 0x28 : Data signal of fb1_offset_addr
//        bit 31~0 - fb1_offset_addr[31:0] (Read/Write)
// 0x2c : reserved
// 0x30 : Data signal of fb2_offset_addr
//        bit 31~0 - fb2_offset_addr[31:0] (Read/Write)
// 0x34 : reserved
// (SC = Self Clear, COR = Clear on Read, TOW = Toggle on Write, COH = Clear on Handshake)

0x18 : Data signal of out_r があって、出力用のAXI4 Master のオフセット・アドレスが指定できることがわかる。各フレームバッファのアドレス指定は、AXI4 Master のオフセット・アドレスからのオフセットで指定する。fb0_offset_addr、fb1_offset_addr、fb2_offset_addr がそうだ。

これで C/RTL 協調シミュレーションを行った。


9335 クロックだったが、絶対アドレス版よりも 11 クロック多い。

オフセット・アドレス版は、絶対アドレス版と違って、DMA_Write_addr\solution1\sim\wrapc_pc の dma_result0.bmp ～ dma_result2.bmp ができていて、同じ 'A' という文字の画像だった。DMA できていた。

C/RTL 協調シミュレーション波形を示す。


絶対アドレス版と同様に、C Inputs -> ins_TVALID と ins_TREADY が両方ともほとんど 1 なので、1クロックに1回データ転送を行えていることがわかる。
Write Channel の m_axi_out_r_AWLEN[7:0] は 0f なので、16 バースト・ライトになっていることがわかる。



最初のAXI4 Lite Slave の設定の様子を観察してみた。


絶対アドレス版と違って、0x18 番地にオフセット・アドレスを設定してから、フレームバッファのアドレスを設定している（0x20, 0x28, 0x30）
オフセット・アドレスに設定された値は 0 だった。この辺りがDMA_Write_addr\solution1\sim\wrapc_pc の dma_result0.bmp ～ dma_result2.bmp にうまく書けている原因なのかもしれない？

Vivado HLS で DMA Write IP を作る（絶対アドレス指定編）

AXI4 Master でフィルタを実装するコードを考えていると、やはり、memcpy() と中のフィルタ処理を重ね合わせられないので、すべてVivado HLS で作ると性能が出しにくい。
そこで、フィルタ部分はAXI4 Stream で作っておいて、DMA 部分は別にAXI4 Master Read - AXI4 Stream のDMA Read とAXI4 Stream - AXI4 Master Write の DMA Write の2つのIP をVivado HLS で作った方が良さそうだ。

ラプラシアンフィルタやアンシャープマスクフィルタのAXI4 Stream 版はあるので、AXI4 Master Write のDMA Write IP をVivado HLS で作ってみることにした。当然、Xilinx 社のAXI VDMAを使用するのが王道だが、なかなか使うのが難しいという側面がある。

DMA Write IP を作る際にはいろいろなバージョンを作ってきた。一番手っ取り早いのは、アドレスを指定することなのだが、1つならばINTERFACE 指示子の m_axi で offset=slave オプションを付ければよいのだが、今回は3つのDMAアドレスを入力して、順番にWrite していこうと思っている。その場合に、どう書けば良いかを探ってみた。なお、現在、何番目のフレームバッファにWrite しているのかを示す active_frame という信号を追加してある。これは、DMA Read 側が見て、今何番目のフレームを書いているかを知ることができるようにして、一番最近に書いたフレームバッファをDMA Read IP がRead できるようにするためだ。

それでは、絶対アドレスを指定してDMA を行う DMA_Write.cpp を示す。

// DMA_Write.cpp
// 2016/07/10 by marsee
//
// frame_buffer0, frame_buffer1, frame_buffer2 には3つのフレームバッファのアドレスを入れる
//

#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>

#include "DMA_Write.h"

int DMA_Write(hls::stream<ap_axis<32,1,1,1> >& ins, volatile int *out,
        unsigned int frame_buffer0, unsigned int frame_buffer1,
        unsigned int frame_buffer2, volatile ap_uint<2> & active_frame){
#pragma HLS INTERFACE ap_vld port=active_frame
#pragma HLS INTERFACE s_axilite port=frame_buffer0
#pragma HLS INTERFACE s_axilite port=frame_buffer1
#pragma HLS INTERFACE s_axilite port=frame_buffer2
#pragma HLS INTERFACE m_axi depth=5000000 port=out offset=off
#pragma HLS INTERFACE axis port=ins
#pragma HLS INTERFACE s_axilite port=return

    ap_axis<32,1,1,1> pix;
    int dma_index;

    for (int i=0; i<MAX_FRAME_NUMBER; i++){
        switch (i){
            case 0 :
                dma_index = frame_buffer0/sizeof(int);
                break;
            case 1 :
                dma_index = frame_buffer1/sizeof(int);
                break;
            case 2 :
                dma_index = frame_buffer2/sizeof(int);
                break;
        } 
        active_frame = i;

        do { // user が 1になった時にフレームがスタートする
#pragma HLS LOOP_TRIPCOUNT min=1 max=1 avg=1
            ins >> pix;
        } while(pix.user == 0);

        for (int y=0; y<VERTICAL_PIXEL_WIDTH; y++){
            for (int x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
                if (!(x==0 && y==0))    // 最初の入力はすでに入力されている
                    ins >> pix;    // AXI4-Stream からの入力
                out[dma_index+(y*HORIZONTAL_PIXEL_WIDTH)+x] = pix.data;
            }
        }
    }
    return 0;
}

次に、DMA_Write.h を示す。

// DMA_Write.h
// 2016/07/10 by marsee
//

//#define HORIZONTAL_PIXEL_WIDTH 800
//#define VERTICAL_PIXEL_WIDTH 600

#define HORIZONTAL_PIXEL_WIDTH 64
#define VERTICAL_PIXEL_WIDTH 48

#define ALL_PIXEL_VALUE (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)

#define MAX_FRAME_NUMBER 3

DMA_Write_tb.cpp を示す。テストベンチは'A' という文字のBMPファイルを3つの領域にDMAするようになっている。DMAされた文字は3つのBMPファイルに落とされる。

// DMA_Write_tb.cpp
// 2016/07/10 by marsee
//

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <ap_axi_sdata.h>

#include "DMA_Write.h"
#include "bmp_header.h"

int DMA_Write(hls::stream<ap_axis<32,1,1,1> >& ins, volatile int *out,
        unsigned int fb0_offset_addr, unsigned int fb1_offset_addr,
        unsigned int fb2_offset_addr, volatile ap_uint<2> & active_frame);

int main()
{
    using namespace std;

    hls::stream<ap_axis<32,1,1,1> > ins;
    ap_axis<32,1,1,1> pix;

    BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
    BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
    FILE *fbmpr, *fbmpw;
    int *rd_bmp, *hw_lapd;
    int blue, green, red;
    ap_uint<2> active_frame;
    int *frame_buffer;

    if ((fbmpr = fopen("test.bmp", "rb")) == NULL){ // test.bmp をオープン
        fprintf(stderr, "Can't open test.bmp by binary read mode\n");
        exit(1);
    }
    // bmpヘッダの読み出し
    fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
    fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
    fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
    fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);

    // ピクセルを入れるメモリをアロケートする
    if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate rd_bmp memory\n");
        exit(1);
    }

    // rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = fgetc(fbmpr);
            green = fgetc(fbmpr);
            red = fgetc(fbmpr);
            rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
        }
    }
    fclose(fbmpr);

    // ins に入力データを用意する
    for(int i=0; i<5; i++){    //　dummy data
           pix.user = 0;
         pix.data = i;
        ins << pix;
    }

    for(int k=0; k<MAX_FRAME_NUMBER; k++){
        for(int j=0; j < bmpihr.biHeight; j++){
            for(int i=0; i < bmpihr.biWidth; i++){
                pix.data = (ap_int<32>)rd_bmp[(j*bmpihr.biWidth)+i];

                if (j==0 && i==0)    // 最初のデータの時に TUSER を 1 にする
                    pix.user = 1;
                else
                    pix.user = 0;

                if (i == bmpihr.biWidth-1) // 行の最後でTLASTをアサートする
                    pix.last = 1;
                else
                    pix.last = 0;

                ins << pix;
            }
        }
    }

    // frame buffer をアロケートする、3倍の領域を取ってそれを3つに分ける
    if ((frame_buffer =(int *)malloc(MAX_FRAME_NUMBER * sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate frame_buffer0 ~ 2\n");
        exit(1);
    }

    DMA_Write(ins, (volatile int *)frame_buffer, (unsigned int)0,
        (unsigned int)(bmpihr.biWidth * bmpihr.biHeight * sizeof(int)),
        (unsigned int)(2 * (bmpihr.biWidth * bmpihr.biHeight) * sizeof(int)),
        active_frame);
    
    // DMAされたデータをBMPフィルに書き込む
    char output_file[] = "dma_result0.bmp";
    for (int i=0; i<MAX_FRAME_NUMBER; i++){
        switch (i){
            case 0:
                strcpy(output_file,"dma_result0.bmp");
                break;
            case 1:
                strcpy(output_file,"dma_result1.bmp");
                break;
            case 2:
                strcpy(output_file,"dma_result2.bmp");
                break;
        }
        if ((fbmpw=fopen(output_file, "wb")) == NULL){
            fprintf(stderr, "Can't open %s by binary write mode\n", output_file);
            exit(1);
        }
        // BMPファイルヘッダの書き込み
        fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
        fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
        fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
        fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
        fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
        fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);

        // RGB データの書き込み、逆順にする
        int offset = i * bmpihr.biWidth * bmpihr.biHeight;
        for (int y=0; y<bmpihr.biHeight; y++){
            for (int x=0; x<bmpihr.biWidth; x++){
                blue = frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
                green = (frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
                red = (frame_buffer[offset+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;

                fputc(blue, fbmpw);
                fputc(green, fbmpw);
                fputc(red, fbmpw);
            }
        }
        fclose(fbmpw);
    }       
    free(rd_bmp);
    free(frame_buffer);
    return 0;
}

Vivado HLS のプロジェクトを示す。


C シミュレーションを行った。
 

dma_result0.bmp ～ dma_result2.bmp ができていて、同じ 'A' という文字の画像だった。DMA できている。

次に Cコードの合成を行った。


Estimated は 8.75 ns , Target は 10 ns なので、十分に満たしている。
Latency は 9241 クロックだ。
Detail の Loop を見ると Loop1 が 9240 クロックで、3080 クロックのIteration を 3 回で 9240 クロックだそうだ。良い感じだ。

合成されたAXI4 Lite SlaveのVerilog HDL ファイルのDMA_Write_AXILiteS_s_axi.v のアドレスマップを下に示す。

//------------------------Address Info-------------------
// 0x00 : Control signals
//        bit 0  - ap_start (Read/Write/COH)
//        bit 1  - ap_done (Read/COR)
//        bit 2  - ap_idle (Read)
//        bit 3  - ap_ready (Read)
//        bit 7  - auto_restart (Read/Write)
//        others - reserved
// 0x04 : Global Interrupt Enable Register
//        bit 0  - Global Interrupt Enable (Read/Write)
//        others - reserved
// 0x08 : IP Interrupt Enable Register (Read/Write)
//        bit 0  - Channel 0 (ap_done)
//        bit 1  - Channel 1 (ap_ready)
//        others - reserved
// 0x0c : IP Interrupt Status Register (Read/TOW)
//        bit 0  - Channel 0 (ap_done)
//        bit 1  - Channel 1 (ap_ready)
//        others - reserved
// 0x10 : Data signal of ap_return
//        bit 31~0 - ap_return[31:0] (Read)
// 0x18 : Data signal of frame_buffer0
//        bit 31~0 - frame_buffer0[31:0] (Read/Write)
// 0x1c : reserved
// 0x20 : Data signal of frame_buffer1
//        bit 31~0 - frame_buffer1[31:0] (Read/Write)
// 0x24 : reserved
// 0x28 : Data signal of frame_buffer2
//        bit 31~0 - frame_buffer2[31:0] (Read/Write)
// 0x2c : reserved
// (SC = Self Clear, COR = Clear on Read, TOW = Toggle on Write, COH = Clear on Handshake)

0x18 - frame_buffer0, 0x20 - frame_buffer1, 0x28 - frame_buffer2 に直接アドレスを書き込むことになる。

C/RTL 協調シミュレーション結果を示す。9324 クロックだった。


ところが、DMA_Write_addr\solution1\sim\wrapc_pc の dma_result0.bmp ～ dma_result2.bmp が’A’の画像にならない。すべて真っ黒の画像になってしまう。

C/RTL 協調シミュレーション波形を示す。


C Inputs -> ins_TVALID と ins_TREADY が両方ともほとんど 1 なので、1クロックに1回データ転送を行えていることがわかる。
Write Channel の m_axi_out_r_AWLEN[7:0] は 0f なので、16 バースト・ライトになっていることがわかる。



WDATA も正常のようだし、不具合が見つからない。



最初に AXI4 Lite Slave で何を設定しているかを見ると、frame_buffer0 のアドレスは、 0x 008bb198 だったようだ。別に問題ないと思うのだが、C/RTL 協調シミュレーションでだけ、ファイルが合っていない。
もしかしたら、大きなアドレスの時には C のテストベンチがRTLシミュレーションのデータを取りそこなうのだろうか？
実際に、この実装をIP にしても動作しそうな気がする。

FPGAの部屋のまとめサイトの更新（2016年7月9日）

FPGAの部屋のまとめサイトを更新しました。

Z-turn Board, Zybot を追加して、今日までの記事を各カテゴリに追加しました。

ZybotのZYBOをLinuxで動作させる1（BOOT.bin, devicetree.dtb）

”Zybot の車輪が回転した”と”Zybotの車輪のセンサー・フィードバックのデータを取得した”で、PWMモジュールでモーターを制御して車輪を動かすのとそのモーターのホール素子で回転数を取得することができた。今まではベアメタル・アプリケーションでやっていたので、これをLinuxに移行したい。それには、今までのZYBO_0_2 の右目用のZYBO の方に制御システムを載せようと思っている。
なお、Zybot のZYBO は右目用と左目用の2台のZYBO を載せる予定で、右目用から左目用ZYBO にHDMI で右目用画像を送って、ステレオカメラとしている。左目用ZYBO にはステレオカメラ処理が乗る予定なので、余裕がない。よって、右目用ZYBO の方に車体の制御と白線検出を任せる予定だ。

まずは、FSBL(First Stage Boot Loader) のアプリケーション・プロジェクトを新規作成して、FSBL を作成した。
その後、FSBL と FPGA のビットストリーム、u-boot.elf をまとめて、BOOT.bin を作成した。


次は、VirtualBox 上のUbuntu 14.04 LTS に移動して、ZYBO_0_2 のzynq-zybo.dts を編集して、Pwmモジュールとモーター・モニタのエントリを追加した。（以前のZYBO_0_2 のデバイス・ツリーについては、”並列ステレオカメラによる距離の測定8（Ubuntuで動作するアプリケーションを作る1）”を参照のこと）


dtc でzynq-zybo.dts をコンパイルして devicetree.dtb を作成した。
具体的には、dtc -I dts -O dtb -o devicetree.dtb zynq-zybo.dts コマンドで、zynq-zybo.dts を dtc でコンパイルして、devicetree.dtb を生成した。


出来上がった devicetree.dtb を示す。


BOOT.bin と devicetree.dtb をUbuntu 14.04 を起動する Micro SDカードの第1パーティションに入れて、ZYBO を起動した。

起動後、ターミナル・ソフト (Tera Term) で、/sys/devices/amba.0 の下を見た。devicetree に設定したデバイスが 登録されていた。


pwm_0, pwm_1, mootor_monitor_0, motor_monitor_1 も登録されていた。

次は、Vivado HLSで生成されたドライバを使用してLinux用のアプリケーション・ソフトウェアを作成しよう。

AXI4 Slave Bus Functional Model のVHDL版4

”AXI4 Slave Bus Functional Model のVHDL版２”の続き。

”AXI4 Slave Bus Functional Model のVerilog HDL版4”と同様に、Xilinx社のIP はAｘREADY がいつもは 1 になっていることから、AｘREADYがいつも 1 になるように変更した。これがデフォルトのモードで、AWREADY_IS_USUALLY_HIGH、ARREADY_IS_USUALLY_HIGH を 0 にすることで、従来のAxREADY がいつもは 0 のモードにすることができる。
動作はVerilog HDL版と同様だ。

axi_slave_BFM.vhd を貼っておく。
（追加）このHDLコードは無保証です。このコードを使用したことによる損害の保証はいたしません。ホビー向けとします。お仕事で使われるなど、信頼性が必要な用途には、Xilinx社で販売している売り物のBFMをご使用下さい。

-----------------------------------------------------------------------------
--
-- AXI Master用 Slave Bus Function Mode (BFM)
-- axi_slave_BFM.vhd
--
-----------------------------------------------------------------------------
-- 2012/02/25 : S_AXI_AWBURST＝1 (INCR) にのみ対応、AWSIZE, ARSIZE = 000 (1byte), 001 (2bytes), 010 (4bytes) のみ対応。
-- 2012/07/04 : READ_ONLY_TRANSACTION を追加。Read機能のみでも+1したデータを出力することが出来るように変更した。
-- sync_fifo を使用したオーバーラップ対応版
-- 2014/07/04 : M_AXIをスレーブに対応した名前のS_AXIに変更
-- 2014/07/16 : Write Respose Channel に sync_fifo を使用した
-- 2014/08/31 : READ_RANDOM_WAIT=1 の時に、S_AXI_RREADY が S_AXI_RVALID に依存するバグをフィック。
--              WRITE_RANDOM_WAIT=1 の時に、S_AXI_WVALID が S_AXI_WREADY に依存するバグをフィック。
--
-- 2016/07/03 : AWREADY_IS_USUALLY_HIGH　と ARREADY_IS_USUALLY_HIGH　の2つのパラメータを追加 by marsee
--
-- ライセンスは二条項BSDライセンス (2-clause BSD license)とします。
--


library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use ieee.std_logic_misc.all;

package m_seq_bfm_pack is
    function M_SEQ16_BFM_F(mseq16in : std_logic_vector
        )return std_logic_vector;
end package m_seq_bfm_pack;
package body m_seq_bfm_pack is
    function M_SEQ16_BFM_F(mseq16in : std_logic_vector
        )return std_logic_vector is
            variable mseq16 : std_logic_vector(15 downto 0);
            variable xor_result : std_logic;
    begin
        xor_result := mseq16in(15) xor mseq16in(12) xor mseq16in(10) xor mseq16in(8) xor mseq16in(7) xor mseq16in(6) xor mseq16in(3) xor mseq16in(2);
        mseq16 := mseq16in(14 downto 0) & xor_result;
        return mseq16;
    end M_SEQ16_BFM_F;
end m_seq_bfm_pack;


library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use ieee.std_logic_misc.all;
use IEEE.math_real.all;

library work;
use work.m_seq_bfm_pack.all;

--library unisim;
--use unisim.vcomponents.all;

entity axi_slave_bfm is
  generic (
    C_S_AXI_ID_WIDTH             : integer := 1;
    C_S_AXI_ADDR_WIDTH           : integer := 32;
    C_S_AXI_DATA_WIDTH           : integer := 32;
    C_S_AXI_AWUSER_WIDTH    : integer := 1;
    C_S_AXI_ARUSER_WIDTH    : integer := 1;
    C_S_AXI_WUSER_WIDTH     : integer := 1;
    C_S_AXI_RUSER_WIDTH     : integer := 1;
    C_S_AXI_BUSER_WIDTH      : integer := 1;
    
    C_S_AXI_TARGET            : integer := 0;
    C_OFFSET_WIDTH            : integer := 10; -- 割り当てるRAMのアドレスのビット幅
    C_S_AXI_BURST_LEN        : integer := 256;
    
    WRITE_RANDOM_WAIT        : integer := 1; -- Write Transaction のデータ転送の時にランダムなWaitを発生させる=1, Waitしない=0
    READ_RANDOM_WAIT        : integer := 0; -- Read Transaction のデータ転送の時にランダムなWaitを発生させる=1, Waitしない=0
    READ_DATA_IS_INCREMENT    : integer := 0; -- ReadトランザクションでRAMの内容をReadする = 0（RAMにWriteしたものをReadする）、Readデータを+1する = 1（データは+1したデータをReadデータとして使用する
    RANDOM_BVALID_WAIT        : integer := 0;    -- Write Data Transaction が終了した後で、BVALID をランダムにWaitする = 1、BVALID をランダムにWaitしない = 0, 31 ~ 0 クロックのWait
    AWREADY_IS_USUALLY_HIGH    : integer := 1; -- AWRAEDY は通常はLow=0, High=1
    ARREADY_IS_USUALLY_HIGH : integer := 1  -- AWRAEDY は通常はLow=0, High=1
    );
  port(
    -- System Signals
    ACLK    : in std_logic;
    ARESETN : in std_logic;

    -- Master Interface Write Address Ports
    S_AXI_AWID     : in  std_logic_vector(C_S_AXI_ID_WIDTH-1 downto 0);
    S_AXI_AWADDR   : in  std_logic_vector(C_S_AXI_ADDR_WIDTH-1 downto 0);
    S_AXI_AWLEN    : in  std_logic_vector(8-1 downto 0);
    S_AXI_AWSIZE   : in  std_logic_vector(3-1 downto 0);
    S_AXI_AWBURST  : in  std_logic_vector(2-1 downto 0);
    -- S_AXI_AWLOCK   : in  std_logic_vector(2-1 downto 0);
    S_AXI_AWLOCK   : in  std_logic_vector(1 downto 0);
    S_AXI_AWCACHE  : in  std_logic_vector(4-1 downto 0);
    S_AXI_AWPROT   : in  std_logic_vector(3-1 downto 0);
    S_AXI_AWQOS    : in  std_logic_vector(4-1 downto 0);
    S_AXI_AWUSER   : in  std_logic_vector(C_S_AXI_AWUSER_WIDTH-1 downto 0);
    S_AXI_AWVALID  : in  std_logic;
    S_AXI_AWREADY  : out std_logic;

    -- Master Interface Write Data Ports
    S_AXI_WDATA  : in  std_logic_vector(C_S_AXI_DATA_WIDTH-1 downto 0);
    S_AXI_WSTRB  : in  std_logic_vector(C_S_AXI_DATA_WIDTH/8-1 downto 0);
    S_AXI_WLAST  : in  std_logic;
    S_AXI_WUSER  : in  std_logic_vector(C_S_AXI_WUSER_WIDTH-1 downto 0);
    S_AXI_WVALID : in  std_logic;
    S_AXI_WREADY : out std_logic;

    -- Master Interface Write Response Ports
    S_AXI_BID    : out std_logic_vector(C_S_AXI_ID_WIDTH-1 downto 0);
    S_AXI_BRESP  : out std_logic_vector(2-1 downto 0);
    S_AXI_BUSER  : out std_logic_vector(C_S_AXI_BUSER_WIDTH-1 downto 0);
    S_AXI_BVALID : out std_logic;
    S_AXI_BREADY : in  std_logic;

    -- Master Interface Read Address Ports
    S_AXI_ARID     : in  std_logic_vector(C_S_AXI_ID_WIDTH-1 downto 0);
    S_AXI_ARADDR   : in  std_logic_vector(C_S_AXI_ADDR_WIDTH-1 downto 0);
    S_AXI_ARLEN    : in  std_logic_vector(8-1 downto 0);
    S_AXI_ARSIZE   : in  std_logic_vector(3-1 downto 0);
    S_AXI_ARBURST  : in  std_logic_vector(2-1 downto 0);
    S_AXI_ARLOCK   : in  std_logic_vector(2-1 downto 0);
    S_AXI_ARCACHE  : in  std_logic_vector(4-1 downto 0);
    S_AXI_ARPROT   : in  std_logic_vector(3-1 downto 0);
    S_AXI_ARQOS    : in  std_logic_vector(4-1 downto 0);
    S_AXI_ARUSER   : in  std_logic_vector(C_S_AXI_ARUSER_WIDTH-1 downto 0);
    S_AXI_ARVALID  : in  std_logic;
    S_AXI_ARREADY  : out std_logic;

    -- Master Interface Read Data Ports
    S_AXI_RID    : out std_logic_vector(C_S_AXI_ID_WIDTH-1 downto 0);
    S_AXI_RDATA  : out std_logic_vector(C_S_AXI_DATA_WIDTH-1 downto 0);
    S_AXI_RRESP  : out std_logic_vector(2-1 downto 0);
    S_AXI_RLAST  : out std_logic;
    S_AXI_RUSER  : out std_logic_vector(C_S_AXI_RUSER_WIDTH-1 downto 0);
    S_AXI_RVALID : out std_logic;
    S_AXI_RREADY : in  std_logic
    );

end axi_slave_bfm;

architecture implementation of axi_slave_bfm is

constant    AxBURST_FIXED    : std_logic_vector := "00";
constant    AxBURST_INCR    : std_logic_vector := "01";
constant    AxBURST_WRAP    : std_logic_vector := "10";

constant    RESP_OKAY        : std_logic_vector := "00";
constant    RESP_EXOKAY        : std_logic_vector := "01";
constant    RESP_SLVERR        : std_logic_vector := "10";
constant    RESP_DECERR        : std_logic_vector := "11";

constant    DATA_BUS_BYTES     : natural := C_S_AXI_DATA_WIDTH/8; -- データバスのビット幅
constant    ADD_INC_OFFSET    : natural := natural(log(real(DATA_BUS_BYTES), 2.0));

-- fifo depth for address
constant    AD_FIFO_DEPTH            : natural := 16;

-- wad_fifo field
constant    WAD_FIFO_WIDTH            : natural := C_S_AXI_ADDR_WIDTH+5+C_S_AXI_ID_WIDTH-1+1;
constant    WAD_FIFO_AWID_HIGH        : natural := C_S_AXI_ADDR_WIDTH+5+C_S_AXI_ID_WIDTH-1;
constant    WAD_FIFO_AWID_LOW        : natural := C_S_AXI_ADDR_WIDTH+5;
constant    WAD_FIFO_AWBURST_HIGH    : natural := C_S_AXI_ADDR_WIDTH+4;
constant    WAD_FIFO_AWBURST_LOW    : natural := C_S_AXI_ADDR_WIDTH+3;
constant    WAD_FIFO_AWSIZE_HIGH    : natural := C_S_AXI_ADDR_WIDTH+2;
constant    WAD_FIFO_AWSIZE_LOW        : natural := C_S_AXI_ADDR_WIDTH;
constant    WAD_FIFO_ADDR_HIGH        : natural := C_S_AXI_ADDR_WIDTH-1;
constant    WAD_FIFO_ADDR_LOW        : natural := 0;

-- wres_fifo field
constant    WRES_FIFO_WIDTH            : natural := 2+C_S_AXI_ID_WIDTH-1+1;
constant    WRES_FIFO_AWID_HIGH        : natural := 2+C_S_AXI_ID_WIDTH-1;
constant    WRES_FIFO_AWID_LOW        : natural := 2;
constant    WRES_FIFO_AWBURST_HIGH    : natural := 1;
constant    WRES_FIFO_AWBURST_LOW    : natural := 0;

-- rad_fifo field
constant    RAD_FIFO_WIDTH            : natural := C_S_AXI_ADDR_WIDTH+13+C_S_AXI_ID_WIDTH-1+1;
constant    RAD_FIFO_ARID_HIGH        : natural := C_S_AXI_ADDR_WIDTH+13+C_S_AXI_ID_WIDTH-1;
constant    RAD_FIFO_ARID_LOW        : natural := C_S_AXI_ADDR_WIDTH+13;
constant    RAD_FIFO_ARBURST_HIGH    : natural := C_S_AXI_ADDR_WIDTH+12;
constant    RAD_FIFO_ARBURST_LOW    : natural := C_S_AXI_ADDR_WIDTH+11;
constant    RAD_FIFO_ARSIZE_HIGH    : natural := C_S_AXI_ADDR_WIDTH+10;
constant    RAD_FIFO_ARSIZE_LOW        : natural := C_S_AXI_ADDR_WIDTH+8;
constant    RAD_FIFO_ARLEN_HIGH        : natural := C_S_AXI_ADDR_WIDTH+7;
constant    RAD_FIFO_ARLEN_LOW        : natural := C_S_AXI_ADDR_WIDTH;
constant    RAD_FIFO_ADDR_HIGH        : natural := C_S_AXI_ADDR_WIDTH-1;
constant    RAD_FIFO_ADDR_LOW        : natural := 0;

-- RAMの生成
constant    SLAVE_ADDR_NUMBER    : integer := 2**(C_OFFSET_WIDTH - ADD_INC_OFFSET);
type ram_array_def is array (SLAVE_ADDR_NUMBER-1 downto 0) of std_logic_vector(C_S_AXI_DATA_WIDTH-1 downto 0);
signal ram_array : ram_array_def := (others => (others => '0'));

-- for write transaction
type write_address_state is (idle_wrad, awr_accept);
type write_data_state is (idle_wrdt, wr_burst);
type write_response_state is (idle_wres, wait_bvalid, bvalid_assert);
signal wradr_cs : write_address_state;
signal wrdat_cs : write_data_state;
signal wrres_cs : write_response_state;
signal addr_inc_step_wr : integer := 1;
signal awready         : std_logic;
signal wr_addr         : std_logic_vector(C_OFFSET_WIDTH-1 downto 0);
signal wr_bvalid     : std_logic;
signal m_seq16_wr    : std_logic_vector(15 downto 0);
signal wready        : std_logic;
type wready_state is (idle_wready, assert_wready, deassert_wready);
signal cs_wready : wready_state;
signal cdc_we : std_logic;
signal wad_fifo_full, wad_fifo_empty : std_logic;
signal wad_fifo_almost_full, wad_fifo_almost_empty : std_logic;
signal wad_fifo_rd_en : std_logic;
signal wad_fifo_wr_en : std_logic;
signal wad_fifo_din : std_logic_vector(WAD_FIFO_WIDTH-1 downto 0);
signal wad_fifo_dout : std_logic_vector(WAD_FIFO_WIDTH-1 downto 0);
signal m_seq16_wr_res    : std_logic_vector(15 downto 0);
signal wr_resp_cnt : std_logic_vector(4 downto 0);
signal wres_fifo_wr_en : std_logic;
signal wres_fifo_full, wres_fifo_empty : std_logic;
signal wres_fifo_almost_full, wres_fifo_almost_empty : std_logic;
signal wres_fifo_rd_en : std_logic;
signal wres_fifo_din : std_logic_vector(WRES_FIFO_WIDTH-1 downto 0);
signal wres_fifo_dout : std_logic_vector(WRES_FIFO_WIDTH-1 downto 0);

-- for read transaction
type read_address_state is (idle_rda, arr_accept);
type read_data_state is (idle_rdd, rd_burst);
type read_last_state is (idle_rlast, rlast_assert);
signal rdadr_cs : read_address_state;
signal rddat_cs : read_data_state;
signal rdlast : read_last_state;
signal addr_inc_step_rd : integer := 1;
signal arready         : std_logic;
signal rd_addr         : std_logic_vector(C_OFFSET_WIDTH-1 downto 0);
signal rd_axi_count    : std_logic_vector(7 downto 0);
signal rvalid        : std_logic;
signal rlast        : std_logic;
signal m_seq16_rd    : std_logic_vector(15 downto 0);
type rvalid_state is (idle_rvalid, assert_rvalid, deassert_rvalid);
signal cs_rvalid : rvalid_state;
signal read_data_count : std_logic_vector(C_S_AXI_DATA_WIDTH-1 downto 0);

signal reset_1d, reset_2d, reset : std_logic := '1';
signal rad_fifo_full, rad_fifo_empty : std_logic;
signal rad_fifo_almost_full, rad_fifo_almost_empty : std_logic;
signal rad_fifo_rd_en : std_logic;
signal rad_fifo_wr_en : std_logic;
signal rad_fifo_din : std_logic_vector(RAD_FIFO_WIDTH-1 downto 0);
signal rad_fifo_dout : std_logic_vector(RAD_FIFO_WIDTH-1 downto 0);

component sync_fifo generic (
    constant    C_MEMORY_SIZE     : integer := 512;    -- Word (not byte), 2のn乗
    constant    DATA_BUS_WIDTH    : integer := 32        -- RAM Data Width
);
 port (
    clk                : in    std_logic;
    rst                : in     std_logic;
    wr_en            : in     std_logic;
    din                : in     std_logic_vector(DATA_BUS_WIDTH-1 downto 0);
    full            : out     std_logic;
    almost_full     : out     std_logic;
    rd_en            : in     std_logic;
    dout            : out    std_logic_vector(DATA_BUS_WIDTH-1 downto 0);
    empty            : out    std_logic;
    almost_empty    : out    std_logic
);
end component;

begin
    -- ARESETN をACLK で同期化
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            reset_1d <= not ARESETN;
            reset_2d <= reset_1d;
        end if;
    end process;
    reset <= reset_2d;
    
    -- AXI4バス Write Address State Machine
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                wradr_cs <= idle_wrad;
                awready <= '0';
            else
                case (wradr_cs) is
                    when idle_wrad =>
                        if S_AXI_AWVALID='1' and wad_fifo_full='0' and wres_fifo_full='0' then -- S_AXI_AWVALID が1にアサートされた
                            wradr_cs <= awr_accept;
                            awready <= '1';
                        end if;
                    when awr_accept => -- S_AXI_AWREADY をアサート
                        wradr_cs <= idle_wrad;
                        awready <= '0';
                end case;
            end if;
        end if;
    end process;
    S_AXI_AWREADY <= not wad_fifo_full when AWREADY_IS_USUALLY_HIGH=1 else awready;
    
    -- S_AXI_AWID & S_AXI_AWBURST & S_AXI_AWSIZE & S_AXI_AWADDR　を保存しておく同期FIFO
    wad_fifo_din <= (S_AXI_AWID & S_AXI_AWBURST & S_AXI_AWSIZE & S_AXI_AWADDR);
    wad_fifo_wr_en <= (S_AXI_AWVALID and (not wad_fifo_full)) when AWREADY_IS_USUALLY_HIGH=1 else awready;

    wad_fifo : sync_fifo generic map(
        C_MEMORY_SIZE => AD_FIFO_DEPTH,
        DATA_BUS_WIDTH => WAD_FIFO_WIDTH
    ) port map (
        clk =>            ACLK,
        rst =>            reset,
        wr_en =>         wad_fifo_wr_en,
        din =>            wad_fifo_din,
        full =>            wad_fifo_full,
        almost_full =>    wad_fifo_almost_full,
        rd_en =>        wad_fifo_rd_en,
        dout =>            wad_fifo_dout,
        empty =>         wad_fifo_empty,
        almost_empty =>    wad_fifo_almost_empty
    );
    wad_fifo_rd_en <= '1' when wready='1' and S_AXI_WVALID='1' and S_AXI_WLAST='1' else '0';

    -- AXI4バス Write Data State Machine
    process (ACLK) begin
        if ACLK'event and ACLK='1' then
            if reset='1' then
                wrdat_cs <= idle_wrdt;
            else
                case( wrdat_cs ) is
                    when idle_wrdt =>
                        if wad_fifo_empty='0' then -- AXI Write アドレス転送の残りが1個以上ある
                            wrdat_cs <= wr_burst;
                        end if;
                    when wr_burst => -- Writeデータの転送
                        if S_AXI_WLAST='1' and S_AXI_WVALID='1' and wready='1' then -- Write Transaction 終了
                            wrdat_cs <= idle_wrdt;
                        end if;
                    when others =>
                
                end case ;
            end if;
        end if;
    end process;

    -- m_seq_wr、16ビットのM系列を計算する
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                m_seq16_wr <= (0 => '1', others => '0');
            else
                if WRITE_RANDOM_WAIT=1 then -- Write Transaction 時にランダムなWaitを挿入する
                    if wrdat_cs=wr_burst then
                        m_seq16_wr <= M_SEQ16_BFM_F(m_seq16_wr);
                    end if;
                else -- Wait無し
                    m_seq16_wr <= (others => '0');
                end if;
            end if;
        end if;
    end process;
                
    -- wready の処理、M系列を計算して128以上だったらWaitする。
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                cs_wready <= idle_wready;
                wready <= '0';
            else
                case (cs_wready) is
                    when idle_wready =>
                        if wrdat_cs=idle_wrdt and wad_fifo_empty='0' then -- 次はwr_burst
                            if m_seq16_wr(7)='0' and wres_fifo_full='0' then -- wready='1'
                                cs_wready <= assert_wready;
                                wready <= '1';
                            else -- m_seq16_wr(7)='1' then -- wready='0'
                                cs_wready <= deassert_wready;
                                wready <= '0';
                            end if;
                        end if;
                    when assert_wready => -- 一度wreadyがアサートされたら、1つのトランザクションが終了するまでwready='1'
                        if wrdat_cs=wr_burst and S_AXI_WLAST='1' and S_AXI_WVALID='1' then -- 終了
                            cs_wready <= idle_wready;
                            wready <= '0';
                        elsif wrdat_cs=wr_burst and S_AXI_WVALID='1' then -- 1つのトランザクション終了。
                            if m_seq16_wr(7)='1' or wres_fifo_full='1' then
                                cs_wready <= deassert_wready;
                                wready <= '0';
                            end if;
                        end if;
                    when deassert_wready =>
                        if m_seq16_wr(7)='0' and wres_fifo_full='0' then -- wready='1'
                            cs_wready <= assert_wready;
                            wready <= '1';
                        end if;
                end case;
            end if;
        end if;
    end process;
    
    S_AXI_WREADY <= wready;
    cdc_we <= '1' when wrdat_cs=wr_burst and wready='1' and S_AXI_WVALID='1' else '0';
    
    -- addr_inc_step_wr の処理
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                addr_inc_step_wr <= 1;
            else
                if wrdat_cs=idle_wrdt and wad_fifo_empty='0' then
                    case (wad_fifo_dout(WAD_FIFO_AWSIZE_HIGH downto WAD_FIFO_AWSIZE_LOW)) is
                        when "000" => -- 8ビット転送
                            addr_inc_step_wr <= 1;
                        when "001" => -- 16ビット転送
                            addr_inc_step_wr <= 2;
                        when "010" => -- 32ビット転送
                            addr_inc_step_wr <= 4;
                        when "011" => -- 64ビット転送
                            addr_inc_step_wr <= 8;
                        when "100" => -- 128ビット転送
                            addr_inc_step_wr <= 16;
                        when "101" => -- 256ビット転送
                            addr_inc_step_wr <= 32;
                        when "110" => -- 512ビット転送
                            addr_inc_step_wr <= 64;
                        when others => --"111" => -- 1024ビット転送
                            addr_inc_step_wr <= 128;
                    end case;
                end if;
            end if;
        end if;
    end process;
    
    -- wr_addr の処理
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                wr_addr <= (others => '0');
            else
                if wrdat_cs=idle_wrdt and wad_fifo_empty='0' then
                    wr_addr <= wad_fifo_dout(C_OFFSET_WIDTH-1 downto 0);
                elsif wrdat_cs=wr_burst and S_AXI_WVALID='1' and wready='1' then -- アドレスを進める
                    wr_addr <= std_logic_vector(unsigned(wr_addr) + addr_inc_step_wr);
                end if;
            end if;
        end if;
    end process;
    
    -- Wirte Response FIFO (wres_fifo)
    wres_fifo : sync_fifo generic map(
        C_MEMORY_SIZE => AD_FIFO_DEPTH,
        DATA_BUS_WIDTH => WRES_FIFO_WIDTH
    ) port map (
        clk =>            ACLK,
        rst =>            reset,
        wr_en =>         wres_fifo_wr_en,
        din =>            wres_fifo_din,
        full =>            wres_fifo_full,
        almost_full =>    wres_fifo_almost_full,
        rd_en =>        wres_fifo_rd_en,
        dout =>            wres_fifo_dout,
        empty =>         wres_fifo_empty,
        almost_empty =>    wres_fifo_almost_empty
    );
    wres_fifo_wr_en <= '1' when S_AXI_WLAST='1' and S_AXI_WVALID='1' and wready='1' else '0'; -- Write Transaction 終了
    wres_fifo_din <= (wad_fifo_dout(WAD_FIFO_AWID_HIGH downto WAD_FIFO_AWID_LOW) & wad_fifo_dout(WAD_FIFO_AWBURST_HIGH downto WAD_FIFO_AWBURST_LOW));
    wres_fifo_rd_en <= '1' when wr_bvalid='1' and S_AXI_BREADY='1' and wres_fifo_empty='0' else '0';

    -- S_AXI_BID の処理
    S_AXI_BID <= wres_fifo_dout(WRES_FIFO_AWID_HIGH downto WRES_FIFO_AWID_LOW);
    
    -- S_AXI_BRESP の処理
    -- S_AXI_AWBURSTがINCRの時はOKAYを返す。それ以外はSLVERRを返す。
    S_AXI_BRESP <= RESP_OKAY when wres_fifo_dout(WRES_FIFO_AWBURST_HIGH downto WRES_FIFO_AWBURST_LOW)=AxBURST_INCR else RESP_SLVERR;
    
    -- wr_bvalid の処理
    -- wr_bvalid のアサートは、Write Data Channelの完了より必ず1クロックは遅延する
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                wrres_cs <= idle_wres;
                wr_bvalid <= '0';
            else
                case( wrres_cs ) is
                    when idle_wres =>
                        if wres_fifo_empty='0' then -- Write Transaction 終了
                            if unsigned(m_seq16_wr_res) = 0 or RANDOM_BVALID_WAIT=0 then
                                wrres_cs <= bvalid_assert;
                                wr_bvalid <= '1';
                            else
                                wrres_cs <= wait_bvalid;
                            end if;
                        end if;
                    when wait_bvalid =>
                        if unsigned(wr_resp_cnt) = 0 then
                            wrres_cs <= bvalid_assert;
                            wr_bvalid <= '1';
                        end if;
                    when bvalid_assert =>
                        if (S_AXI_BREADY='1') then
                            wrres_cs <= idle_wres;
                            wr_bvalid <= '0';
                        end if;
                    when others =>
                
                end case ;
            end if;
        end if;
    end process;
    S_AXI_BVALID <= wr_bvalid;
    S_AXI_BUSER <= (others => '0');

    -- wr_resp_cnt
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                wr_resp_cnt <= (others => '0');
            else
                if wrres_cs=idle_wres and wres_fifo_empty='0' then
                    wr_resp_cnt <= m_seq16_wr_res(4 downto 0);
                elsif unsigned(wr_resp_cnt) /= 0 then
                    wr_resp_cnt <= std_logic_vector(unsigned(wr_resp_cnt) - 1);
                end if;
            end if;
        end if;
    end process;

    -- m_seq_wr_res、16ビットのM系列を計算する
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                m_seq16_wr_res <= (0 => '1', others => '0');
            else
                m_seq16_wr_res <= M_SEQ16_BFM_F(m_seq16_wr_res);
            end if;
        end if;
    end process;
    
    
    -- AXI4バス Read Address Transaction State Machine
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                rdadr_cs <= idle_rda;
                arready <= '0';
            else
                case (rdadr_cs) is
                    when idle_rda =>
                        if S_AXI_ARVALID='1' and rad_fifo_full='0' then -- Read Transaction 要求
                            rdadr_cs <= arr_accept;
                            arready <= '1';
                        end if;
                    when arr_accept => -- S_AXI_ARREADY をアサート
                        rdadr_cs <= idle_rda;
                        arready <= '0';
                end case;
            end if;
        end if;
    end process;
    S_AXI_ARREADY <= not rad_fifo_full when ARREADY_IS_USUALLY_HIGH=1 else arready;

    -- S_AXI_ARID & S_AXI_ARBURST & S_AXI_ARSIZE & S_AXI_ARLEN & S_AXI_ARADDR を保存しておく同期FIFO
    rad_fifo_din <= (S_AXI_ARID & S_AXI_ARBURST & S_AXI_ARSIZE & S_AXI_ARLEN & S_AXI_ARADDR);
    rad_fifo_wr_en <= (S_AXI_ARVALID and (not rad_fifo_full)) when ARREADY_IS_USUALLY_HIGH=1 else arready;

    rad_fifo : sync_fifo generic map (
        C_MEMORY_SIZE =>    AD_FIFO_DEPTH,
        DATA_BUS_WIDTH =>    RAD_FIFO_WIDTH
    ) port map (
        clk =>            ACLK,
        rst =>            reset,
        wr_en =>        rad_fifo_wr_en,
        din =>             rad_fifo_din,
        full =>            rad_fifo_full,
        almost_full =>    rad_fifo_almost_full,
        rd_en =>        rad_fifo_rd_en,
        dout =>            rad_fifo_dout,
        empty =>        rad_fifo_empty,
        almost_empty =>    rad_fifo_almost_empty
    );
    rad_fifo_rd_en <= '1' when rvalid='1' and S_AXI_RREADY='1' and rlast='1' else '0';

    -- AXI4バス Read Data Transaction State Machine
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                rddat_cs <= idle_rdd;
            else
                case (rddat_cs) is
                    when idle_rdd =>
                        if rad_fifo_empty='0' then -- AXI Read アドレス転送の残りが1個以上ある
                            rddat_cs <= rd_burst;
                        end if;
                    when rd_burst =>
                        if unsigned(rd_axi_count)=0 and rvalid='1' and S_AXI_RREADY='1' then -- Read Transaction 終了
                            rddat_cs <= idle_rdd;
                        end if;
                end case;
            end if;
        end if;
    end process;

    -- m_seq_rd、16ビットのM系列を計算する
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                m_seq16_rd <= (others => '1'); -- Writeとシードを変更する
            else
                if READ_RANDOM_WAIT=1 then -- Read Transaciton のデータ転送でランダムなWaitを挿入する場合
                    if rddat_cs=rd_burst then
                        m_seq16_rd <= M_SEQ16_BFM_F(m_seq16_rd);
                    end if;
                else -- Wati無し
                    m_seq16_rd <= (others => '0');
                end if;
            end if;
        end if;
    end process;
                
    -- rvalid の処理、M系列を計算して128以上だったらWaitする。
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                cs_rvalid <= idle_rvalid;
                rvalid <= '0';
            else
                case (cs_rvalid) is
                    when idle_rvalid =>
                        if rddat_cs=idle_rdd and rad_fifo_empty='0' then -- 次はrd_burst
                            if m_seq16_rd(7)='0' then -- rvalid='1'
                                cs_rvalid <= assert_rvalid;
                                rvalid <= '1';
                            else -- m_seq16_rd(7)='1' then -- rvalid='0'
                                cs_rvalid <= deassert_rvalid;
                                rvalid <= '0';
                            end if;
                        end if;
                    when assert_rvalid => -- 一度rvalidがアサートされたら、1つのトランザクションが終了するまでrvalid='1'
                        if rddat_cs=rd_burst and rlast='1' and S_AXI_RREADY='1' then -- 終了
                            cs_rvalid <= idle_rvalid;
                            rvalid <= '0';
                        elsif rddat_cs=rd_burst and S_AXI_RREADY='1' then -- 1つのトランザクション終了。
                            if m_seq16_rd(7)='1' then
                                cs_rvalid <= deassert_rvalid;
                                rvalid <= '0';
                            end if;
                        end if;
                    when deassert_rvalid =>
                        if m_seq16_rd(7)='0' then -- rvalid='1'
                            cs_rvalid <= assert_rvalid;
                            rvalid <= '1';
                        end if;
                end case;
            end if;
        end if;
    end process;
    
    S_AXI_RVALID <= rvalid;
    
    -- addr_inc_step_rd の処理
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                addr_inc_step_rd <= 1;
            else
                if rddat_cs=idle_rdd and rad_fifo_empty='0' then
                    case (rad_fifo_dout(RAD_FIFO_ARSIZE_HIGH downto RAD_FIFO_ARSIZE_LOW)) is
                        when "000" => -- 8ビット転送
                            addr_inc_step_rd <= 1;
                        when "001" => -- 16ビット転送
                            addr_inc_step_rd <= 2;
                        when "010" => -- 32ビット転送
                            addr_inc_step_rd <= 4;
                        when "011" => -- 64ビット転送
                            addr_inc_step_rd <= 8;
                        when "100" => -- 128ビット転送
                            addr_inc_step_rd <= 16;
                        when "101" => -- 256ビット転送
                            addr_inc_step_rd <= 32;
                        when "110" => -- 512ビット転送
                            addr_inc_step_rd <= 64;
                        when others => -- "111" => -- 1024ビット転送
                            addr_inc_step_rd <= 128;
                    end case;
                end if;
            end if;
        end if;
    end process;
    
    -- rd_addr の処理
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                rd_addr <= (others => '0');
            else
                if rddat_cs=idle_rdd and rad_fifo_empty='0' then
                    rd_addr <= rad_fifo_dout(C_OFFSET_WIDTH-1 downto 0);
                elsif rddat_cs=rd_burst and S_AXI_RREADY='1' and rvalid='1' then
                    rd_addr <= std_logic_vector(unsigned(rd_addr) + addr_inc_step_rd);
                end if;
            end if;
        end if;
    end process;
    
    -- rd_axi_count の処理（AXIバス側のデータカウント）
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                rd_axi_count <= (others => '0');
            else
                if rddat_cs=idle_rdd and rad_fifo_empty='0' then -- rd_axi_count のロード
                    rd_axi_count <= rad_fifo_dout(RAD_FIFO_ARLEN_HIGH downto RAD_FIFO_ARLEN_LOW);
                elsif rddat_cs=rd_burst and rvalid='1' and S_AXI_RREADY='1' then -- Read Transaction が1つ終了
                    rd_axi_count <= std_logic_vector(unsigned(rd_axi_count) - 1);
                end if;
            end if;
        end if;
    end process;
    
    -- rdlast State Machine
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                rdlast <= idle_rlast;
                rlast <= '0';
            else
                case (rdlast) is
                    when idle_rlast =>
                        if unsigned(rd_axi_count)=1 and rvalid='1' and S_AXI_RREADY='1' then -- バーストする場合
                            rdlast <= rlast_assert;
                            rlast <= '1';
                        elsif rddat_cs=idle_rdd and rad_fifo_empty='0' and unsigned(rad_fifo_dout(RAD_FIFO_ARLEN_HIGH downto RAD_FIFO_ARLEN_LOW))=0 then -- 転送数が1の場合
                            rdlast <= rlast_assert;
                            rlast <= '1';
                        end if;
                    when rlast_assert => 
                        if rvalid='1' and S_AXI_RREADY='1' then -- Read Transaction 終了（rd_axi_count=0は決定）
                            rdlast <= idle_rlast;
                            rlast <= '0';
                        end if;
                end case;
            end if;
        end if;
    end process;
    S_AXI_RLAST <= rlast;
    
    -- S_AXI_RID, S_AXI_RUSER の処理
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                S_AXI_RID <= (others => '0');
            else
                if rddat_cs=idle_rdd and rad_fifo_empty='0' then
                    S_AXI_RID <= rad_fifo_dout(RAD_FIFO_ARID_HIGH downto RAD_FIFO_ARID_LOW);
                end if;
            end if;
        end if;
    end process;
    S_AXI_RUSER <= (others => '0');
    
    -- S_AXI_RRESP は、S_AXI_ARBURST がINCR の場合はOKAYを返す。それ以外はSLVERRを返す。
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                S_AXI_RRESP <= (others => '0');
            else
                if rddat_cs=idle_rdd and rad_fifo_empty='0' then
                    if rad_fifo_dout(RAD_FIFO_ARBURST_HIGH downto RAD_FIFO_ARBURST_LOW)=AxBURST_INCR then
                        S_AXI_RRESP <= RESP_OKAY;
                    else
                        S_AXI_RRESP <= RESP_SLVERR;
                    end if;
                end if;
            end if;
        end if;
    end process;
    
    -- RAM
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if cdc_we='1' then
                for i in 0 to C_S_AXI_DATA_WIDTH/8-1 loop
                    if S_AXI_WSTRB(i)='1' then -- Byte Enable
                        ram_array(TO_INTEGER(unsigned(wr_addr(C_OFFSET_WIDTH-1 downto ADD_INC_OFFSET))))(i*8+7 downto i*8) <= S_AXI_WDATA(i*8+7 downto i*8);
                    end if;
                end loop;
            end if;
        end if;
    end process;

    -- Read Transaciton の時に +1 されたReadデータを使用する（Read 毎に+1）
    process (ACLK) begin
        if ACLK'event and ACLK='1' then 
            if reset='1' then
                read_data_count <= (others => '0');
            else
                if rddat_cs=rd_burst and rvalid='1' and S_AXI_RREADY='1' then -- Read Transaction が1つ終了
                    read_data_count <= std_logic_vector(unsigned(read_data_count) + 1);
                end if;
            end if;
        end if;
    end process;
    
    S_AXI_RDATA <= ram_array(TO_INTEGER(unsigned(rd_addr(C_OFFSET_WIDTH-1 downto ADD_INC_OFFSET)))) when READ_DATA_IS_INCREMENT=0 else read_data_count;
    
end implementation;

sync_fifo.vhd を示す。
（追加）このHDLコードは無保証です。このコードを使用したことによる損害の保証はいたしません。ホビー向けとします。お仕事で使われるなど、信頼性が必要な用途には、Xilinx社で販売している売り物のBFMをご使用下さい。

-- Synchronous FIFO for Simulation
--
-- 2014/07/03 by marsee
--
-- ライセンスは二条項BSDライセンス (2-clause BSD license)とします。
--

library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use ieee.std_logic_misc.all;
use IEEE.math_real.all;

entity sync_fifo is 
    generic (
        C_MEMORY_SIZE    : integer := 512;    -- Word (not byte), 2のn乗
        DATA_BUS_WIDTH    : integer := 32        -- RAM Data Width
    );
    port (
        clk                : in    std_logic;
        rst                : in    std_logic;
        wr_en            : in    std_logic;
        din                : in    std_logic_vector(DATA_BUS_WIDTH-1 downto 0);
        full            : out    std_logic;
        almost_full        : out    std_logic;
        rd_en            : in    std_logic;
        dout            : out    std_logic_vector(DATA_BUS_WIDTH-1 downto 0);
        empty            : out     std_logic;
        almost_empty    : out    std_logic
    );
end sync_fifo;

architecture RTL of sync_fifo is

constant C_MEMORY_LENGTH : natural := natural(ceil(log(real(C_MEMORY_SIZE), 2.0))); -- C_MEMORY_SIZEの2進数の桁数

type mem_type is array (0 to C_MEMORY_SIZE-1) of std_logic_vector(DATA_BUS_WIDTH-1 downto 0);
signal mem             : mem_type := (OTHERS => (OTHERS => '0'));
signal mem_waddr    : std_logic_vector(C_MEMORY_LENGTH-1 downto 0) := (others => '0');
signal mem_raddr    : std_logic_vector(C_MEMORY_LENGTH-1 downto 0) := (others => '0');
signal rp            : std_logic_vector(C_MEMORY_LENGTH-1 downto 0) := (others => '0');
signal wp            : std_logic_vector(C_MEMORY_LENGTH-1 downto 0) := (others => '0');

signal almost_full_node        : std_logic;
signal almost_empty_node    : std_logic;
signal full_node                : std_logic;
signal empty_node                : std_logic;
begin
    -- Write
    process (clk) begin
        if clk'event and clk='1' then
            if rst='1' then
                mem_waddr <= (others => '0');
                wp <= (others => '0');
            else
                if wr_en='1' then
                    mem_waddr <= std_logic_vector(unsigned(mem_waddr) + 1);
                    wp <= std_logic_vector(unsigned(wp) + 1);
                end if;
            end if;
        end if;
    end process;

    process (clk) begin
        if clk'event and clk='1' then
            if wr_en='1' then
                mem(TO_INTEGER(unsigned(mem_waddr))) <= din;
            end if;
        end if;
    end process;

    full_node <= '1' when std_logic_vector(unsigned(wp)+1)=rp else '0';
    full <= full_node;
    almost_full_node <= '1' when std_logic_vector(unsigned(wp)+2)=rp else '0';
    almost_full <= full_node or almost_full_node;

    -- Read
    process (clk) begin
        if clk'event and clk='1' then
            if rst='1' then
                mem_raddr <= (others => '0');
                rp <= (others => '0');
            else
                if rd_en='1' then
                    mem_raddr <= std_logic_vector(unsigned(mem_raddr) + 1);
                    rp <= std_logic_vector(unsigned(rp) + 1);
                end if;
            end if;
        end if;
    end process;

    dout <= mem(TO_INTEGER(unsigned(mem_raddr)));

    empty_node <= '1' when wp=rp else '0';
    empty <= empty_node;
    almost_empty_node <= '1' when wp=std_logic_vector(unsigned(rp)+1) else '0';
    almost_empty <= empty_node or almost_empty_node;
end RTL;

AXI4 Slave Bus Functional Model のVerilog HDL版4

”AXI4 Slave Bus Functional Model のVerilog HDL版２”の続き。

AXI4 Slave のBFM (axi_slave_BFM.v) を公開する。
今回の修正は、Xilinx社のIP はAｘREADY がいつもは 1 になっていることから、AｘREADYがいつも 1 になるように変更した。これがデフォルトのモードで、AWREADY_IS_USUALLY_HIGH、ARREADY_IS_USUALLY_HIGH を 0 にすることで、従来のAxREADY がいつもは 0 のモードにすることができる。

下にaxi_slave_BFM.v をテストするプロジェクトを示す。


AWREADY_IS_USUALLY_HIGH=1, ARREADY_IS_USUALLY_HIGH=1 の時のシミュレーション波形を示す。


AWREADY と ARREADY が 1 固定なのがわかる。

AWREADY_IS_USUALLY_HIGH=1, ARREADY_IS_USUALLY_HIGH=1 の時のシミュレーション波形を示す。


AWREADY と ARREADY がいつもは 0 で、AWVALID、ARVALID が 1 になった時だけ、1 にアサートされるのがわかる。

axi_slave_BFM.v を貼っておく。
（追加）このHDLコードは無保証です。このコードを使用したことによる損害の保証はいたしません。ホビー向けとします。お仕事で使われるなど、信頼性が必要な用途には、Xilinx社で販売している売り物のBFMをご使用下さい。

/* AXI Master用 Slave Bus Function Mode (BFM)
   axi_slave_BFM.v

   2012/02/25 : S_AXI_AWBURST＝1 (INCR) にのみ対応、AWSIZE, ARSIZE = 000 (1byte), 001 (2bytes), 010 (4bytes) のみ対応。
   2012/07/04 : READ_ONLY_TRANSACTION を追加。Read機能のみでも+1したデータを出力することが出来るように変更した。
   2014/01/05 : ポート名をM_AXI?からS_AXI?に修正、Verilogに移植（By Koba）
*/
// 2014/07/18 : Write Respose Channel に sync_fifo を使用した by marsee
// 2014/08/31 : READ_RANDOM_WAIT=1 の時に、S_AXI_RREADY が S_AXI_RVALID に依存するバグをフィック。 by marsee
//              WRITE_RANDOM_WAIT=1 の時に、S_AXI_WVALID が S_AXI_WREADY に依存するバグをフィック。 by marsee
//
// 2016/07/03 : AWREADY_IS_USUALLY_HIGH　と ARREADY_IS_USUALLY_HIGH　の2つのパラメータを追加 by marsee
//
// ライセンスは二条項BSDライセンス (2-clause BSD license)とします。
//


module axi_slave_bfm #(
    parameter integer C_S_AXI_ID_WIDTH       = 1,
    parameter integer C_S_AXI_ADDR_WIDTH     = 32,
    parameter integer C_S_AXI_DATA_WIDTH     = 32,
    parameter integer C_S_AXI_AWUSER_WIDTH   = 1,
    parameter integer C_S_AXI_ARUSER_WIDTH   = 1,
    parameter integer C_S_AXI_WUSER_WIDTH    = 1,
    parameter integer C_S_AXI_RUSER_WIDTH    = 1,
    parameter integer C_S_AXI_BUSER_WIDTH    = 1,

    parameter integer C_S_AXI_TARGET         = 0,
    parameter integer C_OFFSET_WIDTH         = 10, // 割り当てるRAMのアドレスのビット幅
    parameter integer C_S_AXI_BURST_LEN      = 256,

    parameter integer WRITE_RANDOM_WAIT      = 1, // Write Transactionデータ転送時にランダムなWaitを発生させる=1、Waitしない=0
    parameter integer READ_RANDOM_WAIT       = 0, // Read Transactionデータ転送時にランダムなWaitを発生させる=1、Waitしない=0
    parameter integer READ_DATA_IS_INCREMENT = 0, // Read TransactionでRAMのデータを読み出す=0、0はじまりの+1データを使う=1
    parameter integer RANDOM_BVALID_WAIT     = 0,  // Write Transaction後、BVALIDをランダムにWaitする=1、ランダムにWaitしない=0
    parameter integer AWREADY_IS_USUALLY_HIGH = 1, // AWRAEDY は通常はLow=0, High=1
    parameter integer ARREADY_IS_USUALLY_HIGH = 1 // AWRAEDY は通常はLow=0, High=1
)
(
    // System Signals
    input ACLK,
    input ARESETN,

    // Slave Interface Write Address Ports
    input   [C_S_AXI_ID_WIDTH-1 : 0]    S_AXI_AWID,
    input   [C_S_AXI_ADDR_WIDTH-1 : 0]  S_AXI_AWADDR,
    input   [8-1 : 0]                   S_AXI_AWLEN,
    input   [3-1 : 0]                   S_AXI_AWSIZE,
    input   [2-1 : 0]                   S_AXI_AWBURST,
    // input S_AXI_AWLOCK [2-1 : 0],
    input   [1 : 0]                     S_AXI_AWLOCK,
    input   [4-1 : 0]                   S_AXI_AWCACHE,
    input   [3-1 : 0]                   S_AXI_AWPROT,
    input   [4-1 : 0]                   S_AXI_AWQOS,
    input   [C_S_AXI_AWUSER_WIDTH-1 :0] S_AXI_AWUSER,
    input                               S_AXI_AWVALID,
    output                              S_AXI_AWREADY,

    // Slave Interface Write Data Ports
    input   [C_S_AXI_DATA_WIDTH-1 : 0]  S_AXI_WDATA,
    input   [C_S_AXI_DATA_WIDTH/8-1 : 0]S_AXI_WSTRB,
    input                               S_AXI_WLAST,
    input   [C_S_AXI_WUSER_WIDTH-1 : 0] S_AXI_WUSER,
    input                               S_AXI_WVALID,
    output                              S_AXI_WREADY,

    // Slave Interface Write Response Ports
    output  [C_S_AXI_ID_WIDTH-1 : 0]    S_AXI_BID,
    output  [2-1 : 0]                   S_AXI_BRESP,
    output  [C_S_AXI_BUSER_WIDTH-1 : 0] S_AXI_BUSER,
    output                              S_AXI_BVALID,
    input                               S_AXI_BREADY,

    // Slave Interface Read Address Ports
    input   [C_S_AXI_ID_WIDTH-1 : 0]    S_AXI_ARID,
    input   [C_S_AXI_ADDR_WIDTH-1 : 0]  S_AXI_ARADDR,
    input   [8-1 : 0]                   S_AXI_ARLEN,
    input   [3-1 : 0]                   S_AXI_ARSIZE,
    input   [2-1 : 0]                   S_AXI_ARBURST,
    input   [2-1 : 0]                   S_AXI_ARLOCK,
    input   [4-1 : 0]                   S_AXI_ARCACHE,
    input   [3-1 : 0]                   S_AXI_ARPROT,
    input   [4-1 : 0]                   S_AXI_ARQOS,
    input   [C_S_AXI_ARUSER_WIDTH-1 : 0]S_AXI_ARUSER,
    input                               S_AXI_ARVALID,
    output                              S_AXI_ARREADY,

    // Slave Interface Read Data Ports
    output  reg [C_S_AXI_ID_WIDTH-1: 0] S_AXI_RID,
    output  [C_S_AXI_DATA_WIDTH-1 : 0]  S_AXI_RDATA,
    output  reg [2-1 : 0]               S_AXI_RRESP,
    output                              S_AXI_RLAST,
    output  [C_S_AXI_RUSER_WIDTH-1 : 0] S_AXI_RUSER,
    output                              S_AXI_RVALID,
    input                               S_AXI_RREADY
);

localparam AXBURST_FIXED = 2'b00;
localparam AXBURST_INCR = 2'b01;
localparam AXBURST_WRAP = 2'b10;
localparam RESP_OKAY = 2'b00;
localparam RESP_EXOKAY = 2'b01;
localparam RESP_SLVERR = 2'b10;
localparam RESP_DECERR = 2'b11;
localparam DATA_BUS_BYTES = (C_S_AXI_DATA_WIDTH / 8);
//localparam ADD_INC_OFFSET = log2(DATA_BUS_BYTES);
localparam ADD_INC_OFFSET = (DATA_BUS_BYTES==1) ? 0:
                            (DATA_BUS_BYTES==2) ? 1:
                            (DATA_BUS_BYTES==4) ? 2:
                            (DATA_BUS_BYTES==8) ? 3:
                            (DATA_BUS_BYTES==16) ? 4:
                            (DATA_BUS_BYTES==32) ? 5:
                            (DATA_BUS_BYTES==64) ? 6:
                            (DATA_BUS_BYTES==128) ? 7: 32'hxxxxxxxx;

// fifo depth for address
localparam AD_FIFO_DEPTH         = 16;

// wad_fifo field
localparam WAD_FIFO_WIDTH        = C_S_AXI_ADDR_WIDTH+5+C_S_AXI_ID_WIDTH-1+1;
localparam WAD_FIFO_AWID_HIGH    = C_S_AXI_ADDR_WIDTH+5+C_S_AXI_ID_WIDTH-1;
localparam WAD_FIFO_AWID_LOW     = C_S_AXI_ADDR_WIDTH+5;
localparam WAD_FIFO_AWBURST_HIGH = C_S_AXI_ADDR_WIDTH+4;
localparam WAD_FIFO_AWBURST_LOW  = C_S_AXI_ADDR_WIDTH+3;
localparam WAD_FIFO_AWSIZE_HIGH  = C_S_AXI_ADDR_WIDTH+2;
localparam WAD_FIFO_AWSIZE_LOW   = C_S_AXI_ADDR_WIDTH;
localparam WAD_FIFO_ADDR_HIGH    = C_S_AXI_ADDR_WIDTH-1;
localparam WAD_FIFO_ADDR_LOW     = 0;

// wres_fifo field
localparam WRES_FIFO_WIDTH          = 2+C_S_AXI_ID_WIDTH-1+1;
localparam WRES_FIFO_AWID_HIGH      = 2+C_S_AXI_ID_WIDTH-1;
localparam WRES_FIFO_AWID_LOW       = 2;
localparam WRES_FIFO_AWBURST_HIGH   = 1;
localparam WRES_FIFO_AWBURST_LOW    = 0;

// rad_fifo field
localparam RAD_FIFO_WIDTH        = C_S_AXI_ADDR_WIDTH+13+C_S_AXI_ID_WIDTH-1+1;
localparam RAD_FIFO_ARID_HIGH    = C_S_AXI_ADDR_WIDTH+13+C_S_AXI_ID_WIDTH-1;
localparam RAD_FIFO_ARID_LOW     = C_S_AXI_ADDR_WIDTH+13;
localparam RAD_FIFO_ARBURST_HIGH = C_S_AXI_ADDR_WIDTH+12;
localparam RAD_FIFO_ARBURST_LOW  = C_S_AXI_ADDR_WIDTH+11;
localparam RAD_FIFO_ARSIZE_HIGH  = C_S_AXI_ADDR_WIDTH+10;
localparam RAD_FIFO_ARSIZE_LOW   = C_S_AXI_ADDR_WIDTH+8;
localparam RAD_FIFO_ARLEN_HIGH   = C_S_AXI_ADDR_WIDTH+7;
localparam RAD_FIFO_ARLEN_LOW    = C_S_AXI_ADDR_WIDTH;
localparam RAD_FIFO_ADDR_HIGH    = C_S_AXI_ADDR_WIDTH-1;
localparam RAD_FIFO_ADDR_LOW     = 0;

// RAMの生成
localparam SLAVE_ADDR_NUMBER = 2 ** (C_OFFSET_WIDTH - ADD_INC_OFFSET);
reg [(C_S_AXI_DATA_WIDTH - 1):0] ram_array [(SLAVE_ADDR_NUMBER - 1):0];

// for write transaction
// write_address_state
localparam IDLE_WRAD  = 1'd0;
localparam AWR_ACCEPT = 1'd1;
reg wradr_cs;

// write_data_state
localparam IDLE_WRDT = 1'd0;
localparam WR_BURST  = 1'd1;
reg wrdat_cs;

// write_response_state
localparam IDLE_WRES     = 2'd0;
localparam WAIT_BVALID   = 2'd1;
localparam BVALID_ASSERT = 2'd2;
reg [1:0] wrres_cs;

integer addr_inc_step_wr = 1;
reg awready;
reg [(C_OFFSET_WIDTH - 1):0]   wr_addr;
reg [(C_S_AXI_ID_WIDTH - 1):0] wr_bid;
reg [1:0] wr_bresp;
reg wr_bvalid;
reg [15:0] m_seq16_wr;
reg wready;

// wready_state
localparam IDLE_WREADY     = 2'd0;
localparam ASSERT_WREADY   = 2'd1;
localparam DEASSERT_WREADY = 2'd2;
reg [1:0] cs_wready;

wire cdc_we;
wire wad_fifo_full;
wire wad_fifo_empty;
wire wad_fifo_almost_full;
wire wad_fifo_almost_empty;
wire wad_fifo_rd_en;
wire wad_fifo_wr_en;
wire [WAD_FIFO_WIDTH-1:0] wad_fifo_din;
wire [WAD_FIFO_WIDTH-1:0] wad_fifo_dout;
reg  [15:0] m_seq16_wr_res;
reg  [4:0]  wr_resp_cnt;

// wres_fifo
wire wres_fifo_wr_en;
wire wres_fifo_full;
wire wres_fifo_empty;
wire wres_fifo_almost_full;
wire wres_fifo_almost_empty;
wire wres_fifo_rd_en;
wire [WRES_FIFO_WIDTH-1:0] wres_fifo_din;
wire [WRES_FIFO_WIDTH-1:0] wres_fifo_dout;

// for read transaction
// read_address_state
localparam IDLE_RDA   = 1'd0;
localparam ARR_ACCEPT = 1'd1;
reg rdadr_cs;

// read_data_state
localparam IDLE_RDD = 1'd0;
localparam RD_BURST = 1'd1;
reg rddat_cs;

// read_last_state
localparam IDLE_RLAST   = 1'd0;
localparam RLAST_ASSERT = 1'd1;
reg rdlast;

integer addr_inc_step_rd = 1;
reg arready;
reg [(C_OFFSET_WIDTH - 1):0] rd_addr;
reg [7:0] rd_axi_count;
reg rvalid;
reg rlast;
reg [15:0] m_seq16_rd;

// rvalid_state
localparam IDLE_RVALID     = 2'd0;
localparam ASSERT_RVALID   = 2'd1;
localparam DEASSERT_RVALID = 2'd2;
reg [1:0] cs_rvalid;

reg [(C_S_AXI_DATA_WIDTH - 1):0] read_data_count;
reg reset_1d;
reg reset_2d;
wire reset;
wire rad_fifo_full;
wire rad_fifo_empty;
wire rad_fifo_almost_full;
wire rad_fifo_almost_empty;
wire rad_fifo_wr_en;
wire rad_fifo_rd_en;
wire [RAD_FIFO_WIDTH-1:0] rad_fifo_din;
wire [RAD_FIFO_WIDTH-1:0] rad_fifo_dout;


// ARESETN をACLK で同期化
always @ ( posedge ACLK ) begin
    reset_1d <= ~ARESETN;
    reset_2d <= reset_1d;
end

assign reset = reset_2d;


// AXI4バス Write Address State Machine
always @ ( posedge ACLK ) begin
    if ( reset ) begin
        wradr_cs <= IDLE_WRAD;
        awready  <= 1'b0;
    end
    else
        case (wradr_cs)
            IDLE_WRAD:  if ((S_AXI_AWVALID == 1'b1) && (wad_fifo_full == 1'b0) && (wres_fifo_full == 1'b0))    // S_AXI_AWVALIDが1にアサートされた
                        begin
                            wradr_cs <= AWR_ACCEPT;
                            awready <= 1'b1;
                        end
            AWR_ACCEPT: begin
                            wradr_cs <= IDLE_WRAD;
                            awready <= 1'b0;
                        end
        endcase
end

assign S_AXI_AWREADY = (AWREADY_IS_USUALLY_HIGH==1) ? ~wad_fifo_full : awready;


// {S_AXI_AWID, S_AXI_AWBURST, S_AXI_AWSIZE, S_AXI_AWADDR}を保存しておく同期FIFO
assign wad_fifo_din = {S_AXI_AWID, S_AXI_AWBURST, S_AXI_AWSIZE, S_AXI_AWADDR};
assign wad_fifo_wr_en = (AWREADY_IS_USUALLY_HIGH==1) ? (S_AXI_AWVALID & ~wad_fifo_full) : awready;

sync_fifo  #(
    .C_MEMORY_SIZE  (AD_FIFO_DEPTH),
    .DATA_BUS_WIDTH (WAD_FIFO_WIDTH)
  ) wad_fifo (
    .clk            (ACLK),
    .rst            (reset),
    .wr_en          (wad_fifo_wr_en),
    .din            (wad_fifo_din),
    .full           (wad_fifo_full),
    .almost_full    (wad_fifo_almost_full),
    .rd_en          (wad_fifo_rd_en),
    .dout           (wad_fifo_dout),
    .empty          (wad_fifo_empty),
    .almost_empty   (wad_fifo_almost_empty)
);

assign wad_fifo_rd_en = (wready & S_AXI_WVALID & S_AXI_WLAST);


// AXI4バス Write Data State Machine
always @( posedge ACLK ) begin
    if ( reset )
        wrdat_cs <= IDLE_WRDT;
    else
        case (wrdat_cs)
            IDLE_WRDT:  if ( wad_fifo_empty == 1'b0 )   // AXI Writeアドレス転送の残りが1個以上ある
                            wrdat_cs <= WR_BURST;
            WR_BURST :  if ( S_AXI_WLAST & S_AXI_WVALID & wready )  // Write Transaction終了
                            wrdat_cs <= IDLE_WRDT;
        endcase
end

// M系列による16ビット乱数生成関数
function [15:0] M_SEQ16_BFM_F;
input [15:0] mseq16in;
reg   xor_result;
begin
    xor_result = mseq16in[15] ^ mseq16in[12] ^ mseq16in[10] ^ mseq16in[8] ^
                 mseq16in[7]  ^ mseq16in[6]  ^ mseq16in[3]  ^ mseq16in[2];
    M_SEQ16_BFM_F = {mseq16in[14:0], xor_result};
end
endfunction


// m_seq_wr、16ビットのM系列を計算する
always @( posedge ACLK ) begin
    if ( reset )
        m_seq16_wr <= 16'b1;
    else begin
        if ( WRITE_RANDOM_WAIT ) begin // Write Transaction時にランダムなWaitを挿入する
            if ( wrdat_cs == WR_BURST )
                m_seq16_wr <= M_SEQ16_BFM_F(m_seq16_wr);
        end
        else    // Wait無し
            m_seq16_wr <= 16'b0;
    end
end


// wready の処理、M系列を計算して128以上だったらWaitする。
always @( posedge ACLK ) begin
    if ( reset ) begin
        cs_wready <= IDLE_WREADY;
        wready    <= 1'b0;
    end
    else
        case (cs_wready)
            IDLE_WREADY:    if ( (wrdat_cs == IDLE_WRDT) && (wad_fifo_empty == 1'b0) ) begin
                                if ( (m_seq16_wr[7] == 1'b0) && (wres_fifo_full==1'b0) ) begin
                                    cs_wready <= ASSERT_WREADY;
                                    wready    <= 1'b1;
                                end
                                else begin
                                    cs_wready <= DEASSERT_WREADY;
                                    wready    <= 1'b0;
                                end
                            end
            ASSERT_WREADY:  if ( (wrdat_cs == WR_BURST) && S_AXI_WLAST && S_AXI_WVALID ) begin
                                cs_wready <= IDLE_WREADY;
                                wready <= 1'b0;
                            end
                            else if ( (wrdat_cs == WR_BURST) && S_AXI_WVALID ) begin
                                if ((m_seq16_wr[7] == 1'b1) || (wres_fifo_full==1'b1)) begin
                                    cs_wready <= DEASSERT_WREADY;
                                    wready <= 1'b0;
                                end
                            end
            DEASSERT_WREADY:if ( (m_seq16_wr[7] == 1'b0) && (wres_fifo_full==1'b0) ) begin
                                cs_wready <= ASSERT_WREADY;
                                wready <= 1'b1;
                            end
        endcase
end

assign S_AXI_WREADY = wready;
assign cdc_we = ( (wrdat_cs == WR_BURST) && wready && S_AXI_WVALID );


// addr_inc_step_wrの処理
always @ ( posedge ACLK ) begin
    if ( reset )
        addr_inc_step_wr <= 1;
    else begin
        if ( (wrdat_cs == IDLE_WRDT) & (wad_fifo_empty == 1'b0) )
            case (wad_fifo_dout[WAD_FIFO_AWSIZE_HIGH:WAD_FIFO_AWSIZE_LOW])
                3'b000 : addr_inc_step_wr <=   1;   //    8ビット転送
                3'b001 : addr_inc_step_wr <=   2;   //   16ビット転送
                3'b010 : addr_inc_step_wr <=   4;   //   32ビット転送
                3'b011 : addr_inc_step_wr <=   8;   //   64ビット転送
                3'b100 : addr_inc_step_wr <=  16;   //  128ビット転送
                3'b101 : addr_inc_step_wr <=  32;   //  256ビット転送
                3'b110 : addr_inc_step_wr <=  64;   //  512ビット転送
                default: addr_inc_step_wr <= 128;   // 1024ビット転送
            endcase
    end
end

// wr_addr の処理
always @ (posedge ACLK ) begin
    if ( reset )
        wr_addr <= 'b0;
    else begin
        if ( (wrdat_cs == IDLE_WRDT) && (wad_fifo_empty == 1'b0) )
            wr_addr <= wad_fifo_dout[(C_OFFSET_WIDTH - 1):0];
        else if ( (wrdat_cs == WR_BURST) && S_AXI_WVALID && wready )    // アドレスを進める
            wr_addr <= (wr_addr + addr_inc_step_wr);
    end
end

// Wirte Response FIFO (wres_fifo)
sync_fifo #(
    .C_MEMORY_SIZE(AD_FIFO_DEPTH),
    .DATA_BUS_WIDTH(WRES_FIFO_WIDTH)
) wres_fifo (
    .clk(ACLK),
    .rst(reset),
    .wr_en(wres_fifo_wr_en),
    .din(wres_fifo_din),
    .full(wres_fifo_full),
    .almost_full(wres_fifo_almost_full),
    .rd_en(wres_fifo_rd_en),
    .dout(wres_fifo_dout),
    .empty(wres_fifo_empty),
    .almost_empty(wres_fifo_almost_empty)
);
assign wres_fifo_wr_en = (S_AXI_WLAST & S_AXI_WVALID & wready) ? 1'b1 : 1'b0;   // Write Transaction 終了
assign wres_fifo_din = {wad_fifo_dout[WAD_FIFO_AWID_HIGH:WAD_FIFO_AWID_LOW], wad_fifo_dout[WAD_FIFO_AWBURST_HIGH:WAD_FIFO_AWBURST_LOW]};
assign wres_fifo_rd_en = (wr_bvalid & S_AXI_BREADY) ? 1'b1 : 1'b0;

// S_AXI_BID の処理
assign S_AXI_BID = wres_fifo_dout[WRES_FIFO_AWID_HIGH:WRES_FIFO_AWID_LOW];

// S_AXI_BRESP の処理
// S_AXI_AWBURSTがINCRの時はOKAYを返す。それ以外はSLVERRを返す。
assign S_AXI_BRESP = (wres_fifo_dout[WRES_FIFO_AWBURST_HIGH:WRES_FIFO_AWBURST_LOW]==AXBURST_INCR) ? RESP_OKAY : RESP_SLVERR;

// wr_bvalid の処理
// wr_bvalid のアサートは、Write Data Channelの完了より必ず1クロックは遅延する
always @ ( posedge ACLK ) begin
    if ( reset ) begin
        wrres_cs <= IDLE_WRES;
        wr_bvalid <= 1'b0;
    end
    else
        case (wrres_cs)
            IDLE_WRES:  if ( wres_fifo_empty == 1'b0 ) begin    // Write Transaction 終了
                            if ( (m_seq16_wr_res == 0) || (RANDOM_BVALID_WAIT == 0) ) begin
                                wrres_cs <= BVALID_ASSERT;
                                wr_bvalid <= 1'b1;
                            end
                            else
                                wrres_cs <= WAIT_BVALID;
                        end
            WAIT_BVALID:if ( wr_resp_cnt == 0 ) begin
                            wrres_cs <= BVALID_ASSERT;
                            wr_bvalid <= 1'b1;
                        end
            BVALID_ASSERT: if ( S_AXI_BREADY ) begin
                            wrres_cs <= IDLE_WRES;
                            wr_bvalid <= 1'b0;
                          end
        endcase
end

assign S_AXI_BVALID = wr_bvalid;
assign S_AXI_BUSER  = 'b0;

// wr_resp_cnt
always @ ( posedge ACLK ) begin
    if ( reset )
        wr_resp_cnt <= 'b0;
    else begin
        if ( (wrres_cs == IDLE_WRES) && (wres_fifo_empty==1'b0) )
            wr_resp_cnt <= m_seq16_wr_res[4:0];
        else if ( wr_resp_cnt!=0 )
            wr_resp_cnt <= wr_resp_cnt - 1;
    end
end

// m_seq_wr_res、16ビットのM系列を計算する
always @ ( posedge ACLK ) begin
    if ( reset )
        m_seq16_wr_res <= 16'b1;
    else
        m_seq16_wr_res <= M_SEQ16_BFM_F(m_seq16_wr_res);
end


// AXI4バス Read Address Transaction State Machine
always @ ( posedge ACLK ) begin
    if ( reset ) begin
        rdadr_cs <= IDLE_RDA;
        arready <= 1'b0;
    end
    else
        case (rdadr_cs)
            IDLE_RDA:   if ( (S_AXI_ARVALID == 1'b1) && (rad_fifo_full == 1'b0) ) begin // Read Transaction要求
                            rdadr_cs <= ARR_ACCEPT;
                            arready  <= 1'b1;
                        end
            ARR_ACCEPT: begin   // S_AXI_ARREADYをアサート
                            rdadr_cs <= IDLE_RDA;
                            arready  <= 1'b0;
                        end
        endcase
end

assign S_AXI_ARREADY = (ARREADY_IS_USUALLY_HIGH==1) ? ~rad_fifo_full : arready;

// S_AXI_ARID & S_AXI_ARBURST & S_AXI_ARSIZE & S_AXI_ARLEN & S_AXI_ARADDR を保存しておく同期FIFO
assign rad_fifo_din ={S_AXI_ARID, S_AXI_ARBURST, S_AXI_ARSIZE, S_AXI_ARLEN, S_AXI_ARADDR};
assign rad_fifo_wr_en = (ARREADY_IS_USUALLY_HIGH==1) ? (S_AXI_ARVALID & ~rad_fifo_full) : arready;

sync_fifo #(
    .C_MEMORY_SIZE  (AD_FIFO_DEPTH),
    .DATA_BUS_WIDTH (RAD_FIFO_WIDTH)
  ) rad_fifo
 (
    .clk            (ACLK),
    .rst            (reset),
    .wr_en          (rad_fifo_wr_en),
    .din            (rad_fifo_din),
    .full           (rad_fifo_full),
    .almost_full    (rad_fifo_almost_full),
    .rd_en          (rad_fifo_rd_en),
    .dout           (rad_fifo_dout),
    .empty          (rad_fifo_empty),
    .almost_empty   (rad_fifo_almost_empty)
);

assign rad_fifo_rd_en = (rvalid & S_AXI_RREADY & rlast);


// AXI4バス Read Data Transaction State Machine
always @( posedge ACLK ) begin
    if ( reset )
        rddat_cs <= IDLE_RDD;
    else
        case (rddat_cs)
            IDLE_RDD:   if ( rad_fifo_empty == 1'b0 )   // AXI Read アドレス転送の残りが1個以上ある
                            rddat_cs <= RD_BURST;
            RD_BURST:   if ( (rd_axi_count == 0) && rvalid && S_AXI_RREADY )  // Read Transaction終了
                            rddat_cs <= IDLE_RDD;
        endcase
end

// m_seq_rd、16ビットのM系列を計算する
always @ ( posedge ACLK ) begin
    if ( reset )
        m_seq16_rd <= 16'hffff;
    else begin
        if ( READ_RANDOM_WAIT) begin
            if ( rddat_cs == RD_BURST )
                m_seq16_rd <= M_SEQ16_BFM_F(m_seq16_rd);
        end else
            m_seq16_rd <= 16'b0;
    end
end


// rvalidの処理、M系列を計算して128以上だったらWaitする
always @( posedge ACLK ) begin
    if ( reset ) begin
        cs_rvalid <= IDLE_RVALID;
        rvalid    <= 1'b0;
    end
    else
        case (cs_rvalid)
            IDLE_RVALID:    if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) ) begin // 次はrd_burst
                                if ( m_seq16_rd[7] == 1'b0 ) begin
                                    cs_rvalid <= ASSERT_RVALID;
                                    rvalid    <= 1'b1;
                                end
                                else begin
                                    cs_rvalid <= DEASSERT_RVALID;
                                    rvalid <= 1'b0;
                                end
                            end
            ASSERT_RVALID:  if ( (rddat_cs == RD_BURST) && rlast && S_AXI_RREADY ) begin    // 終了
                                cs_rvalid <= IDLE_RVALID;
                                rvalid    <= 1'b0;
                            end
                            else if ( (rddat_cs == RD_BURST) & S_AXI_RREADY ) begin // 1つのトランザクション終了
                                if ( m_seq16_rd[7] ) begin
                                    cs_rvalid <= DEASSERT_RVALID;
                                    rvalid    <= 1'b0;
                                end
                            end
            DEASSERT_RVALID:if ( m_seq16_rd[7] == 1'b0 ) begin
                                cs_rvalid <= ASSERT_RVALID;
                                rvalid    <= 1'b1;
                            end
        endcase
end

assign S_AXI_RVALID = rvalid;

// addr_inc_step_rdの処理
always @( posedge ACLK ) begin
    if ( reset )
        addr_inc_step_rd <= 1;
    else begin
        if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) )
            case (rad_fifo_dout[RAD_FIFO_ARSIZE_HIGH:RAD_FIFO_ARSIZE_LOW])
                3'b000: addr_inc_step_rd <=   1;    //    8ビット転送
                3'b001: addr_inc_step_rd <=   2;    //   16ビット転送
                3'b010: addr_inc_step_rd <=   4;    //   32ビット転送
                3'b011: addr_inc_step_rd <=   8;    //   64ビット転送
                3'b100: addr_inc_step_rd <=  16;    //  128ビット転送
                3'b101: addr_inc_step_rd <=  32;    //  256ビット転送
                3'b110: addr_inc_step_rd <=  64;    //  512ビット転送
                default:addr_inc_step_rd <= 128;    // 1024ビット転送
            endcase
        end
end


// rd_addr の処理
always @ ( posedge ACLK ) begin
    if ( reset )
        rd_addr <= 'b0;
    else begin
        if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) )
            rd_addr <= rad_fifo_dout[(C_OFFSET_WIDTH - 1):0];
        else if ( (rddat_cs == RD_BURST) && S_AXI_RREADY && rvalid )
            rd_addr <= (rd_addr + addr_inc_step_rd);
    end
end


// rd_axi_countの処理（AXIバス側のデータカウント）
always @ ( posedge ACLK ) begin
    if ( reset )
        rd_axi_count <= 'b0;
    else begin
        if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) ) // rd_axi_countのロード
            rd_axi_count <= rad_fifo_dout[RAD_FIFO_ARLEN_HIGH:RAD_FIFO_ARLEN_LOW];
        else if ( (rddat_cs == RD_BURST) && rvalid && S_AXI_RREADY )    // Read Transactionが1つ終了
            rd_axi_count <= rd_axi_count - 1;
    end
end


// rdlast State Machine
always @ ( posedge ACLK ) begin
    if ( reset ) begin
        rdlast <= IDLE_RLAST;
        rlast  <= 1'b0;
    end
    else
        case (rdlast)
            IDLE_RLAST: if ( (rd_axi_count == 1) && rvalid && S_AXI_RREADY ) begin  // バーストする場合
                            rdlast <= RLAST_ASSERT;
                            rlast  <= 1'b1;
                        end
                        else if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) &&
                                  (rad_fifo_dout[RAD_FIFO_ARLEN_HIGH:RAD_FIFO_ARLEN_LOW] == 0) ) begin // 転送数が1の場合
                            rdlast <= RLAST_ASSERT;
                            rlast  <= 1'b1;
                        end
            RLAST_ASSERT:if ( rvalid && S_AXI_RREADY ) begin    // Read Transaction終了（rd_axi_count=0は決定）
                            rdlast <= IDLE_RLAST;
                            rlast  <= 1'b0;
                         end
        endcase
end

assign S_AXI_RLAST = rlast;


// S_AXI_RID, S_AXI_RUSER の処理
always @ ( posedge ACLK ) begin
    if ( reset )
        S_AXI_RID <= 'b0;
    else begin
        if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) )
            S_AXI_RID <= rad_fifo_dout[RAD_FIFO_ARID_HIGH:RAD_FIFO_ARID_LOW];
    end
end

assign S_AXI_RUSER = 'b0;


// S_AXI_RRESP は、S_AXI_ARBURST がINCR の場合はOKAYを返す。それ以外はSLVERRを返す。
always @( posedge ACLK ) begin
    if ( reset )
        S_AXI_RRESP <= 'b0;
    else begin
        if ( (rddat_cs == IDLE_RDD) && (rad_fifo_empty == 1'b0) ) begin
            if ((rad_fifo_dout[RAD_FIFO_ARBURST_HIGH:RAD_FIFO_ARBURST_LOW] == AXBURST_INCR))
                S_AXI_RRESP <= RESP_OKAY;
            else
                S_AXI_RRESP <= RESP_SLVERR;
        end
    end
end

// RAM
integer i;

always @( posedge ACLK ) begin
    if ( cdc_we ) begin :Block_Name_2
        for (i=0; i<(C_S_AXI_DATA_WIDTH / 8); i=i+1) begin
            if ( S_AXI_WSTRB[i] )
                ram_array[wr_addr[(C_OFFSET_WIDTH - 1):ADD_INC_OFFSET]][(i * 8) +: 8]
                    <= S_AXI_WDATA[(i * 8) +: 8];
        end
    end
end

// Read Transaciton の時に +1 されたReadデータを使用する（Read 毎に+1）
always @( posedge ACLK ) begin
    if ( reset )
        read_data_count <= 'b0;
    else begin
        if ( (rddat_cs == RD_BURST) && rvalid && S_AXI_RREADY )
            read_data_count <= read_data_count + 1;
    end
end

assign S_AXI_RDATA = (READ_DATA_IS_INCREMENT == 0) ?
                      ram_array[rd_addr[(C_OFFSET_WIDTH - 1):ADD_INC_OFFSET]] : read_data_count;

endmodule

sync_fifo.v を示します。
（追加）このHDLコードは無保証です。このコードを使用したことによる損害の保証はいたしません。ホビー向けとします。お仕事で使われるなど、信頼性が必要な用途には、Xilinx社で販売している売り物のBFMをご使用下さい。

// Synchronous FIFO for Simulation
//
// 2013/11/08 by marsee
//
//  ライセンスは二条項BSDライセンス (2-clause BSD license)とします。
//

`default_nettype none

module sync_fifo #(
    parameter integer C_MEMORY_SIZE =        512,    // Word (not byte), 2のn乗
    parameter integer DATA_BUS_WIDTH =        32        // RAM Data Width
)
(
    input    wire    clk,
    input    wire    rst,
    input    wire    wr_en,
    input    wire    [DATA_BUS_WIDTH-1:0]    din,
    output    wire    full,
    output    wire    almost_full,
    input    wire    rd_en,
    output    wire     [DATA_BUS_WIDTH-1:0]    dout,
    output    wire    empty,
    output    wire    almost_empty
);

    // Beyond Circuts, Constant Function in Verilog 2001を参照しました
    // http://www.beyond-circuits.com/wordpress/2008/11/constant-functions/
    function integer log2;
        input integer addr;
        begin
            addr = addr - 1;
            for (log2=0; addr>0; log2=log2+1)
                addr = addr >> 1;
        end
    endfunction

    reg        [DATA_BUS_WIDTH-1:0]    mem    [0:C_MEMORY_SIZE-1];
    reg        [log2(C_MEMORY_SIZE)-1:0]   mem_waddr = 0;
    reg        [log2(C_MEMORY_SIZE)-1:0]   mem_raddr = 0;
    reg        [log2(C_MEMORY_SIZE)-1:0]    rp = 0;
    reg        [log2(C_MEMORY_SIZE)-1:0]    wp = 0;

    wire    [log2(C_MEMORY_SIZE)-1:0]   plus_1 = 1;
    wire    [log2(C_MEMORY_SIZE)-1:0]   plus_2 = 2;

    wire    almost_full_node;
    wire    almost_empty_node;

    integer i;
    // initialize RAM Data
    initial begin
        for (i=0; i<C_MEMORY_SIZE; i=i+1)
            mem[i] = 0;
    end

    // Write
    always @(posedge clk) begin
        if (rst) begin
            mem_waddr <= 0;
            wp <= 0;
        end else begin
            if (wr_en) begin
                mem_waddr <= mem_waddr + 1;
                wp <= wp + 1;
            end
        end
    end

    always @(posedge clk) begin
         if (wr_en) begin
            mem[mem_waddr] <= din;
        end
    end

    assign full = (wp+plus_1 == rp) ? 1'b1 : 1'b0;
    assign almost_full_node = (wp+plus_2 == rp) ? 1'b1 : 1'b0;
    assign almost_full = full | almost_full_node;

    // Read
    always @(posedge clk) begin
        if (rst) begin
            mem_raddr <= 0;
            rp <= 0;
        end else begin
            if (rd_en) begin
                mem_raddr <= mem_raddr + 1;
                rp <= rp + 1;
            end
        end
    end

    assign dout = mem[mem_raddr];

    assign empty = (wp == rp) ? 1'b1 : 1'b0;
    assign almost_empty_node = (wp == rp+plus_1) ? 1'b1 : 1'b0;
    assign almost_empty = empty | almost_empty_node;
endmodule

`default_nettype wire

このAXI4 Slave BFM をテストしてみてバグがあったら、ぜひ教えてください。よろしくお願いします。

Zybotの車輪のセンサー・フィードバックのデータを取得した

”Zybot の車輪が回転した”で、Vivado HLS で作成したPWMモジュールIP でZybot の車輪を回転させた。

今回はPmodHB5 のセンサー・フィードバック・ピンのSA と SB から車輪の回転角速度を検出してみた。車輪の回転角速度を検出には、”PmodHB5 のセンサー・フィードバック処理IP を作ってみた3”で作ったPmodHB5 のセンサー・フィードバック処理IP を使っている。

ZYBO_0_162_2 プロジェクトでPWMモジュールIP とセンサー・フィードバック処理IP を使用している。そのブロック・デザインを示す。


2つのIP は PmodHB5_inf_right と PmodHB5_inf_left の2つのモジュール内に入っている。そのブロック・デザインを示す。


IP のアドレスマップを示す。


ビットストリームの生成までを行った。レポートを示す。


ハードウェアをエクスポートして、SDKを立ち上げた。
mm_test プロジェクトに mm_test.c を作成した。mm_test.c を示す。

/* * mm_test.c * *  Created on: 2016/06/29 *      Author: ono */
#include <stdio.h>
#include <stdlib.h>
#include "xil_io.h"
#include "xparameters.h"
#include "sleep.h"
#include "xmotor_monitor.h"

 int main(){
    XMotor_monitor XMmoniL, XMmoniR;
    XMotor_monitor_Config *XMmoniLPTR, *XMmoniRPTR;

    XMmoniLPTR = XMotor_monitor_LookupConfig(0);
    if(!XMmoniLPTR){
        fprintf(stderr, "Left XMotor monitor configuration failed.\n");
        return(-1);
    }
    int XMmL_status = XMotor_monitor_CfgInitialize(&XMmoniL, XMmoniLPTR);
    if (XMmL_status != XST_SUCCESS){
        fprintf(stderr, "Could not Initialize Left XMotor monitor\n");
        return(-1);
    }

    XMmoniRPTR = XMotor_monitor_LookupConfig(1);
    if(!XMmoniRPTR){
        fprintf(stderr, "Right XMotor monitor configuration failed.\n");
        return(-1);
    }
    int XMmR_status = XMotor_monitor_CfgInitialize(&XMmoniR, XMmoniRPTR);
    if (XMmR_status != XST_SUCCESS){
        fprintf(stderr, "Could not Initialize Right XMotor monitor\n");
        return(-1);
    }

    while (!XMotor_monitor_IsIdle(&XMmoniL));
    while (!XMotor_monitor_IsIdle(&XMmoniR));

    XMotor_monitor_Start(&XMmoniL);
    //XMotor_monitor_EnableAutoRestart(&XMmoniL);
    while (!XMotor_monitor_IsIdle(&XMmoniL));

    XMotor_monitor_Start(&XMmoniR);
    //XMotor_monitor_EnableAutoRestart(&XMmoniR);
    while (!XMotor_monitor_IsIdle(&XMmoniR));

    u32 sa_countL = XMotor_monitor_Get_sa_count_V(&XMmoniL);
    u32 sa_countR = XMotor_monitor_Get_sa_count_V(&XMmoniR);

    u32 sb_levelL = XMotor_monitor_Get_sb_level_V(&XMmoniL);
    u32 sb_levelR = XMotor_monitor_Get_sb_level_V(&XMmoniR);

    u32 returnL = XMotor_monitor_Get_return(&XMmoniL);
    u32 returnR = XMotor_monitor_Get_return(&XMmoniR);

    printf("sa_countL = %d, sb_levelL = %d, returnL = %d\n", (unsigned int)sa_countL,
            (unsigned int)sb_levelL, (unsigned int)returnL);
    printf("sa_countR = %d, sb_levelR = %d, returnR = %d\n", (unsigned int)sa_countR,
            (unsigned int)sb_levelR, (unsigned int)returnR);


    return 0;
}

これを実行してみた。


sb_level はL, R 共にばらついてしまう。これはなぜかわからないが、回転方向についてはPWMモジュールに設定した値を信じることにする。
次に、回転角速度については、6回を平均することにした。
sa_countL は (2364 + 2361 + 2331 + 2398 + 2396 + 2328) * 1.04 / 6 ≒ 2457 us （100MHz クロックで 100 回平均しているので 1 / 100 のクロックになっている）
sa_countR は (2265 + 2262 + 2324 + 2298 + 2295 + 2239) * 1.04 / 6 ≒ 2371 us （100MHz クロックで 100 回平均しているので 1 / 100 のクロックになっている）

sa_countL は 2457 us = 407 Hz 、sa_countR は 421 Hz

オシロスコープで Right 側のSA と SB を見たところ、約 400 Hz だった。精度的にはあまり良くないか？もしかして、RとLを間違っている？

アリス・イン・ワンダーランド（時間の旅）の映画を見てきました

今日は、奥さんとアリス・イン・ワンダーランド（時間の旅）の映画を見てきました。
映像も素晴らしく、展開も面白くて、とっても良かったです。

PmodHB5 のセンサー・フィードバック処理IP を作ってみた3

”PmodHB5 のセンサー・フィードバック処理IP を作ってみた2”の続き。

どうもセンサー・フィードバックの値が安定しないと思ったら、アルゴリズムを間違っていた。SA の値が最初に 0 の途中のときに値が少なくなってしまう。そこで修正を行った。今回は最初に値を保存してから、反転しているとスタートするようにした。
さらに、100MHｚ クロックでキャプチャすると速すぎるのではないか？と思った。1MHzくらいでサンプルしたい。
HDL だと 100MHz の 100 回に 1 回のイネーブルを出すことで、1MHz サンプルにできるが、C で書くとどうしたらよいか考えた。そこで、100 回足し算して、50 以上だった 1 と、50 未満だったら 0 と判定することにした。
書き直した motor_monitor.cpp を貼っておく。

// motor_monitor.cpp
// 2016/06/15 by marsee
//

#include <ap_int.h>
#include <hls_stream.h>

#include "motor_monitor.h"

#define SUM_COUNT    100

// sa と sb の値を SUM_COUNT 分だけ積算して 1 か 0 かを決定する
void sum_sa_sb(hls::stream<ap_uint_1 >& sa, hls::stream<ap_uint_1 >& sb,
        ap_uint_1 & sa_val, ap_uint_1 & sb_val){

    ap_uint_1 sad;
    ap_uint_1 sbd;
    unsigned int sum_sa=0, sum_sb=0;

    for (int i=0; i<SUM_COUNT; i++){
#pragma HLS PIPELINE II=1
        sa >> sad;
        sb >> sbd;
        sum_sa += (unsigned int)sad;
        sum_sb += (unsigned int)sbd;
    }
    if (sum_sa >= SUM_COUNT/2)
        sa_val = 1;
    else
        sa_val = 0;

    if (sum_sb >= SUM_COUNT/2)
        sb_val = 1;
    else
        sb_val = 0;
}

int motor_monitor(hls::stream<ap_uint_1 >& sa, hls::stream<ap_uint_1 >& sb,
        ap_uint<32> & sa_count, ap_uint_1 & sb_level, int & sum_count){
#pragma HLS INTERFACE ap_stable port=sum_count
#pragma HLS INTERFACE s_axilite port=sum_count
#pragma HLS INTERFACE s_axilite port=return
#pragma HLS INTERFACE s_axilite port=sb_level
#pragma HLS INTERFACE s_axilite port=sa_count
#pragma HLS INTERFACE ap_hs port=sa
#pragma HLS INTERFACE ap_hs port=sb

    ap_uint_1 sad, sa_val, next_sa;
    ap_uint_1 sbd, next_sb;
    ap_uint<32> sac;
    ap_uint_1 sble;

    sum_count = SUM_COUNT+4; // sum_sa_sb() の呼び出しWaitを加算した
    // 最初の値を保存する
    sum_sa_sb(sa, sb, sa_val, next_sb);

    for (sac=0; sac!=0xffffffff; sac++){ // saの値が変わらない間はループ
        sum_sa_sb(sa, sb, next_sa, next_sb);
        if (sa_val != next_sa)
            break;
    }
    sa_val = next_sa;
    if (sac == 0xffffffff) // overflow
        return 1;

    if (next_sa == 1) // next_sa が立ち上がり
        sble = next_sb;

    for (sac=1; sac!=0xffffffff; sac++){ // saの値が変わらない間はループ
        sum_sa_sb(sa, sb, next_sa, next_sb);
        if (sa_val != next_sa)
            break;
    }
    sa_val = next_sa;
    if (sac == 0xffffffff) // overflow
        return 1;

    if (next_sa == 1) // next_sa が立ち上がり
        sble = next_sb;

    for (sac++; sac!=0xffffffff; sac++){ // saの値が変わらない間はループ
        sum_sa_sb(sa, sb, next_sa, next_sb);
        if (sa_val != next_sa)
            break;
    }
    if (sac == 0xffffffff) // overflow
        return 1;

    sa_count = sac;
    sb_level = sble;

    return 0;
}

motor_monitor.h を示す。

// motor_monitor.h
// 2016/06/16 by marsee
//

#ifndef __MOTOR_MONITOR___
#define __MOTOR_MONITOR___

#include <ap_int.h>

typedef ap_uint<1> ap_uint_1;
#endif

motor_monitor_tb.cpp を示す。

// motor_monitor_tb.cpp
// 2016/06/15 by marsee
//

#include <ap_int.h>
#include <hls_stream.h>

#include "motor_monitor.h"

#define DATASIZE 2000

int motor_monitor(hls::stream<ap_uint_1 >& sa, hls::stream<ap_uint_1 >& sb,
        ap_uint<32> & sa_count, ap_uint_1 & sb_level, int & sum_count);

int main(){
    using namespace std;

    hls::stream<ap_uint_1> sa;
    hls::stream<ap_uint_1> sb;

    ap_uint<32> sa_count;
    ap_uint_1 sb_level;
    int sum_count;

    ap_uint_1 salv=0;
    ap_uint_1 sblv=0;
    int cnt = 0;
    for (int j=0; j<(4*2); j++){
        for (int i=0; i<DATASIZE/4; i++){
            sa << salv;
            sb << sblv;
        }
        if (cnt==0)
            sblv = ~sblv;
        else if (cnt==1)
            salv = ~salv;
        else if (cnt==2)
            sblv = ~sblv;
        else // cnt == 3
            salv = ~salv;
        if (cnt==3)
            cnt = 0;
        else
            cnt++;
    }

    int ret_val = motor_monitor(sa, sb, sa_count, sb_level, sum_count);

    printf("sa_count = %d, sb_level = %d, return value = %d, sum_count = %d\n",
            (unsigned int)sa_count, (unsigned int)sb_level, ret_val, sum_count);

    return 0;
}

これで、C シミュレーションを行った。


うまく行っている。

C コードへの合成を行った。


Loop1, Loop2 の Latency の min が 104 クロックになっている。



C/RTLコシミュレーションを行った。


C/RTLコシミュレーション波形を示す。


拡大してみると、sum_sa_sb() が 100 クロック + 4 クロック = 104 クロックであることがわかる。そこで、sum_count には 104 を返すことにした。


これでIP 化を行った。

実機でテストしてみると sa_count の値はばらついてはいるが、大きく変動することはなくなった。しかし、 sb_level がいい加減だった。まだ何か間違っているだろうか？わかる人がいたら、ぜひご指摘ください。
sb_level は安定しなくてもPWM でどっち方向にモーターを回すかを設定しているので、回転方向は別に取れなくてもよいのだが。。。