void execute_state_entry(state_t state)
{
switch(state)
{
case READY_STATE:
LCD_clear();
LCD_putsub("Wez go \ndo buzi");
break;
case DRINKING_STATE:
led_on();
LCD_clear();
start_time_measurement();
break;
case VERIFICATION_STATE:
verified_time = *get_current_time();
LCD_display_time(&verified_time);
verification_state_duration = 0u;
break;
case DISPLAY_STATE:
stop_time_measurement();
//led_on();
led_flashing_duration = 0u;
//LCD_display_time(get_current_time()); // drinking time end display
break;
case ADC_STATE:
LCD_clear();
break;
}
state_is_new = 0u;
}
int main(int argc, char *argv[])
{
FILE *mat;
FILE *vec;
FILE *end;
double main_buf[MAXSIZE];
double main_vec_buf[MAXSIZE];
Cnum mat_buf[MAXSIZE];
Cnum vec_buf[MAXSIZE];
Cnum result[MAXSIZE];
double Row=0, Col=0;
double vec_row;
double mat_RC_data[6];
long mat_in=0;
int j=0;
mat = fopen("matrix.dat","rb");
vec = fopen("vector_input.dat","rb");
end = fopen("vector_output.dat","wb");
// exe input number 검사
if (argc == 4 && atoi(argv[1]) && atoi(argv[2]))
printf("input = %d x %d, Thread = %d \n",atoi(argv[1]), atoi(argv[2]),atoi(argv[3]));
else
{
fputs("[Error] Not a correct input", stderr);
exit(1);
}
// 파일 유무 검사
if(mat==NULL)
{
fputs("Matrix File error", stderr);
exit(1);
}
if(vec==NULL)
{
fputs("Vector File error", stderr);
exit(1);
}
start_time_measurement(); //시간측정 시작
// 매트릭스 크기
int point;
for(int i=3;i>0;i--)
{
point = i*4*sizeof(double);
point = -point;
if(fseek(mat, point, SEEK_END)==-1)
{
printf("error\n");
}
fread(&mat_RC_data[j],sizeof(double),2,mat);
Row=mat_RC_data[j]+1;
j++;
Col=mat_RC_data[j]+1;
j++;
printf("row : %f, col : %f\n",Row,Col);
}
/*
if(( (mat_RC_data[0]) != (mat_RC_data[2]) != (mat_RC_data[4]) ))
{
fputs("[ERROR] Matrix File ROW_DATA error", stderr);
exit(1);
}
if(mat_RC_data[1]+2!=mat_RC_data[3]+1!=mat_RC_data[5])
{
fputs("[ERROR] Matrix File COL_DATA error", stderr);
exit(1);
}
*/
// matrix.dat 행 열값 부분 오류 검사
if(mat_RC_data[4]-mat_RC_data[2]>1)
{
fputs("[ERROR] Matrix File ROW_DATA error", stderr);
exit(1);
}
if(mat_RC_data[2]-mat_RC_data[0]>1)
{
fputs("[ERROR] Matrix File ROW_DATA error", stderr);
exit(1);
}
if(mat_RC_data[5]-mat_RC_data[3]>2)
{
fputs("[ERROR] Matrix File ROW_DATA error", stderr);
exit(1);
}
if(mat_RC_data[3]-mat_RC_data[1]>2)
{
fputs("[ERROR] Matrix File ROW_DATA error", stderr);
exit(1);
}
printf("전체 열의 값 = %f\n전체 행의 값 = %f \n", Row, Col);
//vec 행렬 행 크기
fseek(vec,0,SEEK_END);
vec_row = ftell(vec)/(sizeof(double)*2);
printf("벡터행렬 행 값 = %f\n",vec_row);
//행렬 크기가 다를 시 오류 메세지 출력
if(Col!=vec_row)
{
fputs("[ERROR] Matrix and Vector are not same", stderr);
exit(1);
}
//행렬값 꺼내서 연산
j=0;
int button=0;
fseek(mat,0,SEEK_SET); //파일 포인터 위치 초기화
fseek(vec,0,SEEK_SET); //파일 포인터 위치 초기화
fseek(end,0,SEEK_SET); //파일 포인터 위치 초기화
for(int k=0; k<Row; k++)
{
//인덱스 초기화
button =0;
j=0;
fseek(vec,0,SEEK_SET); //파일 포인터 위치 초기화
//mat 행렬 가져와서 mat_buf[]에 저장 100개씩 -> ////vec 행의 갯수에 따라로 수정해야함 //// 일단은 100개단위로
for(int i=0; i<Col*4; i++)
{
fread(&main_buf[i], sizeof(double), 1, mat);
//printf("값 : %f ", main_buf[i]);
if(i>3 && i%4==0)
j++;
// 구조체에 값저장
if(button==2)
mat_buf[j].realnum=main_buf[i];
else if(button==3)
{
mat_buf[j].imanum=main_buf[i];
}
button++;
if(button==4)
button=0;
}
//vec 행렬 가져와서 vec_buf[]에 저장
j=0;
for(int y=0; y<Col*2; y++)
{
fread(&main_vec_buf[y],sizeof(double),1,vec);
if(y>1&&y%2==0)
j++;
if(y%2==0)
vec_buf[j].realnum = main_vec_buf[y];
else
{
vec_buf[j].imanum = main_vec_buf[y];
}
}
// mat * vec
for(int i=0; i<Col; i++)
{
result[i]=mult(mat_buf[i],vec_buf[i]);
}
for(int i=1; i<Col; i++)
{
result[0]=sum(result[0],result[i]);
}
fwrite(&result[0].realnum,sizeof(double),1,end);
fwrite(&result[0].imanum,sizeof(double),1,end);
}
printf("Calculation Complete ");
end_time_measurement(); //시간 측정 끝
fclose(mat);
fclose(vec);
fclose(end);
return 0;
}
void Navi6dWrapper::update(const baro_data_t &baro,
const odometer_data_t &odo,
const marg_data_t &marg) {
osalDbgCheck(ready);
start_time_measurement();
/* restart sins if requested */
if (*restart != restart_cache) {
sins_cold_start();
restart_cache = *restart;
}
nav_sins.init_params.dT = marg.dT;
nav_sins.kalman_params.gnss_mean_pos_sigma = *R_pos_sns;
nav_sins.kalman_params.gnss_mean_alt_sigma = *R_alt_sns;
nav_sins.kalman_params.gnss_mean_vel_sigma = *R_vel_sns;
nav_sins.kalman_params.sigma_R.v_odo_x = *R_odo; //odo
nav_sins.kalman_params.sigma_R.v_nhl_y = *R_nhl_y; //nonhol
nav_sins.kalman_params.sigma_R.v_nhl_z = *R_nhl_z; //nonhol
nav_sins.kalman_params.sigma_R.alt_baro = *R_baro; //baro
nav_sins.kalman_params.sigma_R.mag_x = *R_mag; //mag
nav_sins.kalman_params.sigma_R.mag_y = *R_mag; //mag
nav_sins.kalman_params.sigma_R.mag_z = *R_mag; //mag
nav_sins.kalman_params.sigma_R.roll = *R_euler; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.pitch = *R_euler; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.yaw = *R_euler; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.v_nav_static = *R_v_nav_st; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.v_veh_static = *R_v_veh_st; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.yaw_static = *R_yaw_st; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_R.yaw_mag = *R_mag_yaw; //roll,pitch,yaw (rad)
nav_sins.kalman_params.sigma_Qm.acc_x = *Qm_acc; //acc
nav_sins.kalman_params.sigma_Qm.acc_y = *Qm_acc; //acc
nav_sins.kalman_params.sigma_Qm.acc_z = *Qm_acc; //accbias
nav_sins.kalman_params.sigma_Qm.gyr_x = *Qm_gyr; //gyr
nav_sins.kalman_params.sigma_Qm.gyr_y = *Qm_gyr; //gyr
nav_sins.kalman_params.sigma_Qm.gyr_z = *Qm_gyr; //gyr
nav_sins.kalman_params.sigma_Qm.acc_b_x = *Qm_acc_bias; //acc_x
nav_sins.kalman_params.sigma_Qm.acc_b_y = *Qm_acc_bias; //acc_y
nav_sins.kalman_params.sigma_Qm.acc_b_z = *Qm_acc_bias; //acc_z
nav_sins.kalman_params.sigma_Qm.gyr_b_x = *Qm_gyr_bias; //gyr_bias
nav_sins.kalman_params.sigma_Qm.gyr_b_y = *Qm_gyr_bias; //gyr_bias
nav_sins.kalman_params.sigma_Qm.gyr_b_z = *Qm_gyr_bias; //gyr_bias
nav_sins.kalman_params.Beta_inv.acc_b = *B_acc_b;
nav_sins.kalman_params.Beta_inv.gyr_b = *B_gyr_b;
nav_sins.kalman_params.Beta_inv.acc_s = 10000000.0;
nav_sins.kalman_params.Beta_inv.gyr_s = 10000000.0;
nav_sins.kalman_params.Beta_inv.acc_no = 10000000.0;
nav_sins.kalman_params.Beta_inv.gyr_no = 10000000.0;
nav_sins.calib_params.ba[0][0] = *acc_bias_x;
nav_sins.calib_params.ba[1][0] = *acc_bias_y;
nav_sins.calib_params.ba[2][0] = *acc_bias_z;
nav_sins.calib_params.bw[0][0] = *gyr_bias_x;
nav_sins.calib_params.bw[1][0] = *gyr_bias_y;
nav_sins.calib_params.bw[2][0] = *gyr_bias_z;
nav_sins.calib_params.sa[0][0] = *acc_scale_x;
nav_sins.calib_params.sa[1][0] = *acc_scale_y;
nav_sins.calib_params.sa[2][0] = *acc_scale_z;
nav_sins.calib_params.sw[0][0] = *gyr_scale_x;
nav_sins.calib_params.sw[1][0] = *gyr_scale_y;
nav_sins.calib_params.sw[2][0] = *gyr_scale_z;
nav_sins.ref_params.mag_dec = deg2rad(*mag_declinate);
nav_sins.calib_params.bm_marg[0][0] = *mag_bias_x;
nav_sins.calib_params.bm_marg[1][0] = *mag_bias_y;
nav_sins.calib_params.bm_marg[2][0] = *mag_bias_z;
nav_sins.calib_params.sm_marg[0][0] = *mag_scale_x;
nav_sins.calib_params.sm_marg[1][0] = *mag_scale_y;
nav_sins.calib_params.sm_marg[2][0] = *mag_scale_z;
nav_sins.calib_params.no_m_marg[0][0] = *mag_nort_x;
nav_sins.calib_params.no_m_marg[1][0] = *mag_nort_y;
nav_sins.calib_params.no_m_marg[2][0] = *mag_nort_z;
/*
nav_sins.calib_params.no_a[0][0] = *acc_nort_0;
nav_sins.calib_params.no_a[1][0] = *acc_nort_1;
nav_sins.calib_params.no_a[2][0] = *acc_nort_2;
nav_sins.calib_params.no_a[3][0] = *acc_nort_3;
nav_sins.calib_params.no_a[4][0] = *acc_nort_4;
nav_sins.calib_params.no_a[5][0] = *acc_nort_5;
nav_sins.calib_params.no_w[0][0] = *gyr_nort_0;
nav_sins.calib_params.no_w[1][0] = *gyr_nort_1;
nav_sins.calib_params.no_w[2][0] = *gyr_nort_2;
nav_sins.calib_params.no_w[3][0] = *gyr_nort_3;
nav_sins.calib_params.no_w[4][0] = *gyr_nort_4;
nav_sins.calib_params.no_w[5][0] = *gyr_nort_5;
*/
nav_sins.calib_params.ba[0][0] = *acc_bias_x;
nav_sins.calib_params.ba[1][0] = *acc_bias_y;
nav_sins.calib_params.ba[2][0] = *acc_bias_z;
nav_sins.calib_params.bw[0][0] = *gyr_bias_x;
nav_sins.calib_params.bw[1][0] = *gyr_bias_y;
nav_sins.calib_params.bw[2][0] = *gyr_bias_z;
nav_sins.calib_params.sa[0][0] = *acc_scale_x;
nav_sins.calib_params.sa[1][0] = *acc_scale_y;
nav_sins.calib_params.sa[2][0] = *acc_scale_z;
nav_sins.calib_params.sw[0][0] = *gyr_scale_x;
nav_sins.calib_params.sw[1][0] = *gyr_scale_y;
nav_sins.calib_params.sw[2][0] = *gyr_scale_z;
nav_sins.marg_b.eu_bs_mag[0][0] = M_PI;
nav_sins.marg_b.eu_bs_mag[1][0] = 0.0;
nav_sins.marg_b.eu_bs_mag[2][0] = -M_PI;
nav_sins.ref_params.eu_vh_base[0][0] = *eu_vh_roll;
nav_sins.ref_params.eu_vh_base[1][0] = *eu_vh_pitch;
nav_sins.ref_params.eu_vh_base[2][0] = *eu_vh_yaw;
nav_sins.ref_params.eu_vh_base[0][0] = *eu_vh_roll;
nav_sins.ref_params.eu_vh_base[1][0] = *eu_vh_pitch;
nav_sins.ref_params.eu_vh_base[2][0] = *eu_vh_yaw;
nav_sins.ref_params.zupt_source = *zupt_src;
nav_sins.ref_params.glrt_gamma = *gamma;
nav_sins.ref_params.glrt_acc_sigma = *acc_sigma;
nav_sins.ref_params.glrt_gyr_sigma = *gyr_sigma;
nav_sins.ref_params.glrt_n = *samples;
nav_sins.ref_params.sns_extr_en = *en_extr;
nav_sins.ref_params.sns_vel_th = *sns_v_th;
dbg_in_fill_gnss(this->gps);
prepare_data_gnss(this->gps);
dbg_in_fill_other(baro, odo, marg);
dbg_in_append_log();
prepare_data(baro, odo, marg);
nav_sins.run();
#if defined(BOARD_BEZVODIATEL)
if (NAV_RUN_STATIONARY_AUTONOMOUS == nav_sins.run_mode ||
NAV_RUN_STATIONARY_PRIMARY == nav_sins.run_mode) {
blue_led_on();
red_led_on();
} else {
blue_led_off();
red_led_off();
}
#endif
navi2acs();
navi2mavlink();
debug2mavlink();
dbg_out_fill(nav_sins.navi_data);
dbg_out_append_log();
stop_time_measurement(marg.dT);
}
