void __led_set(led_id_t mask, int state)
{
switch(mask) {
case STATUS_LED_RED:
if(STATUS_LED_ON == state) {
red_led_on();
} else {
red_led_off();
}
break;
case STATUS_LED_GREEN:
if(STATUS_LED_ON == state) {
green_led_on();
} else {
green_led_off();
}
break;
case STATUS_LED_YELLOW:
if(STATUS_LED_ON == state) {
yellow_led_on();
} else {
yellow_led_off();
}
break;
case STATUS_LED_BLUE:
if(STATUS_LED_ON == state) {
blue_led_on();
} else {
blue_led_off();
}
break;
}
}
void __led_toggle(led_id_t mask)
{
if (STATUS_LED_RED == mask) {
if (STATUS_LED_ON == saved_state[STATUS_LED_RED])
red_led_off();
else
red_led_on();
} else if (STATUS_LED_GREEN == mask) {
if (STATUS_LED_ON == saved_state[STATUS_LED_GREEN])
green_led_off();
else
green_led_on();
} else if (STATUS_LED_YELLOW == mask) {
if (STATUS_LED_ON == saved_state[STATUS_LED_YELLOW])
yellow_led_off();
else
yellow_led_on();
} else if (STATUS_LED_BLUE == mask) {
if (STATUS_LED_ON == saved_state[STATUS_LED_BLUE])
blue_led_off();
else
blue_led_on();
}
}
void __led_set(led_id_t mask, int state)
{
if (STATUS_LED_BLUE == mask) {
if (STATUS_LED_ON == state)
blue_led_on();
else
blue_led_off();
} else if (STATUS_LED_RED == mask) {
if (STATUS_LED_ON == state)
red_led_on();
else
red_led_off();
} else if (STATUS_LED_GREEN == mask) {
if (STATUS_LED_ON == state)
green_led_on();
else
green_led_off();
} else if (STATUS_LED_YELLOW == mask) {
if (STATUS_LED_ON == state)
yellow_led_on();
else
yellow_led_off();
}
}
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);
}
