float pid::loop_once(float err, float err_dot) {
calc_dt();
err_sum += err * dt;
if((err_sum > max_err_sum) || (err_sum < (-1 * max_err_sum))) {
err_sum = max_err_sum;
}
float command = gain_kp * err + gain_kd * err_dot + gain_ki * err_sum;
return command;
}
int main(int argc, char *argv[])
{
int dev, ret_val;
unsigned int reg_val;
// find and init FPGA device
ret_val = fpga_init(argc, argv, &dev);
if (ret_val<0) return -1;
double dt_c[N];
double dt_f[N];
printf("N_d is %d\n",N_d);
printf("N_d is %d\n",N_d);
printf("size is %d\n",(int)SIZE_DWORD);
try{
x_1.resize(13);
y_1.resize(13);
t_1.resize(13);
x_2.resize(13);
y_2.resize(13);
t_2.resize(13);
for (int i = 0; i < 13; i++){
x_1[i].resize(N_d);
y_1[i].resize(N_d);
t_1[i].resize(N_d);
x_2[i].resize(N_d);
y_2[i].resize(N_d);
t_2[i].resize(N_d);
}
} catch (const std::bad_alloc& ba){
std::cout << "bad_alloc caught: " << ba.what() << std::endl;
}
FILE *f_p, *f_p1;
int error = 0;
if (flag_compare==1){
f_p = fopen ("MT_coords_CPU.txt","w");
if (f_p==NULL) {
printf("Error opening file!\n");
return -1;
}
}
if (flag_file) f_p1 = fopen (out_file,"w");
else f_p1 = fopen ("MT_coords_FPGA.txt","w");
if (f_p1==NULL) {
printf("Error opening file!\n");
return -1;
}
printf("TOTAL_STEPS = %d\nSTEPS_TO_WRITE = %d\n", TOTAL_STEPS, STEPS_TO_WRITE);
// get golden results
init_coords(x_1,y_1,t_1);
init_coords(x_2,y_2,t_2);
/*
* в этом цикле проводим вычислени¤ и сравниваем результаты
*
* в данный момент чтобы проверить сравнение на каждом шаге вызываетс¤ функци¤ mt_cpu
* с параметром load_coords = 1 (иначе состо¤ни¤ глобальных массивов координат будет все врем¤ мен¤тьс¤)
*
* огда вместо mt_cpu будет реализаци¤ на OpenCL, то надо делать по-другому:
* перед циклом один раз вызываем mt_cpu (load_coords = 1), в цикле уменьшаем количество итераций на 1 и
* вызываем mt_cpu (load_coords = 0)
*
*/
error = 0;
printf("\nFlag rand is SET, %d\n\n",flag_rand);
if (flag_rand==1) {
srand (time(NULL));
// set seed vals
for (int i =0; i < NUM_SEEDS; i++){
#ifdef TEST_SEEDS
seeds[i] = test_seeds[i];
#else
seeds[i]=rand();
#endif
unsigned int addr = SEED_REG + 4*i;
RD_WriteDeviceReg32m(dev, CNTRL_BAR, addr, seeds[i]);
}
printf("\nFlag rand is SET\n\n");
RD_ReadDeviceReg32m(dev, CNTRL_BAR, COMMAND_REG, reg_val);
// deassert rand reset
reg_val |= (1<<4);
RD_WriteDeviceReg32m(dev, CNTRL_BAR, COMMAND_REG, reg_val);
// set rand_enable flag
reg_val |= (1<<7);
RD_WriteDeviceReg32m(dev, CNTRL_BAR, COMMAND_REG, reg_val);
// start rand core
reg_val |= (1<<5);
RD_WriteDeviceReg32m(dev, CNTRL_BAR, COMMAND_REG, reg_val);
}
printf("\n\nhereerereerere\n\n");
for(int k=0; k<N; k++) {
int err;
struct timeval tt1, tt2;
if (flag_compare==1){
get_time(&tt1);
if (mt_cpu(STEPS_TO_WRITE,1, flag_rand, flag_seed, seeds, x_1,y_1,t_1,x_1,y_1, t_1)<0) { printf("Nan Error in cpu. Step is %d. Exitting....\n",k); break;}
get_time(&tt2);
calc_dt(&tt1,&tt2, &dt_c[k]);
}
get_time(&tt1);
mt_fpga(dev,STEPS_TO_WRITE,1,x_2,y_2,t_2,x_2,y_2, t_2);
get_time(&tt2);
calc_dt(&tt1,&tt2, &dt_f[k]);
flag_seed = 0;
printf("Step %d\n\t CPU Time = %f\n\t FPGA Time = %f\n",k,dt_c[k],dt_f[k] );
if (flag_compare==1){
err = compare_results(x_1,y_1,t_1,x_2,y_2,t_2);
if (err) {
error += err;
printf("Compare results failed at step = %d, errors = %d\n", k, error);
}
}
if (flag_compare==1) print_coords(f_p, x_1, y_1, t_1);
print_coords(f_p1, x_2, y_2, t_2);
}
if (flag_compare==1){
if (!error)
printf("Test OK!\n");
}
if (flag_compare==1) fclose(f_p);
fclose(f_p1);
RD_ReadDeviceReg32m(dev, CNTRL_BAR, COMMAND_REG, reg_val);
// assert rand reset
reg_val &= ~(1<<4);
RD_WriteDeviceReg32m(dev, CNTRL_BAR, COMMAND_REG, reg_val);
// reset rand_enable flag
reg_val &= ~(1<<7);
RD_WriteDeviceReg32m(dev, CNTRL_BAR, COMMAND_REG, reg_val);
free(wr_buf_free);
free(rd_buf_free);
RD_CloseDevice(pd_ptr);
return 0;
}