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); }