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