// disp_lap_mask_on_serial.c
// 2017/02/25 by marsee
//
// Refered to http://japan.xilinx.com/support/documentation/sw_manuals_j/xilinx2014_4/ug902-vivado-high-level-synthesis.pdf
//
#include <stdio.h>
#include <stdlib.h>
#include "xfastx_corner_det.h"
#include "XUnsharp_mask_axis.h"
#include "XLap_filter_axis.h"
#include "xparameters.h"
#include "xaxivdma.h"
#include "xil_io.h"
#include "sleep.h"
#define NUMBER_OF_WRITE_FRAMES 3 // Note: If not at least 3 or more, the image is not displayed in succession.
#define HORIZONTAL_PIXELS 800
#define VERTICAL_LINES 600
#define PIXEL_NUM_OF_BYTES 4
#define FASTX_THRESHOLD 20
#define FRAME_BUFFER_ADDRESS 0x10000000
static XAxiVdma_DmaSetup Vdma0_WriteCfg;
void cam_i2c_init(volatile unsigned *mt9d111_i2c_axi_lites) {
mt9d111_i2c_axi_lites[64] = 0x2; // reset tx fifo ,address is 0x100, i2c_control_reg
mt9d111_i2c_axi_lites[64] = 0x1; // enable i2c
}
void cam_i2x_write_sync(void) {
// unsigned c;
// c = *cam_i2c_rx_fifo;
// while ((c & 0x84) != 0x80)
// c = *cam_i2c_rx_fifo; // No Bus Busy and TX_FIFO_Empty = 1
usleep(1000);
}
void cam_i2c_write(volatile unsigned *mt9d111_i2c_axi_lites, unsigned int device_addr, unsigned int write_addr, unsigned int write_data){
mt9d111_i2c_axi_lites[66] = 0x100 | (device_addr & 0xfe); // Slave IIC Write Address, address is 0x108, i2c_tx_fifo
mt9d111_i2c_axi_lites[66] = write_addr;
mt9d111_i2c_axi_lites[66] = (write_data >> 8)|0xff; // first data
mt9d111_i2c_axi_lites[66] = 0x200 | (write_data & 0xff); // second data
cam_i2x_write_sync();
}
int main(){
XFastx_corner_det fastx_filter;
XFastx_corner_det_Config *fastx_filterPtr;
XLap_filter_axis lap_filter;
XLap_filter_axis_Config *lap_filterPtr;
XUnsharp_mask_axis usm_filter;
XUnsharp_mask_axis_Config *usm_filterPtr;
XAxiVdma_Config *XAxiVdma0_Config;
XAxiVdma XAxiVdma0;
int XAxiVdma0_Status;
float k;
int k_fixed;
int i, threshold;
XAxiVdma0_Config = XAxiVdma_LookupConfig(XPAR_CAMERA_INTERFACE_AXI_VDMA_0_DEVICE_ID); // Look up the hardware configuration for a device instance
if (XAxiVdma0_Config == NULL){
fprintf(stderr, "No AXI VDMA found\n");
return(-1);
}
XAxiVdma0_Status = XAxiVdma_CfgInitialize(&XAxiVdma0, XAxiVdma0_Config, XAxiVdma0_Config->BaseAddress); // Initialize the driver with hardware configuration
if (XAxiVdma0_Status != XST_SUCCESS){
fprintf(stderr, "XAxiVdma_CfgInitialize() failed\n");
return(-1);
}
XAxiVdma_Reset(&XAxiVdma0, XAXIVDMA_WRITE);
while(XAxiVdma_ResetNotDone(&XAxiVdma0, XAXIVDMA_WRITE)) ;
XAxiVdma0_Status = XAxiVdma_SetFrmStore(&XAxiVdma0, NUMBER_OF_WRITE_FRAMES, XAXIVDMA_WRITE); // Set the number of frame store buffers to use.
Vdma0_WriteCfg.VertSizeInput = VERTICAL_LINES;
Vdma0_WriteCfg.HoriSizeInput = HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES;
Vdma0_WriteCfg.Stride = HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES; // Indicates the number of address bytes between the first pixels of each video line.
Vdma0_WriteCfg.FrameDelay = 0; // Indicates the minimum number of frame buffers the Genlock slave is to be behind the locked master. This field is only used if the channel is enabled for Genlock Slave operations. This field has no meaning in other Genlock modes.
Vdma0_WriteCfg.EnableCircularBuf = 1; // Indicates frame buffer Circular mode or frame buffer Park mode. 1 = Circular Mode Engine continuously circles through frame buffers.
Vdma0_WriteCfg.EnableSync = 0; // Enables Genlock or Dynamic Genlock Synchronization. 0 = Genlock or Dynamic Genlock Synchronization disabled.
Vdma0_WriteCfg.PointNum = 0; // No Gen-Lock
Vdma0_WriteCfg.EnableFrameCounter = 0; // Endless transfers
Vdma0_WriteCfg.FixedFrameStoreAddr = 0; // We are not doing parking
XAxiVdma0_Status = XAxiVdma_DmaConfig(&XAxiVdma0, XAXIVDMA_WRITE, &Vdma0_WriteCfg);
if (XAxiVdma0_Status != XST_SUCCESS){
fprintf(stderr, "XAxiVdma_DmaConfig() failed\n");
return(-1);
}
// Frame buffer address set
unsigned int frame_addr = (unsigned int)FRAME_BUFFER_ADDRESS;
for (i=0; i<NUMBER_OF_WRITE_FRAMES; i++){
Vdma0_WriteCfg.FrameStoreStartAddr[i] = frame_addr;
//frame_addr += HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES * VERTICAL_LINES;
}
XAxiVdma0_Status = XAxiVdma_DmaSetBufferAddr(&XAxiVdma0, XAXIVDMA_WRITE, Vdma0_WriteCfg.FrameStoreStartAddr);
if (XAxiVdma0_Status != XST_SUCCESS){
fprintf(stderr, "XAxiVdma_DmaSetBufferAddr() failed\n");
return(-1);
}
// Look Up the device configuration
fastx_filterPtr = XFastx_corner_det_LookupConfig(0);
if (!fastx_filterPtr){
fprintf(stderr, "XFastx_corner_det configuration failed.\n");
return(-1);
}
lap_filterPtr = XLap_filter_axis_LookupConfig(0);
if (!lap_filterPtr){
fprintf(stderr, "XLap_filter_axis configuration failed.\n");
return(-1);
}
usm_filterPtr = XUnsharp_mask_axis_LookupConfig(0);
if (!usm_filterPtr){
fprintf(stderr, "XUnsharp_mask_axis configuration failed.\n");
return(-1);
}
// Initialize the Device
int fastx_status = XFastx_corner_det_CfgInitialize(&fastx_filter, fastx_filterPtr);
if (fastx_status != XST_SUCCESS){
fprintf(stderr, "Could not Initialize XFastx_corner_det\n");
return(-1);
}
int lap_status = XLap_filter_axis_CfgInitialize(&lap_filter, lap_filterPtr);
if (lap_status != XST_SUCCESS){
fprintf(stderr, "Could not Initialize XLap_filter_axis\n");
return(-1);
}
int usm_status = XUnsharp_mask_axis_CfgInitialize(&usm_filter, usm_filterPtr);
if (usm_status != XST_SUCCESS){
fprintf(stderr, "Could not Initialize XUnsharp_mask_axis\n");
return(-1);
}
// Unsharp_masking filter k_fixed set
XFastx_corner_det_Set_cols(&fastx_filter, (u32)800);
XFastx_corner_det_Set_rows(&fastx_filter, (u32)600);
XFastx_corner_det_Set_threshold(&fastx_filter, (u32)FASTX_THRESHOLD);
XUnsharp_mask_axis_Set_k_fixed_V(&usm_filter, (u32)10); // k = 2.5
// axis_switch_1, 1to2 ,Select M01_AXIS
// Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x40), 0);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x44), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x48), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x4C), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR), 0x2); // Commit registers
// axis_switch_0, 2to1, Select S01_AXIS
// Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR+0x40), 0x0);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR), 0x2); // Commit registers
// VDMA start
XAxiVdma0_Status = XAxiVdma_DmaStart(&XAxiVdma0, XAXIVDMA_WRITE);
if (XAxiVdma0_Status != XST_SUCCESS){
fprintf(stderr, "XAxiVdma_DmaStart() failed\n");
return(-1);
}
// mt9d111_inf_axis_0, axi_iic_0, bitmap_disp_cntrler_axi_master_0
volatile unsigned int *bmdc_axi_lites;
volatile unsigned int *mt9d111_axi_lites;
volatile unsigned int *mt9d111_i2c_axi_lites;
bmdc_axi_lites = (volatile unsigned *)XPAR_BITMAP_DISP_CNTRLER_AXI_MASTER_0_BASEADDR;
mt9d111_axi_lites = (volatile unsigned *)XPAR_CAMERA_INTERFACE_MT9D111_INF_AXIS_0_BASEADDR;
mt9d111_i2c_axi_lites = (volatile unsigned *)XPAR_CAMERA_INTERFACE_AXI_IIC_0_BASEADDR;
bmdc_axi_lites[0] = (volatile unsigned int)FRAME_BUFFER_ADDRESS; // Bitmap Display Controller start
mt9d111_axi_lites[0] = (volatile unsigned int)FRAME_BUFFER_ADDRESS; // Camera Interface start (Address is dummy)
// CMOS Camera initialize, MT9D111
cam_i2c_init(mt9d111_i2c_axi_lites);
cam_i2c_write(mt9d111_i2c_axi_lites, 0xba, 0xf0, 0x1); // Changed regster map to IFP page 1
cam_i2c_write(mt9d111_i2c_axi_lites, 0xba, 0x97, 0x20); // RGB Mode, RGB565
mt9d111_axi_lites[1] = 0; // One_shot_mode is disabled
while(1){
printf("\n\r0:normal camera, 1:fastx, 2:laplacian filter, 3:unsharp_masking filter, 999:end");
printf("\n\rPlease input = ");
scanf("%d", &i);
if (i == 999)
break;
switch(i){
case 0 : // normal camera
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x40), 0);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x44), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x48), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x4C), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR), 0x2); // Commit registers
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR+0x40), 0x0);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR), 0x2); // Commit registers
while(1){
printf("\n\rEnter 999 to return to the function selection mode\n");
scanf("%d", &i);
if (i == 999)
break;
}
break;
case 1 : // fastx
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x40), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x44), 0);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x48), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x4C), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR), 0x2); // Commit registers
XFastx_corner_det_Start(&fastx_filter);
XFastx_corner_det_EnableAutoRestart(&fastx_filter);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR+0x40), 0x1);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR), 0x2); // Commit registers
while(1){
printf("\n\rEnter 999 to return; threshold = ");
scanf("%d", &threshold);
if (threshold == 999)
break;
XFastx_corner_det_Set_threshold(&fastx_filter, (u32)threshold);
}
XFastx_corner_det_DisableAutoRestart(&fastx_filter);
break;
case 2 : // laplacian filter
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x40), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x44), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x48), 0);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x4C), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR), 0x2); // Commit registers
XLap_filter_axis_Start(&lap_filter);
XLap_filter_axis_EnableAutoRestart(&lap_filter);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR+0x40), 0x2);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR), 0x2); // Commit registers
while(1){
printf("\n\rEnter 999 to return to the function selection mode\n");
scanf("%d", &i);
if (i == 999)
break;
}
XLap_filter_axis_DisableAutoRestart(&lap_filter);
break;
case 3 :
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x40), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x44), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x48), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x4C), 0);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR), 0x2); // Commit registers
XUnsharp_mask_axis_Start(&usm_filter);
XUnsharp_mask_axis_EnableAutoRestart(&usm_filter);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR+0x40), 0x3);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR), 0x2); // Commit registers
while(1){
printf("\n\rEnter 999 to return; k (0.25 ~ 3.75) = ");
scanf("%f", &k);
if (k == 999)
break;
if (k < 0)
k_fixed = 0;
else if (k > 4)
k_fixed = (int)(3.75*4);
else
k_fixed = (int)(k*4);
XUnsharp_mask_axis_Set_k_fixed_V(&usm_filter, (u32)k_fixed);
}
XUnsharp_mask_axis_DisableAutoRestart(&usm_filter);
break;
}
}
return(0);
}
// obstacle_avoid.cpp : Obstacle avoidance
// 2017/02/13 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 <termios.h>
#include "xpwm.h"
#include "xmotor_monitor.h"
#define DIR_LEFT_NORMAL 1
#define DIR_LEFT_REVERSE 0
#define DIR_RIGHT_NORMAL 0q
#define DIR_RIGHT_REVERSE 1
#define PIXEL_NUM_OF_BYTES 4
#define SVGA_HORIZONTAL_PIXELS 800
#define SVGA_VERTICAL_LINES 600
#define SVGA_ALL_DISP_ADDRESS (SVGA_HORIZONTAL_PIXELS * SVGA_VERTICAL_LINES * PIXEL_NUM_OF_BYTES)
#define CHECK_LINE_NUMBER 290
#define OBSTACLE_H_HIGH_LIMIT 200
#define OBSTACLE_H_LOW_LIMIT 160
#define SEARCH_OBSTACLE_THRESHOLD 15
#define OBSTACLE_AVOID_DISTANCE 60.0 // cm
#define OBSTACLE_AVOID_NEAR_DISTANCE 30.0 // cm
//#define DEBUG
//#define MOTOR_OFF
// Start RGB2HSV
//
void rgb2hsv_on(){
int fd2, fd3, fd15;
volatile unsigned *axis_switch_0, *axis_switch_1;
volatile unsigned *rgb2hsv_0;
// axis_switch_0 (UIO2)
fd2 = open("/dev/uio2", O_RDWR); // axis_switch_0 interface AXI4 Lite Slave
if (fd2 < 1){
fprintf(stderr, "/dev/uio2 (axis_switch_0) open error\n");
exit(-1);
}
axis_switch_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd2, 0);
if (!axis_switch_0){
fprintf(stderr, "axis_switch_0 mmap error\n");
exit(-1);
}
// axis_switch_1 (UIO3)
fd3 = open("/dev/uio3", O_RDWR); // axis_switch_1 interface AXI4 Lite Slave
if (fd3 < 1){
fprintf(stderr, "/dev/uio3 (axis_switch_1) open error\n");
exit(-1);
}
axis_switch_1 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd3, 0);
if (!axis_switch_1){
fprintf(stderr, "axis_switch_1 mmap error\n");
exit(-1);
}
// rgb2hsv_0 (UIO15)
fd15 = open("/dev/uio15", O_RDWR); // rgb2hsv_0 interface AXI4 Lite Slave
if (fd15 < 1){
fprintf(stderr, "/dev/uio15 (rgb2hsv_0) open error\n");
exit(-1);
}
rgb2hsv_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd15, 0);
if (!rgb2hsv_0){
fprintf(stderr, "rgb2hsv_0 mmap error\n");
exit(-1);
}
// axis_switch_1, 1to2 ,Select M01_AXIS
// Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
axis_switch_1[16] = 0x80000000; // 0x40 = 0x80000000; disable
axis_switch_1[17] = 0x80000000; // 0x44 = 0x80000000; disable
axis_switch_1[18] = 0x80000000; // 0x48 = 0x80000000, disable
axis_switch_1[19] = 0x0; // 0x4C = 0
axis_switch_1[0] = 0x2; // 0x0 = 2; Commit registers
// rgb2hsv IP AXIS Start
rgb2hsv_0[0] = 0x01; // Start bit set
rgb2hsv_0[0] = 0x80; // Auto Restart bit set
// axis_switch_0, 2to1, Select S01_AXIS
// Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
axis_switch_0[16] = 0x3; // 0x40 = 0x3;
axis_switch_0[0] = 0x2; // 0x0 = 2; Commit registers
munmap((void *)axis_switch_0, 0x10000);
munmap((void *)axis_switch_1, 0x10000);
munmap((void *)rgb2hsv_0, 0x10000);
close(fd2);
close(fd3);
close(fd15);
}
// Motor
//
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);
}
void motor_initialize(XPwm &motorL, XPwm &motorR, XMotor_monitor &mmL, XMotor_monitor &mmR){
XPwm *motorLp, *motorRp;
XMotor_monitor *mmLp, *mmRp;
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(motorLp, 0);
XPwm_Set_dir_V(motorLp, 1);
XPwm_Set_sw_late_V(motorRp, 0);
XPwm_Set_dir_V(motorRp, 0);
motor_settings(motorLp, motorRp);
}
void SetMotorLR(XPwm &motorL, XPwm &motorR, int left_rate, int right_rate){
static int PowerR=0;
static int PowerL=0;
#ifndef MOTOR_OFF
if (PowerR == 0 && PowerL == 0 && left_rate != 0 && right_rate != 0){
XPwm_Set_sw_late_V(&motorL, 25);
XPwm_Set_sw_late_V(&motorR, 25);
usleep(500000);
}
XPwm_Set_sw_late_V(&motorL, left_rate);
XPwm_Set_sw_late_V(&motorR, right_rate);
#endif
PowerL = left_rate;
PowerR = right_rate;
}
// Search Obstacle
// return 0 -- Success, return 1 -- I can not find a marker.
//
int SearchObstacle(volatile unsigned int *addr, int &obstacle_size, int &obstacle_start){
enum STATE {IDLE, FIND_OBSTACLE};
enum STATE cs = IDLE;
int h = 0;
obstacle_size = 0;
for (int i=0; i<SVGA_HORIZONTAL_PIXELS; i++){
h = (addr[i]/65536) & 0x1ff; // H select
#ifdef DEBUG
if (i==SVGA_HORIZONTAL_PIXELS/2)
printf("H = %d\n", h);
#endif
switch (cs){
case IDLE :
if (obstacle_size<SEARCH_OBSTACLE_THRESHOLD){
if (h<OBSTACLE_H_HIGH_LIMIT && h>OBSTACLE_H_LOW_LIMIT){
obstacle_size++;
} else {
obstacle_size = 0;
}
} else {
cs = FIND_OBSTACLE;
}
break;
case FIND_OBSTACLE :
if (h<OBSTACLE_H_HIGH_LIMIT && h>OBSTACLE_H_LOW_LIMIT){ // obstacle
if (obstacle_size == SEARCH_OBSTACLE_THRESHOLD){
if (i>=SEARCH_OBSTACLE_THRESHOLD)
obstacle_start = i-SEARCH_OBSTACLE_THRESHOLD;
else // error
obstacle_start = 0;
}
obstacle_size++;
} else {
return(0); // end
}
break;
}
}
return(1); // error
}
// ultrasonic sensor detect
float us_sensor_det(volatile unsigned int *ultrasonic_sensor_inf_0){
ultrasonic_sensor_inf_0[0] = 0x1; // ap_start = 1
while((ultrasonic_sensor_inf_0[0] & 0x2) == 0) // wait ap_done
usleep(500); // 5 us
if(int ret_val = (int)ultrasonic_sensor_inf_0[4]) // 0x10 : Data signal of ap_return
printf("Error: %d\n", ret_val);
float measure = (float)ultrasonic_sensor_inf_0[6] * 0.000342 / 2.0; // 0x18 : Data signal of count_val
#ifdef DEBUG
printf("Distance is %.2f cm\n", measure);
#endif
return(measure);
}
// debug_print()
void debug_print(const char s[]){
#ifdef DEBUG
printf("%s\n", s);
//usleep(500000);
#endif
}
int main(){
int fd1;
XPwm motorL, motorR;
XMotor_monitor mmL, mmR;
unsigned char attr[1024];
unsigned long phys_addr;
int c;
int obstacle_size, obstacle_start, obstacle_center;
struct termios save_settings;
struct termios settings;
volatile unsigned int *ultrasonic_sensor_inf_0;
int fd16;
int distance;
// Reffered to http://d.hatena.ne.jp/mFumi/20101002/1286003738
tcgetattr(0,&save_settings);
settings = save_settings;
settings.c_lflag &= ~(ECHO|ICANON);
settings.c_cc[VTIME] = 0;
settings.c_cc[VMIN] = 1;
tcsetattr(0,TCSANOW,&settings);
fcntl(0,F_SETFL,O_NONBLOCK);
// Start RGB2HSV
rgb2hsv_on();
// Motor Initialize
motor_initialize(motorL, motorR, mmL, mmR);
// udmabuf0
int fdf = open("/dev/udmabuf0", O_RDWR | O_SYNC); // frame_buffer, The cache is disabled.
if (fdf == -1){
fprintf(stderr, "/dev/udmabuf0 open error\n");
exit(-1);
}
volatile unsigned int *frame_buffer = (volatile unsigned *)mmap(NULL, 3*SVGA_ALL_DISP_ADDRESS, PROT_READ|PROT_WRITE, MAP_SHARED, fdf, 0);
if (!frame_buffer){
fprintf(stderr, "frame_buffer mmap error\n");
exit(-1);
}
// phys_addr of udmabuf0
int fdp = open("/sys/devices/virtual/udmabuf/udmabuf0/phys_addr", O_RDONLY);
if (fdp == -1){
fprintf(stderr, "/sys/devices/virtual/udmabuf/udmabuf0/phys_addr open error\n");
exit(-1);
}
read(fdp, attr, 1024);
sscanf((const char *)attr, "%lx", &phys_addr);
close(fdp);
printf("phys_addr = %x\n", (unsigned int)phys_addr);
// ultrasonic_sensor_inf_0 (UIO 16)
fd16 = open("/dev/uio16", O_RDWR); // ultrasonic_sensor_inf_0 interface AXI4 Lite Slave
if (fd16 < 1){
fprintf(stderr, "/dev/uio16 (ultrasonic_sensor_inf_0) open error\n");
exit(-1);
}
ultrasonic_sensor_inf_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd16, 0);
if (!ultrasonic_sensor_inf_0){
fprintf(stderr, "ultrasonic_sensor_inf_0 mmap error\n");
exit(-1);
}
// main loop
printf("Obstacle avoidance.\n");
printf("if pushed 'q' key then exit.\n");
SetMotorLR(motorL, motorR, 25, 25); // Slow Straight
while(1){
if ((c = getchar()) != EOF){
if (c == 'q'){
SetMotorLR(motorL, motorR, 0, 0); // Stop
break;
}
}
if ((distance=us_sensor_det(ultrasonic_sensor_inf_0)) <= OBSTACLE_AVOID_DISTANCE){
int result = SearchObstacle(&frame_buffer[SVGA_HORIZONTAL_PIXELS*CHECK_LINE_NUMBER], obstacle_size, obstacle_start);
obstacle_center = obstacle_start + obstacle_size/2;
#ifdef DEBUG
printf("obstacle_size = %d, obstacle_start = %d, obstacle_center = %d\n", obstacle_size, obstacle_start, obstacle_center);
#endif
if (distance <= OBSTACLE_AVOID_NEAR_DISTANCE){
if (obstacle_center <= SVGA_HORIZONTAL_PIXELS/2){
debug_print("Strong right turn\n");
SetMotorLR(motorL, motorR, 45, 25); // Strong right turn
} else {
debug_print("Strong left turn\n");
SetMotorLR(motorL, motorR, 20, 45); // Strong left turn
}
} else {
if (obstacle_center <= SVGA_HORIZONTAL_PIXELS/2){
debug_print("Right turn\n");
SetMotorLR(motorL, motorR, 40, 20); // Right turn
} else {
debug_print("Left turn\n");
SetMotorLR(motorL, motorR, 15, 40); // left turn
}
}
} else {
SetMotorLR(motorL, motorR, 25, 25); // Slow Straight
}
}
// Reffered to http://d.hatena.ne.jp/mFumi/20101002/1286003738
tcsetattr(0,TCSANOW,&save_settings);
return(0);
}
よって、そのように修正した。#pragma HLS INTERFACE axis register both port=ins
#pragma HLS INTERFACE axis register both port=outs
// unroll_test.cpp
// 2017/02/17 by marsee
//
#include <hls_stream.h>
#define HORIZONTAL_PIXEL_WIDTH 5
#define VERTICAL_PIXEL_WIDTH 5
int unroll_test(hls::stream<int>& ins, hls::stream<int>& outs){
#pragma HLS INTERFACE s_axilite port=return
#pragma HLS INTERFACE axis register both port=outs
#pragma HLS INTERFACE axis register both port=ins
int pix;
int usm;
int line_buf[2][HORIZONTAL_PIXEL_WIDTH];
#pragma HLS array_partition variable=line_buf block factor=2 dim=1
#pragma HLS resource variable=line_buf core=RAM_2P
int pix_mat[3][3];
#pragma HLS array_partition variable=pix_mat complete
Loop_y : for (int y=0; y<VERTICAL_PIXEL_WIDTH; y++){
Loop_x : for (int x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
//#pragma HLS PIPELINE II=1
ins >> pix;
Loop_i : for (int i=0; i<3; i++){
//#pragma HLS UNROLL factor=3
Loop_j : for (int j=0; j<2; j++){
//#pragma HLS UNROLL factor=2
pix_mat[i][j] = pix_mat[i][j+1];
}
}
pix_mat[0][2] = line_buf[0][x];
pix_mat[1][2] = line_buf[1][x];
pix_mat[2][2] = pix;
line_buf[0][x] = line_buf[1][x]; // 行の入れ替え
line_buf[1][x] = pix;
}
usm = pix_mat[0][0] + pix_mat[0][1] + pix_mat[0][2]
+ pix_mat[1][0] + pix_mat[1][1] + pix_mat[1][2]
+ pix_mat[2][0] + pix_mat[2][1] + pix_mat[2][2];
outs << usm;
}
return(0);
}
// unsharp_mask_axis.cpp
// 2015/09/24 by marsee
// assertテスト用
#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include "unsharp_mask_axis.h"
// アンシャープマスキング・フィルタ
// x0y0 x1y0 x2y0 -k -j -k
// x0y1 x1y1 x2y1 -k 9+8k -k x 1/9
// x0y2 x1y2 x2y2 -k -k -k
//
// k : 鮮鋭化の強さ(固定小数点) , k != 0
// num_adec_k : Kの小数点の位置
// 2015/09/27 : 演算の小数部は num_adec_k*2 ビットとする。
//
#define PRECISION 6 // 小数点以下の桁数、精度(1以上)
int unsharp_masking(int pix_mat[3][3], int k, int num_adec_k)
{
int y;
int xy[3][3];
int result=0;
int z;
int x1y1 = (9<<(PRECISION+num_adec_k))/k + (8<<PRECISION);
for (int i=0; i<=16; i += 8){
for (int j=0; j<3; j++){
for (int k=0; k<3; k++){
xy[j][k] = (pix_mat[j][k] >> i) & 0xff; // RGBのいずれかを抽出
}
}
y = -(xy[0][0]<<PRECISION) -(xy[0][1]<<PRECISION) -(xy[0][2]<<PRECISION)
-(xy[1][0]<<PRECISION) +x1y1*xy[1][1] -(xy[1][2]<<PRECISION)
-(xy[2][0]<<PRECISION) -(xy[2][1]<<PRECISION) -(xy[2][2]<<PRECISION);
y = ((k * y)/9) >> num_adec_k; // k は num_adc_k だけ左シフトされているので戻す
z = y + (1<<(PRECISION-1)); // 四捨五入 +0.5
z = z >> PRECISION; // 小数点以下切り捨て
if (z<0) // 飽和演算
z = 0;
else if (z>255)
z = 255;
result += z<<i; // i=0 : blue, i=8 : green, i=16 : red
}
return(result);
}
assertの挿入箇所が適切ではありません.assertに与えられた条件式はその挿入箇所で評価されますので,被演算対象に対しては値が確定した演算前,代入対象については値が確定した代入後,可及的速やかに適用する必要があります.
// ap_fixed_test.c
// 2017/02/12 by marsee
#include <assert.h>
#include <ap_fixed.h>
typedef ap_fixed<16,8, AP_RND, AP_SAT> ap_xfix_def;
//typedef ap_ufixed<16,8, AP_RND, AP_SAT> ap_xfix_def;
int ap_fixed_test(ap_xfix_def in0, ap_xfix_def in1, int limit,
ap_xfix_def *out){
ap_xfix_def temp;
assert(in0 > (ap_xfix_def)(-1.0) && in0 < (ap_xfix_def)1.0);
temp = in0;
assert(temp > (ap_xfix_def)(-10.0) && temp < (ap_xfix_def)10.0);
//assert(limit <= 10);
for (int i=0; i<limit; i++){
#pragma HLS LOOP_TRIPCOUNT min=1 max=10 avg=2
assert(temp > (ap_xfix_def)(-10.0) && temp < (ap_xfix_def)10.0);
temp = temp * (ap_xfix_def)2.0;
assert(temp > (ap_xfix_def)(-10.0) && temp < (ap_xfix_def)10.0);
}
assert(temp > (ap_xfix_def)(-10.0) && temp < (ap_xfix_def)10.0);
assert(in1 > (ap_xfix_def)(-2.0) && in1 < (ap_xfix_def)2.0);
*out = temp * in1;
return(0);
}
// unsharp_mask_axis.cpp
// 2015/09/24 by marsee
// ap_fixedバージョン 2015/10/04
//
#include <stdio.h>
#include <string.h>
#include <assert.h>
#include <ap_int.h>
#include <ap_fixed.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include <iostream>
#include "unsharp_mask_axis.h"
using namespace std;
// アンシャープマスキング・フィルタ
// x0y0 x1y0 x2y0 -k -j -k
// x0y1 x1y1 x2y1 -k 9+8k -k x 1/9
// x0y2 x1y2 x2y2 -k -k -k
//
// k : 鮮鋭化の強さ(固定小数点) , k != 0
// num_adec_k : Kの小数点の位置
//
int unsharp_masking(int pix_mat[3][3], k_fixed_td k_fixed)
{
ap_ufixed<8, 8, AP_RND, AP_SAT> xy[3][3];
int result=0;
ap_ufixed<8, 8, AP_RND, AP_SAT> z;
x1y1_fixed_td x1y1;
y_fixed_td y;
/*ap_ufixed<8, 8> xy[3][3]; int result=0; ap_ufixed<8, 8, AP_RND, AP_SAT> z; x1y1_fixed_td x1y1; y_fixed_td y;*/
x1y1 = (x1y1_fixed_td)9/(x1y1_fixed_td)k_fixed + (x1y1_fixed_td)8; // ビット長は 9+8 が2^5=32 より小さく、k_fixedで割るので、NUM_ADC_Kが増える可能性がある
for (int i=0; i<=16; i += 8){
for (int j=0; j<3; j++){
for (int k=0; k<3; k++){
xy[j][k] = (pix_mat[j][k] >> i) & 0xff; // RGBのいずれかを抽出
}
}
y = -xy[0][0] -xy[0][1] -xy[0][2]
-xy[1][0] +x1y1*xy[1][1] -xy[1][2]
-xy[2][0] -xy[2][1] -xy[2][2];
assert(y > (y_fixed_td)(-10) && y < (y_fixed_td)(10));
y = (k_fixed * y)/(y_fixed_td)9;
assert(y > (y_fixed_td)(-10) && y < (y_fixed_td)(10));
y = y+(y_fixed_td)0.5; // 四捨五入
assert(y > (y_fixed_td)(-10) && y < (y_fixed_td)(10));
if (y < 0)
z = 0;
else if (y > 255)
z = 255;
else
z = y;
result += z.to_int()<<i; // i=0 : blue, i=8 : green, i=16 : red
}
return(result);
}
// unsharp_mask_axis.h
// 2015/09/26 by marsee
#ifndef __UNSHARP_MASK_AXIS_H_
#define __UNSHARP_MASK_AXIS_H_
//#define HORIZONTAL_PIXEL_WIDTH 1280
//#define VERTICAL_PIXEL_WIDTH 720
#define HORIZONTAL_PIXEL_WIDTH 64
#define VERTICAL_PIXEL_WIDTH 48
#define ALL_PIXEL_VALUE (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)
#define PRECISION 6 // 小数点以下の桁数、精度(0 以上の数を指定する)
#define K_BITLEN 4 // k のビット長
#define NUM_ADC_K 2 // k の小数点の位置
typedef ap_ufixed<K_BITLEN, K_BITLEN-NUM_ADC_K> k_fixed_td;
typedef ap_fixed<6+PRECISION+NUM_ADC_K, (6+PRECISION+NUM_ADC_K)-PRECISION> x1y1_fixed_td;
typedef ap_fixed<6+PRECISION+NUM_ADC_K+8+3, (6+PRECISION+NUM_ADC_K+8+3)-PRECISION> y_fixed_td;
#define K 2.5 // 鮮鋭化の強さ
#endif
// ap_fixed_test.c
// 2017/02/12 by marsee
#include <assert.h>
#include <ap_fixed.h>
typedef ap_fixed<16,8, AP_RND, AP_SAT> ap_xfix_def;
//typedef ap_ufixed<16,8, AP_RND, AP_SAT> ap_xfix_def;
int ap_fixed_test(ap_xfix_def in0, ap_xfix_def in1, int limit,
ap_xfix_def *out){
ap_xfix_def temp;
//assert(in0 > (ap_xfix_def)(-1.0) && in0 < (ap_xfix_def)1.0);
//assert(in1 > (ap_xfix_def)(-2.0) && in1 < (ap_xfix_def)2.0);
//assert(*out > (ap_xfix_def)(-50.0) && *out < (ap_xfix_def)50.0);
//assert(in0 < (ap_xfix_def)1.0);
//assert(in1 < (ap_xfix_def)2.0);
//assert(*out < (ap_xfix_def)50.0);
temp = in0;
//assert(limit <= 10);
for (int i=0; i<limit; i++)
#pragma HLS LOOP_TRIPCOUNT min=1 max=10 avg=2
temp = temp * (ap_xfix_def)2.0;
*out = temp * in1;
return(0);
}
C の assert マクロは、 範囲情報をアサートするのに使用される場合、 合成でサポートされます。 たとえば、 変数 とループ境界の上限を指定でき ます。
// ap_fixed_test.c
// 2017/02/12 by marsee
#include <assert.h>
#include <ap_fixed.h>
int ap_fixed_test(ap_fixed<16,8, AP_RND, AP_SAT> in0, ap_fixed<16,8, AP_RND, AP_SAT> in1, int limit,
ap_fixed<16,8, AP_RND, AP_SAT> *out){
ap_fixed<16,8, AP_RND, AP_SAT> temp;
/*assert(-1 <= in0 <= 1); assert(-2 <= in1 <= 2); assert(-10 <= temp <= 10); assert(-100 <= *out <= 100); */
temp = in0;
//assert(limit <= 10);
for (int i=0; i<limit; i++)
#pragma HLS LOOP_TRIPCOUNT min=1 max=10 avg=2
temp = temp * (ap_fixed<16,8, AP_RND, AP_SAT>)2.0;
*out = temp * in1;
return(0);
}
set_property IOSTANDARD LVCMOS33 [get_ports {cam_data[7]}]
set_property IOSTANDARD LVCMOS33 [get_ports {cam_data[6]}]
set_property IOSTANDARD LVCMOS33 [get_ports {cam_data[5]}]
set_property IOSTANDARD LVCMOS33 [get_ports {cam_data[4]}]
set_property IOSTANDARD LVCMOS33 [get_ports {cam_data[3]}]
set_property IOSTANDARD LVCMOS33 [get_ports {cam_data[2]}]
set_property IOSTANDARD LVCMOS33 [get_ports {cam_data[1]}]
set_property IOSTANDARD LVCMOS33 [get_ports {cam_data[0]}]
set_property IOSTANDARD LVCMOS33 [get_ports mt9d111_iic_scl_io]
set_property IOSTANDARD LVCMOS33 [get_ports mt9d111_iic_sda_io]
set_property IOSTANDARD LVCMOS33 [get_ports href]
set_property IOSTANDARD LVCMOS33 [get_ports pclk]
set_property IOSTANDARD LVCMOS33 [get_ports standby]
set_property IOSTANDARD LVCMOS33 [get_ports vsync]
set_property IOSTANDARD LVCMOS33 [get_ports xck]
set_property CLOCK_DEDICATED_ROUTE FALSE [get_nets pclk_IBUF]
set_property CLOCK_DEDICATED_ROUTE FALSE [get_nets pclk_IBUF_BUFG]
create_clock -period 55.560 -name pclk -waveform {0.000 27.780} [get_ports pclk]
set_input_delay -clock [get_clocks pclk] 10.800 [get_ports {{cam_data[0]} {cam_data[1]} {cam_data[2]} {cam_data[3]} {cam_data[4]} {cam_data[5]} {cam_data[6]} {cam_data[7]} href pclk vsync}]
set_property PULLUP true [get_ports mt9d111_iic_scl_io]
set_property PULLUP true [get_ports mt9d111_iic_sda_io]
set_property PACKAGE_PIN W14 [get_ports {cam_data[7]}]
set_property PACKAGE_PIN V16 [get_ports {cam_data[6]}]
set_property PACKAGE_PIN Y14 [get_ports {cam_data[5]}]
set_property PACKAGE_PIN W16 [get_ports {cam_data[4]}]
set_property PACKAGE_PIN T11 [get_ports {cam_data[3]}]
set_property PACKAGE_PIN V12 [get_ports {cam_data[2]}]
set_property PACKAGE_PIN T10 [get_ports {cam_data[1]}]
set_property PACKAGE_PIN W13 [get_ports {cam_data[0]}]
set_property IOSTANDARD TMDS_33 [get_ports TMDS_tx_0_B_p]
set_property IOSTANDARD TMDS_33 [get_ports TMDS_tx_1_R_p]
set_property IOSTANDARD TMDS_33 [get_ports TMDS_tx_2_G_p]
set_property IOSTANDARD TMDS_33 [get_ports TMDS_tx_clk_p]
set_property PACKAGE_PIN U19 [get_ports href]
set_property PACKAGE_PIN Y16 [get_ports standby]
set_property PACKAGE_PIN K17 [get_ports TMDS_tx_0_B_p]
set_property PACKAGE_PIN K19 [get_ports TMDS_tx_1_R_p]
set_property PACKAGE_PIN J18 [get_ports TMDS_tx_2_G_p]
set_property PACKAGE_PIN L16 [get_ports TMDS_tx_clk_p]
set_property PACKAGE_PIN Y19 [get_ports vsync]
set_property PACKAGE_PIN W19 [get_ports xck]
set_property PACKAGE_PIN Y18 [get_ports mt9d111_iic_scl_io]
set_property PACKAGE_PIN U18 [get_ports mt9d111_iic_sda_io]
set_property PACKAGE_PIN Y17 [get_ports pclk]
set_false_path -from [get_clocks pclk] -to [get_clocks clk_fpga_0]
set_false_path -from [get_clocks clk_fpga_0] -to [get_clocks pclk]
set_false_path -from [get_clocks [get_clocks -of_objects [get_pins pynq_fastx_i/bitmap_disp_cntrler_axi_master_0/inst/dvi_disp_i/BUFR_pixel_clk_io/O]]] -to [get_clocks clk_fpga_0]
コマンドを入力し、pynq_fastx ブロックデザインを生成した。cd z:/PYNQ_FASTX_164/
source pynq_fastx.tcl
高位合成ツールVivado HLS 特設記事
と第1章 【画像フィルタをハードウェア化!】無料のVivado HL WebPACK Editionで高位合成にチャレンジしよう(AXI4マスタ編)
アルゴリズム通りに記述したフィルタCプログラムをハードウェア化してみよう P.78
の記事中のソースコードと画像です。第2章 【ステップアップで高速化!】
無料のVivado HL WebPACK Editionで高位合成にチャレンジしよう(AXI4マスタ編)
ライン・バッファ/バースト転送/最適化指示子を駆使した高速化テクニック P.94
// fastx_2.cpp
// 2016/04/02 by marsee
// 2016/04/09 : FAST Corners Detection
// 2017/02/03 : 元の写真を入れないで、コーナー検出の点だけ表示した
#include "fastx.h"
void fastx_corner_det(AXI_STREAM& INPUT_STREAM, AXI_STREAM& OUTPUT_STREAM, int rows, int cols, int threshold) {
#pragma HLS INTERFACE ap_stable port=threshold
#pragma HLS INTERFACE s_axilite port=threshold
#pragma HLS DATAFLOW
#pragma HLS INTERFACE ap_stable port=cols
#pragma HLS INTERFACE ap_stable port=rows
#pragma HLS INTERFACE s_axilite port=return
#pragma HLS INTERFACE axis port=OUTPUT_STREAM
#pragma HLS INTERFACE axis port=INPUT_STREAM
#pragma HLS INTERFACE s_axilite port=cols
#pragma HLS INTERFACE s_axilite port=rows
RGB_IMAGE img_0(rows, cols);
RGB_IMAGE img_1(rows, cols);
RGB_IMAGE img_1_(rows, cols);
#pragma HLS STREAM variable=img_1_.data_stream depth=8192
// FASTX に最大 7 ラインのレイテンシ、Dilate に最大 3 ラインのレイテンシがあるそうだ
// 1ラインのピクセル数X10 ラインのFIFO バッファが必要 800x10 < 8192 (2の13乗)
// http://japan.xilinx.com/support/documentation/application_notes/xapp1167.pdf
// の 10 ページ参照
GRAY_IMAGE img_1g(rows, cols);
GRAY_IMAGE mask(rows, cols);
GRAY_IMAGE dmask(rows, cols);
GRAY_IMAGE img_2g(rows, cols);
RGB_IMAGE img_3(rows, cols);
RGB_PIXEL color(255, 0, 0);
hls::AXIvideo2Mat(INPUT_STREAM, img_0);
hls::Duplicate(img_0, img_1, img_1_);
hls::CvtColor<HLS_BGR2GRAY>(img_1, img_1g);
hls::FASTX(img_1g, mask, threshold, true);
hls::Dilate(mask, dmask);
//hls::PaintMask(img_1_, dmask, img_3, color);
hls::CvtColor<HLS_GRAY2BGR>(dmask, img_3);
hls::Mat2AXIvideo(img_3, OUTPUT_STREAM);
}
// 2016/04/03 : GRAY_IMAGE を追加
#ifndef __FASTX_H__
#define __FASTX_H__
#include "ap_axi_sdata.h"
#include "hls_video.h"
#define MAX_HEIGHT 600
#define MAX_WIDTH 800
typedef hls::stream<ap_axiu<32,1,1,1> > AXI_STREAM;
typedef hls::Scalar<3, unsigned char> RGB_PIXEL;
typedef hls::Mat<MAX_HEIGHT, MAX_WIDTH, HLS_8UC3> RGB_IMAGE;
typedef hls::Mat<MAX_HEIGHT, MAX_WIDTH, HLS_8UC1> GRAY_IMAGE;
#endif
// fastx.cpp
// 2016/04/02 by marsee
// 2016/04/09 : FAST Corners Detection
#include "fastx.h"
void fastx_corner_det(AXI_STREAM& INPUT_STREAM, AXI_STREAM& OUTPUT_STREAM, int rows, int cols, int threshold) {
#pragma HLS INTERFACE ap_stable port=threshold
#pragma HLS INTERFACE s_axilite port=threshold
#pragma HLS DATAFLOW
#pragma HLS INTERFACE ap_stable port=cols
#pragma HLS INTERFACE ap_stable port=rows
#pragma HLS INTERFACE s_axilite port=return
#pragma HLS INTERFACE axis port=OUTPUT_STREAM
#pragma HLS INTERFACE axis port=INPUT_STREAM
#pragma HLS INTERFACE s_axilite port=cols
#pragma HLS INTERFACE s_axilite port=rows
RGB_IMAGE img_0(rows, cols);
RGB_IMAGE img_1(rows, cols);
RGB_IMAGE img_1_(rows, cols);
#pragma HLS STREAM variable=img_1_.data_stream depth=8192
// FASTX に最大 7 ラインのレイテンシ、Dilate に最大 3 ラインのレイテンシがあるそうだ
// 1ラインのピクセル数X10 ラインのFIFO バッファが必要 800x10 < 8192 (2の13乗)
// http://japan.xilinx.com/support/documentation/application_notes/xapp1167.pdf
// の 10 ページ参照
GRAY_IMAGE img_1g(rows, cols);
GRAY_IMAGE mask(rows, cols);
GRAY_IMAGE dmask(rows, cols);
GRAY_IMAGE img_2g(rows, cols);
RGB_IMAGE img_3(rows, cols);
RGB_PIXEL color(255, 0, 0);
hls::AXIvideo2Mat(INPUT_STREAM, img_0);
hls::Duplicate(img_0, img_1, img_1_);
hls::CvtColor<HLS_BGR2GRAY>(img_1, img_1g);
hls::FASTX(img_1g, mask, threshold, true);
hls::Dilate(mask, dmask);
hls::PaintMask(img_1_, dmask, img_3, color);
hls::Mat2AXIvideo(img_3, OUTPUT_STREAM);
}
// fastx_tb.cpp
// 2016/04/02 by marsee
// OpenCV 2 の Mat を使用したバージョン
// 2016/04/09 : FAST Corners Detection
#include <iostream>
#include "hls_opencv.h"
#include "fastx.h"
using namespace cv;
#define INPUT_IMAGE "test.jpg"
#define OUTPUT_IMAGE "test_result.jpg"
#define OUTPUT_IMAGE_CV "test_result_cv.jpg"
#define THESHOLD_LEVEL 60
void fastx_corner_det(AXI_STREAM& INPUT_STREAM, AXI_STREAM& OUTPUT_STREAM, int rows, int cols, int threshold);
void opencv_fastx_corner_det(Mat& src, Mat& dst, int threshold);
int main (int argc, char** argv) {
// OpenCV で 画像を読み込む
Mat src = imread(INPUT_IMAGE);
AXI_STREAM src_axi, dst_axi;
// Mat フォーマットから AXI4 Stream へ変換
cvMat2AXIvideo(src, src_axi);
// fastx_corner_det() 関数をコール
fastx_corner_det(src_axi, dst_axi, src.rows, src.cols, THESHOLD_LEVEL);
// AXI4 Stream から Mat フォーマットへ変換
// dst は宣言時にサイズとカラー・フォーマットを定義する必要がある
Mat dst(src.rows, src.cols, CV_8UC3);
AXIvideo2cvMat(dst_axi, dst);
// Mat フォーマットからファイルに書き込み
imwrite(OUTPUT_IMAGE, dst);
// opencv_fastx_corner_det() をコール
Mat dst_cv(src.rows, src.cols, CV_8UC3);
opencv_fastx_corner_det(src, dst_cv, THESHOLD_LEVEL);
imwrite(OUTPUT_IMAGE_CV, dst_cv);
// dst と dst_cv が同じ画像かどうか?比較する
for (int y=0; y<src.rows; y++){
Vec3b* dst_ptr = dst.ptr<Vec3b>(y);
Vec3b* dst_cv_ptr = dst_cv.ptr<Vec3b>(y);
for (int x=0; x<src.cols; x++){
Vec3b dst_bgr = dst_ptr[x];
Vec3b dst_cv_bgr = dst_cv_ptr[x];
// bgr のどれかが間違っていたらエラー
if (dst_bgr[0] != dst_cv_bgr[0] || dst_bgr[1] != dst_cv_bgr[1] || dst_bgr[2] != dst_cv_bgr[2]){
printf("x = %d, y = %d, Error dst=%d,%d,%d dst_cv=%d,%d,%d\n", x, y,
dst_bgr[0], dst_bgr[1], dst_bgr[0], dst_cv_bgr[0], dst_cv_bgr[1], dst_cv_bgr[2]);
//return 1;
}
}
}
printf("Test with 0 errors.\n");
return 0;
}
void opencv_fastx_corner_det(Mat& src, Mat& dst, int threshold){
src.copyTo(dst); // 深いコピー
std::vector<Mat> layers;
std::vector<KeyPoint> keypoints;
split(src, layers);
FAST(layers[0], keypoints, threshold, true);
for (int i = 0; i < keypoints.size(); i++) {
rectangle(dst,
Point(keypoints[i].pt.x-1, keypoints[i].pt.y-1),
Point(keypoints[i].pt.x+1, keypoints[i].pt.y+1),
Scalar(255,0), CV_FILLED);
}
}
//.din(data_in), // Bus [63 : 0]
.din({data_in[31:0], data_in[63:32]}), // Bus [63 : 0]
// fastx_on_serial.c
// 2017/04/13 by marsee
//
// Refered to http://japan.xilinx.com/support/documentation/sw_manuals_j/xilinx2014_4/ug902-vivado-high-level-synthesis.pdf
//
// 2016/04/21 : threshold をシリアル経由で入力する
// 2017/01/30 : すべての処理を記述するように変更
//
#include <stdio.h>
#include <stdlib.h>
#include "xfastx_corner_det.h"
#include "xparameters.h"
#include "xaxivdma.h"
#include "xil_io.h"
#include "sleep.h"
#define NUMBER_OF_WRITE_FRAMES 3 // Note: If not at least 3 or more, the image is not displayed in succession.
#define HORIZONTAL_PIXELS 800
#define VERTICAL_LINES 600
#define PIXEL_NUM_OF_BYTES 4
#define FASTX_THRESHOLD 20
#define FRAME_BUFFER_ADDRESS 0x10000000
static XAxiVdma_DmaSetup Vdma0_WriteCfg;
void cam_i2c_init(volatile unsigned *mt9d111_i2c_axi_lites) {
mt9d111_i2c_axi_lites[64] = 0x2; // reset tx fifo ,address is 0x100, i2c_control_reg
mt9d111_i2c_axi_lites[64] = 0x1; // enable i2c
}
void cam_i2x_write_sync(void) {
// unsigned c;
// c = *cam_i2c_rx_fifo;
// while ((c & 0x84) != 0x80)
// c = *cam_i2c_rx_fifo; // No Bus Busy and TX_FIFO_Empty = 1
usleep(1000);
}
void cam_i2c_write(volatile unsigned *mt9d111_i2c_axi_lites, unsigned int device_addr, unsigned int write_addr, unsigned int write_data){
mt9d111_i2c_axi_lites[66] = 0x100 | (device_addr & 0xfe); // Slave IIC Write Address, address is 0x108, i2c_tx_fifo
mt9d111_i2c_axi_lites[66] = write_addr;
mt9d111_i2c_axi_lites[66] = (write_data >> 8)|0xff; // first data
mt9d111_i2c_axi_lites[66] = 0x200 | (write_data & 0xff); // second data
cam_i2x_write_sync();
}
int main(){
XFastx_corner_det Ximage_filter;
XFastx_corner_det_Config *Ximage_filterPtr;
XAxiVdma_Config *XAxiVdma0_Config;
XAxiVdma XAxiVdma0;
int XAxiVdma0_Status;
int threshold;
XAxiVdma0_Config = XAxiVdma_LookupConfig(XPAR_CAMERA_INTERFACE_AXI_VDMA_0_DEVICE_ID); // Look up the hardware configuration for a device instance
if (XAxiVdma0_Config == NULL){
fprintf(stderr, "No AXI VDMA found\n");
return(-1);
}
XAxiVdma0_Status = XAxiVdma_CfgInitialize(&XAxiVdma0, XAxiVdma0_Config, XAxiVdma0_Config->BaseAddress); // Initialize the driver with hardware configuration
if (XAxiVdma0_Status != XST_SUCCESS){
fprintf(stderr, "XAxiVdma_CfgInitialize() failed\n");
return(-1);
}
XAxiVdma_Reset(&XAxiVdma0, XAXIVDMA_WRITE);
while(XAxiVdma_ResetNotDone(&XAxiVdma0, XAXIVDMA_WRITE)) ;
XAxiVdma0_Status = XAxiVdma_SetFrmStore(&XAxiVdma0, NUMBER_OF_WRITE_FRAMES, XAXIVDMA_WRITE); // Set the number of frame store buffers to use.
Vdma0_WriteCfg.VertSizeInput = VERTICAL_LINES;
Vdma0_WriteCfg.HoriSizeInput = HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES;
Vdma0_WriteCfg.Stride = HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES; // Indicates the number of address bytes between the first pixels of each video line.
Vdma0_WriteCfg.FrameDelay = 0; // Indicates the minimum number of frame buffers the Genlock slave is to be behind the locked master. This field is only used if the channel is enabled for Genlock Slave operations. This field has no meaning in other Genlock modes.
Vdma0_WriteCfg.EnableCircularBuf = 1; // Indicates frame buffer Circular mode or frame buffer Park mode. 1 = Circular Mode Engine continuously circles through frame buffers.
Vdma0_WriteCfg.EnableSync = 0; // Enables Genlock or Dynamic Genlock Synchronization. 0 = Genlock or Dynamic Genlock Synchronization disabled.
Vdma0_WriteCfg.PointNum = 0; // No Gen-Lock
Vdma0_WriteCfg.EnableFrameCounter = 0; // Endless transfers
Vdma0_WriteCfg.FixedFrameStoreAddr = 0; // We are not doing parking
XAxiVdma0_Status = XAxiVdma_DmaConfig(&XAxiVdma0, XAXIVDMA_WRITE, &Vdma0_WriteCfg);
if (XAxiVdma0_Status != XST_SUCCESS){
fprintf(stderr, "XAxiVdma_DmaConfig() failed\n");
return(-1);
}
// Frame buffer address set
unsigned int frame_addr = (unsigned int)FRAME_BUFFER_ADDRESS;
int i;
for (i=0; i<NUMBER_OF_WRITE_FRAMES; i++){
Vdma0_WriteCfg.FrameStoreStartAddr[i] = frame_addr;
//frame_addr += HORIZONTAL_PIXELS * PIXEL_NUM_OF_BYTES * VERTICAL_LINES;
}
XAxiVdma0_Status = XAxiVdma_DmaSetBufferAddr(&XAxiVdma0, XAXIVDMA_WRITE, Vdma0_WriteCfg.FrameStoreStartAddr);
if (XAxiVdma0_Status != XST_SUCCESS){
fprintf(stderr, "XAxiVdma_DmaSetBufferAddr() failed\n");
return(-1);
}
// Look Up the device configuration
Ximage_filterPtr = XFastx_corner_det_LookupConfig(0);
if (!Ximage_filterPtr){
fprintf(stderr, "XFastx_corner_det configuration failed.\n");
return(-1);
}
// Initialize the Device
int Xlap_status = XFastx_corner_det_CfgInitialize(&Ximage_filter, Ximage_filterPtr);
if (Xlap_status != XST_SUCCESS){
fprintf(stderr, "Could not Initialize XFastx_corner_det\n");
return(-1);
}
// image_filter rows, cols, threshold=20 set
XFastx_corner_det_Set_cols(&Ximage_filter, (u32)800);
XFastx_corner_det_Set_rows(&Ximage_filter, (u32)600);
XFastx_corner_det_Set_threshold(&Ximage_filter, (u32)FASTX_THRESHOLD);
// axis_switch_1, 1to2 ,Select M01_AXIS
// Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x40), 0x80000000); // disable
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR+0x44), 0);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR), 0x2); // Commit registers
// fastx filter AXIS Start
XFastx_corner_det_Start(&Ximage_filter);
XFastx_corner_det_EnableAutoRestart(&Ximage_filter);
// axis_switch_0, 2to1, Select S01_AXIS
// Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR+0x40), 0x1);
Xil_Out32((XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR), 0x2); // Commit registers
// VDMA start
XAxiVdma0_Status = XAxiVdma_DmaStart(&XAxiVdma0, XAXIVDMA_WRITE);
if (XAxiVdma0_Status != XST_SUCCESS){
fprintf(stderr, "XAxiVdma_DmaStart() failed\n");
return(-1);
}
// mt9d111_inf_axis_0, axi_iic_0, bitmap_disp_cntrler_axi_master_0
volatile unsigned int *bmdc_axi_lites;
volatile unsigned int *mt9d111_axi_lites;
volatile unsigned int *mt9d111_i2c_axi_lites;
bmdc_axi_lites = (volatile unsigned *)XPAR_BITMAP_DISP_CNTRLER_AXI_MASTER_0_BASEADDR;
mt9d111_axi_lites = (volatile unsigned *)XPAR_CAMERA_INTERFACE_MT9D111_INF_AXIS_0_BASEADDR;
mt9d111_i2c_axi_lites = (volatile unsigned *)XPAR_CAMERA_INTERFACE_AXI_IIC_0_BASEADDR;
bmdc_axi_lites[0] = (volatile unsigned int)FRAME_BUFFER_ADDRESS; // Bitmap Display Controller start
mt9d111_axi_lites[0] = (volatile unsigned int)FRAME_BUFFER_ADDRESS; // Camera Interface start (Address is dummy)
// CMOS Camera initialize, MT9D111
cam_i2c_init(mt9d111_i2c_axi_lites);
cam_i2c_write(mt9d111_i2c_axi_lites, 0xba, 0xf0, 0x1); // Changed regster map to IFP page 1
cam_i2c_write(mt9d111_i2c_axi_lites, 0xba, 0x97, 0x20); // RGB Mode, RGB565
mt9d111_axi_lites[1] = 0; // One_shot_mode is disabled
// threshold set
while(1){
printf("\n\rthreshold = ");
scanf("%d", &threshold);
if (threshold == 999)
break;
XFastx_corner_det_Set_threshold(&Ximage_filter, (u32)threshold);
}
return(0);
}
日 | 月 | 火 | 水 | 木 | 金 | 土 |
---|---|---|---|---|---|---|
- | - | - | 1 | 2 | 3 | 4 |
5 | 6 | 7 | 8 | 9 | 10 | 11 |
12 | 13 | 14 | 15 | 16 | 17 | 18 |
19 | 20 | 21 | 22 | 23 | 24 | 25 |
26 | 27 | 28 | - | - | - | - |