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 super_dead_mode(){
uint16_t counter = 0;
while(!config.health){
handle_music();
// Manage base station comms
uint8_t b;
if(CHECK_CHAR()){
b=AVAIL_CHAR();
if(b == 0x10) {
control_transfer();
}
}
counter++;
delay_1_ms();
if(counter > config.death_period){
counter = 0;
led_off();
Send_Byte(config.id);
play_song((uint16_t*)dead_song,sizeof(dead_song)/sizeof(uint16_t),10000,0);
}
if(counter == config.death_period-50){
red_led_on();
}
}
}
/**
****************************************************************************************
* @brief Sets-up the LEDS to indicate disconnection
*
* @param None
*
* @return void
****************************************************************************************
*/
void leds_set_disconnected()
{
high_priority_indications_active = true;
if (!(GetWord16(SYS_STAT_REG) & DBG_IS_UP) || !dbg_uses_led_pins()) {
green_led_off();
red_led_on();
app_timer_set(APP_RED_LED_TIMER, TASK_APP, RED_ON);
leds_block_sleep();
}
}
void mem_error_cb(memtest_t *memp, testtype e, size_t address) {
(void)memp;
(void)e;
(void)address;
green_led_off();
red_led_on();
osalThreadSleepMilliseconds(10);
errors++;
osalSysHalt("Memory broken");
}
void hit_by(uint8_t who)
{
uint16_t respawn_timer;
add_to_hitlist(who);
Save(FLASH_HITLIST,(uint16_t*)&hitlist,HITLIST_SIZE);
config.health --;
Save(FLASH_CONFIG,(uint16_t*)&config, CONFIG_SIZE);
play_song((uint16_t*)death_song,sizeof(death_song)/sizeof(uint16_t),60000,0);
if(!config.health)
{
red_led_on();
super_dead_mode();
return;
}
respawn_timer = config.respawn_delay;
while(respawn_timer)
{
respawn_timer--;
red_led_on();
for(uint8_t i=0;i<50;i++)
{
handle_music();
delay_1_ms();
}
if(respawn_timer < 30)
{
led_off();
}
for(uint8_t i=0;i<50;i++)
{
handle_music();
delay_1_ms();
}
}
}
void cheat(){
while(1){
red_led_on();
tone(7813);
for(uint8_t i=0; i<200; i++){
delay_1_ms();
}
led_off();
for(uint8_t i=0; i<200; i++){
delay_1_ms();
}
}
}
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();
}
}
void render_game_over() {
int led_delay = 200;
oledReset();
oledPrintText(game_over, 2, 1);
for (int i = 0; i < 3; i++) {
red_led_on(led_delay);
delay(led_delay);
}
//OrbitOledUpdate();
delay(750);
return;
}
void collision() {
if ((ball_x_posn <= 3) && (ball_y_posn >= (paddle_y1_posn - 1)) && (ball_y_posn <= (paddle_y2_posn + 1))) {
xBdir = 1; // move it right
score += 1;
}
// hits ceiling
if(ball_y_posn <= 0) {
mod_ball_path();
yBdir = 0;
ball_y_posn = 0;
}
// hits floor
else if(ball_y_posn >= 28) {
yBdir = 1;
mod_ball_path();
ball_y_posn = 28;
}
// hits paddle left wall
// Paddle width 2, so we have it like this
if(ball_x_posn <= 1) {
red_led_on(250);
lose();
}
// hits right wall
else if(ball_x_posn >= (right_wall_posn - 2))
{
mod_ball_path();
ball_x_posn = right_wall_posn - 2;
xBdir = 0;
}
}
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;
}
}
void __led_set(led_id_t mask, int state)
{
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();
}
}
static THD_FUNCTION(BootBlinkThread, arg) {
chRegSetThreadName("BootBlink");
(void)arg;
while (!chThdShouldTerminateX()) {
red_led_on();
warning_led_off();
PAUSE();
red_led_off();
warning_led_on();
PAUSE();
warning_led_off();
red_led_off();
PAUSE();
}
warning_led_off();
red_led_off();
chThdExit(MSG_OK);
}
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 board_init_f(ulong bootflag)
{
bd_t *bd;
init_fnc_t **init_fnc_ptr;
gd_t *id;
ulong addr, addr_sp;
#ifdef CONFIG_PRAM
ulong reg;
#endif
bootstage_mark_name(BOOTSTAGE_ID_START_UBOOT_F, "board_init_f");
/* Pointer is writable since we allocated a register for it */
gd = (gd_t *) ((CONFIG_SYS_INIT_SP_ADDR) & ~0x07);
/* compiler optimization barrier needed for GCC >= 3.4 */
__asm__ __volatile__("": : :"memory");
memset((void *)gd, 0, sizeof(gd_t));
gd->mon_len = _bss_end_ofs;
#ifdef CONFIG_OF_EMBED
/* Get a pointer to the FDT */
gd->fdt_blob = _binary_dt_dtb_start;
#elif defined CONFIG_OF_SEPARATE
/* FDT is at end of image */
gd->fdt_blob = (void *)(_end_ofs + _TEXT_BASE);
#endif
/* Allow the early environment to override the fdt address */
gd->fdt_blob = (void *)getenv_ulong("fdtcontroladdr", 16,
(uintptr_t)gd->fdt_blob);
for (init_fnc_ptr = init_sequence; *init_fnc_ptr; ++init_fnc_ptr) {
if ((*init_fnc_ptr)() != 0) {
hang ();
}
}
#ifdef CONFIG_OF_CONTROL
/* For now, put this check after the console is ready */
if (fdtdec_prepare_fdt()) {
panic("** CONFIG_OF_CONTROL defined but no FDT - please see "
"doc/README.fdt-control");
}
#endif
debug("monitor len: %08lX\n", gd->mon_len);
/*
* Ram is setup, size stored in gd !!
*/
debug("ramsize: %08lX\n", gd->ram_size);
#if defined(CONFIG_SYS_MEM_TOP_HIDE)
/*
* Subtract specified amount of memory to hide so that it won't
* get "touched" at all by U-Boot. By fixing up gd->ram_size
* the Linux kernel should now get passed the now "corrected"
* memory size and won't touch it either. This should work
* for arch/ppc and arch/powerpc. Only Linux board ports in
* arch/powerpc with bootwrapper support, that recalculate the
* memory size from the SDRAM controller setup will have to
* get fixed.
*/
gd->ram_size -= CONFIG_SYS_MEM_TOP_HIDE;
#endif
addr = CONFIG_SYS_SDRAM_BASE + gd->ram_size;
#ifdef CONFIG_LOGBUFFER
#ifndef CONFIG_ALT_LB_ADDR
/* reserve kernel log buffer */
addr -= (LOGBUFF_RESERVE);
debug("Reserving %dk for kernel logbuffer at %08lx\n", LOGBUFF_LEN,
addr);
#endif
#endif
#ifdef CONFIG_PRAM
/*
* reserve protected RAM
*/
reg = getenv_ulong("pram", 10, CONFIG_PRAM);
addr -= (reg << 10); /* size is in kB */
debug("Reserving %ldk for protected RAM at %08lx\n", reg, addr);
#endif /* CONFIG_PRAM */
#if !(defined(CONFIG_SYS_ICACHE_OFF) && defined(CONFIG_SYS_DCACHE_OFF))
/* reserve TLB table */
addr -= (4096 * 4);
/* round down to next 64 kB limit */
addr &= ~(0x10000 - 1);
gd->tlb_addr = addr;
debug("TLB table at: %08lx\n", addr);
#endif
/* round down to next 4 kB limit */
addr &= ~(4096 - 1);
debug("Top of RAM usable for U-Boot at: %08lx\n", addr);
#ifdef CONFIG_LCD
#ifdef CONFIG_FB_ADDR
gd->fb_base = CONFIG_FB_ADDR;
#else
/* reserve memory for LCD display (always full pages) */
addr = lcd_setmem(addr);
gd->fb_base = addr;
#endif /* CONFIG_FB_ADDR */
#endif /* CONFIG_LCD */
/*
* reserve memory for U-Boot code, data & bss
* round down to next 4 kB limit
*/
addr -= gd->mon_len;
addr &= ~(4096 - 1);
debug("Reserving %ldk for U-Boot at: %08lx\n", gd->mon_len >> 10, addr);
#ifndef CONFIG_SPL_BUILD
/*
* reserve memory for malloc() arena
*/
addr_sp = addr - TOTAL_MALLOC_LEN;
debug("Reserving %dk for malloc() at: %08lx\n",
TOTAL_MALLOC_LEN >> 10, addr_sp);
/*
* (permanently) allocate a Board Info struct
* and a permanent copy of the "global" data
*/
addr_sp -= sizeof (bd_t);
bd = (bd_t *) addr_sp;
gd->bd = bd;
debug("Reserving %zu Bytes for Board Info at: %08lx\n",
sizeof (bd_t), addr_sp);
#ifdef CONFIG_MACH_TYPE
gd->bd->bi_arch_number = CONFIG_MACH_TYPE; /* board id for Linux */
#endif
addr_sp -= sizeof (gd_t);
id = (gd_t *) addr_sp;
debug("Reserving %zu Bytes for Global Data at: %08lx\n",
sizeof (gd_t), addr_sp);
/* setup stackpointer for exeptions */
gd->irq_sp = addr_sp;
#ifdef CONFIG_USE_IRQ
addr_sp -= (CONFIG_STACKSIZE_IRQ+CONFIG_STACKSIZE_FIQ);
debug("Reserving %zu Bytes for IRQ stack at: %08lx\n",
CONFIG_STACKSIZE_IRQ+CONFIG_STACKSIZE_FIQ, addr_sp);
#endif
/* leave 3 words for abort-stack */
addr_sp -= 12;
/* 8-byte alignment for ABI compliance */
addr_sp &= ~0x07;
#else
addr_sp += 128; /* leave 32 words for abort-stack */
gd->irq_sp = addr_sp;
#endif
red_led_on();
debug("New Stack Pointer is: %08lx\n", addr_sp);
#ifdef CONFIG_POST
post_bootmode_init();
post_run(NULL, POST_ROM | post_bootmode_get(0));
#endif
gd->bd->bi_baudrate = gd->baudrate;
/* Ram ist board specific, so move it to board code ... */
dram_init_banksize();
display_dram_config(); /* and display it */
gd->relocaddr = addr;
gd->start_addr_sp = addr_sp;
gd->reloc_off = addr - _TEXT_BASE;
debug("relocation Offset is: %08lx\n", gd->reloc_off);
memcpy(id, (void *)gd, sizeof(gd_t));
relocate_code(addr_sp, id, addr);
/* NOTREACHED - relocate_code() does not return */
}
static void general_test (NANDDriver *nandp, size_t first,
size_t last, size_t read_rounds){
size_t block, page, round;
bool status;
uint8_t op_status;
uint32_t recc, wecc;
red_led_on();
/* initialize time measurement units */
chTMObjectInit(&tmu_erase);
chTMObjectInit(&tmu_write_data);
chTMObjectInit(&tmu_write_spare);
chTMObjectInit(&tmu_read_data);
chTMObjectInit(&tmu_read_spare);
/* perform basic checks */
for (block=first; block<last; block++){
if (!nandIsBad(nandp, block)){
if (!is_erased(nandp, block)){
op_status = nandErase(nandp, block);
osalDbgCheck(0 == (op_status & 1)); /* operation failed */
}
}
}
/* write block with pattern, read it back and compare */
for (block=first; block<last; block++){
if (!nandIsBad(nandp, block)){
for (page=0; page<nandp->config->pages_per_block; page++){
pattern_fill();
chTMStartMeasurementX(&tmu_write_data);
op_status = nandWritePageData(nandp, block, page,
nand_buf, nandp->config->page_data_size, &wecc);
chTMStopMeasurementX(&tmu_write_data);
osalDbgCheck(0 == (op_status & 1)); /* operation failed */
chTMStartMeasurementX(&tmu_write_spare);
op_status = nandWritePageSpare(nandp, block, page,
nand_buf + nandp->config->page_data_size,
nandp->config->page_spare_size);
chTMStopMeasurementX(&tmu_write_spare);
osalDbgCheck(0 == (op_status & 1)); /* operation failed */
/* read back and compare */
for (round=0; round<read_rounds; round++){
memset(nand_buf, 0, NAND_PAGE_SIZE);
chTMStartMeasurementX(&tmu_read_data);
nandReadPageData(nandp, block, page,
nand_buf, nandp->config->page_data_size, &recc);
chTMStopMeasurementX(&tmu_read_data);
osalDbgCheck(0 == (recc ^ wecc)); /* ECC error detected */
chTMStartMeasurementX(&tmu_read_spare);
nandReadPageSpare(nandp, block, page,
nand_buf + nandp->config->page_data_size,
nandp->config->page_spare_size);
chTMStopMeasurementX(&tmu_read_spare);
osalDbgCheck(0 == memcmp(ref_buf, nand_buf, NAND_PAGE_SIZE)); /* Read back failed */
}
}
/* make clean */
chTMStartMeasurementX(&tmu_erase);
op_status = nandErase(nandp, block);
chTMStopMeasurementX(&tmu_erase);
osalDbgCheck(0 == (op_status & 1)); /* operation failed */
status = is_erased(nandp, block);
osalDbgCheck(true == status); /* blocks was not erased successfully */
}/* if (!nandIsBad(nandp, block)){ */
}
red_led_off();
}
int main(){
// Initialize Peripherals
interface_init();
red_led_on();
uart_init(BAUDRATE);
animation_manager_init();
sys_timer_start();
audio_init();
sei(); // enable global interrupts
// Load Default Animation
animation_manager_load_animation(START_ANIMATION);
// Enter Setup if Requested
_delay_ms(100);
if(deb_switch_1()){
setup_wb_run();
}
else if(deb_switch_2()){
setup_orientation_run();
}
// Load Default Animation
animation_manager_load_animation(START_ANIMATION);
// Set White Balance
_delay_ms(300);
display_wb_update();
while(uart_async_run()); // setup white balance
// Control Panel is Ready => Signal this by Turning the LED Green
red_led_off();
green_led_on();
while(1){
// Sleep Mode
if(!switch_on_off()){ // if switched off
go_to_sleep();
}
// Change animations
sw_check();
if(sw_check_pressed(SW_LEFT, 200, true)){
animation_manager_dec_animation();
}
else if(sw_check_pressed(SW_RIGHT, 200, true)){
animation_manager_inc_animation();
}
else if(sw_check_pressed(SW_RAND, 300, true)){
animation_manager_random_animation();
}
// Generate Image
animation_manager_run(0);
// Check Audio
audio_start();
while(audio_run());
audio_process();
// Display Image
while(uart_async_run()){
interface_async_run();
}
}
}
bool process_record_user(uint16_t keycode, keyrecord_t *record) {
switch (keycode) {
case QWERTY:
if (record->event.pressed) {
set_single_persistent_default_layer(_QWERTY);
}
return false;
break;
case LOWER:
if (record->event.pressed) {
layer_on(_LOWER);
grn_led_on();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
} else {
layer_off(_LOWER);
grn_led_off();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
}
return false;
break;
//SHIFT is handled as LSHIFT in the general case - 'toggle' shoudl activate caps, while the layer is only active when shift is held.
case RAISE:
if (record->event.pressed) {
layer_on(_RAISE);
red_led_on();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
} else {
layer_off(_RAISE);
red_led_off();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
}
return false;
break;
case ADJUST:
if (record->event.pressed) {
layer_on(_ADJUST);
} else {
layer_off(_ADJUST);
}
return false;
break;
//MOUSE layer needs to be handled the same way as NUMKEY, but differently from shift
case MOUKEY:
if (record->event.pressed) {
layer_on(_MOUSE);
blu_led_on();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
} else {
layer_off(_MOUSE);
blu_led_off();
update_tri_layer(_LOWER, _RAISE, _ADJUST);
}
return false;
break;
//mouse buttons, for 1-3, to update the mouse report:
case MS_BTN1:
currentReport = pointing_device_get_report();
if (record->event.pressed) {
if (shift_held && shift_suspended){
register_code(KC_LSFT);
shift_suspended = false;
}
//update mouse report here
currentReport.buttons |= MOUSE_BTN1; //MOUSE_BTN1 is a const defined in report.h
} else {
//update mouse report here
currentReport.buttons &= ~MOUSE_BTN1;
}
pointing_device_set_report(currentReport);
return false;
break;
case MS_BTN2:
currentReport = pointing_device_get_report();
if (record->event.pressed) {
if (shift_held && shift_suspended){
register_code(KC_LSFT);
shift_suspended = false;
}
//update mouse report here
currentReport.buttons |= MOUSE_BTN2; //MOUSE_BTN2 is a const defined in report.h
} else {
//update mouse report here
currentReport.buttons &= ~MOUSE_BTN2;
}
pointing_device_set_report(currentReport);
return false;
break;
case MS_BTN3:
currentReport = pointing_device_get_report();
if (record->event.pressed) {
if (shift_held && shift_suspended){
register_code(KC_LSFT);
shift_suspended = false;
}
//update mouse report here
currentReport.buttons |= MOUSE_BTN3; //MOUSE_BTN3 is a const defined in report.h
} else {
//update mouse report here
currentReport.buttons &= ~MOUSE_BTN3;
}
pointing_device_set_report(currentReport);
return false;
break;
//Additionally, if NS_ keys are in use, then shift may be held (but is
//disabled for the unshifted keycodes to be send. Check the bool and
//register shift as necessary.
// default:
// if (shift_held){
// register_code(KC_LSFT);
// }
// break;
}
return true;
};
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);
}