Example #1
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;
	  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;
	}
}
Example #2
0
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();
    }
}
Example #4
0
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");
}
Example #5
0
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();
    }
  }
}
Example #6
0
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();
        }
    }
}
Example #7
0
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();
	}
}
Example #8
0
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;
}
Example #9
0
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;
    }

}
Example #10
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;
	}
}
Example #11
0
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();
	}
}
Example #12
0
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);
}
Example #13
0
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();
	}
}
Example #14
0
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 */
}
Example #15
0
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();
}
Example #16
0
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();
		}

	}
}
Example #17
0
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;
};
Example #18
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);
}