static void cmd_test(BaseChannel *chp, int argc, char *argv[]) {
  Thread *tp;

  (void)argv;
  if (argc > 0) {
    shellPrintLine(chp, "Usage: test");
    return;
  }
  tp = chThdCreateFromHeap(NULL, TEST_WA_SIZE, chThdGetPriority(),
                           TestThread, chp);
  if (tp == NULL) {
    shellPrintLine(chp, "out of memory");
    return;
  }
  chThdWait(tp);
}

static void cmd_test(BaseSequentialStream *chp, int argc, char *argv[]) {
  Thread *tp;

  (void)argv;
  if (argc > 0) {
    chprintf(chp, "Usage: test\r\n");
    return;
  }
  tp = chThdCreateFromHeap(NULL, TEST_WA_SIZE, chThdGetPriority(),
                           TestThread, chp);
  if (tp == NULL) {
    chprintf(chp, "out of memory\r\n");
    return;
  }
  chThdWait(tp);
}

/** \brief DeInit/Cleanup HydraNFC functions
 *
 * \param con t_hydra_console*: hydra console (optional can be NULL if unused)
 * \return void
 *
 */
void hydranfc_cleanup(t_hydra_console *con)
{
	(void)con;

	if(key_sniff_thread != NULL) {
		chThdTerminate(key_sniff_thread);
		chThdWait(key_sniff_thread);
		key_sniff_thread = NULL;
	}

	bsp_spi_deinit(BSP_DEV_SPI2);
	extStop(&EXTD1);

	/* deinit GPIO config (reinit using hydrabus_init() */
	deinit_gpio();
}

static void wavePlayEventHandler(uint8_t evt)
{
	static uint8_t prevEvt=0;

	if (evt && evt < 15)
		if (prevEvt!=evt) {
			prevEvt=evt;
			codec_sendBeep();
		}

	switch (evt)
	{
		case BTN_RIGHT:
			lcd_cls();
			ui_displayPreviousMenu();
			break;

		case BTN_LEFT:
			chThdTerminate(playerThread);
			chThdWait(playerThread);
			playerThread=NULL;
			break;

		case BTN_MID_DOWN:
			vol--;
			if (vol < 150) vol=150;
			codec_volCtl(vol);
			break;

		case BTN_MID_UP:
			vol++;
			if (vol > 220) vol=220;
			codec_volCtl(vol);
			break;

		case BTN_MID_SEL:
			if (!pause) {
				pause=1;
				ui_drawBottomBar("Stop", "Play", "Exit");
			} else {
				pause=0;
				ui_drawBottomBar("Stop", "Pause", "Exit");
			}
			codec_pauseResumePlayback(pause);
			break;
	}
}

//-----------------------------------------------------------------------------
int kuroBoxExternalDisplayStop(void)
{
	chThdTerminate(eDisplayThread);

	chSysLock();
	if (eDisplayThreadForSleep)
	{
		eDisplayThreadForSleep->p_u.rdymsg = (msg_t)1; // just a random non-0
		chSchReadyI(eDisplayThreadForSleep);
		eDisplayThreadForSleep = NULL;
	}
	chSysUnlock();

	chThdWait(eDisplayThread);
	eDisplayThread = NULL;
	return KB_OK;
}

static void bmk5_execute(void) {

  uint32_t n = 0;
  void *wap = wa[0];
  tprio_t prio = chThdGetPriority() - 1;
  test_wait_tick();
  test_start_timer(1000);
  do {
    chThdWait(chThdCreateStatic(wap, WA_SIZE, prio, thread2, NULL));
    n++;
#if defined(SIMULATOR)
    ChkIntSources();
#endif
  } while (!test_timer_done);
  test_print("--- Score : ");
  test_printn(n);
  test_println(" threads/S");
}

/*
 * Application entry point.
 */
int main(void) {
  halInit();
  chSysInit();

  /*
   * Serial port initialization.
   */
  sdStart(&SD1, NULL); 
  chprintf((BaseSequentialStream *)&SD1, "Main (SD1 started)\r\n");

   /*
  *Semaphore Initialization
  */
  chSemInit(&mySemaphore, 1);

  /*
   * Set mode of PINES
   */

  palSetPadMode(GPIO0_PORT, GPIO0_PAD, PAL_MODE_INPUT);   //Input T1
  palSetPadMode(GPIO1_PORT, GPIO1_PAD, PAL_MODE_INPUT);   //Input T2
  palSetPadMode(GPIO4_PORT, GPIO4_PAD, PAL_MODE_INPUT);   //Input T3
  palSetPadMode(GPIO17_PORT, GPIO17_PAD, PAL_MODE_INPUT); //Input T4
  
  palSetPadMode(GPIO18_PORT, GPIO18_PAD, PAL_MODE_OUTPUT); //Output T1
  palSetPadMode(GPIO22_PORT, GPIO22_PAD, PAL_MODE_OUTPUT); //Output T2
  palSetPadMode(GPIO23_PORT, GPIO23_PAD, PAL_MODE_OUTPUT); //Output T3
  palSetPadMode(GPIO24_PORT, GPIO24_PAD, PAL_MODE_OUTPUT); //Output T4
  
  /*
   * Creates the blinker thread.
   */
  chThdCreateStatic(waTh1, sizeof(waTh1), NORMALPRIO, Th1, NULL);
  chThdCreateStatic(waTh2, sizeof(waTh2), NORMALPRIO, Th2, NULL);
  chThdCreateStatic(waTh3, sizeof(waTh3), NORMALPRIO, Th3, NULL);
  chThdCreateStatic(waTh4, sizeof(waTh4), NORMALPRIO, Th4, NULL);
  
  /*
   * Events servicing loop.
   */
  chThdWait(chThdSelf());

  return 0;
}

int main(void)
{
	halInit();
	chSysInit();

	thread_t *tp;


	/*analog input*/
	palSetPadMode(GPIOA, 6, PAL_MODE_INPUT_ANALOG); // this is 10th channel

	/*analog output*/
    palSetPadMode(GPIOA, 4, PAL_MODE_INPUT_ANALOG);

    tp = chThdCreateStatic(analogReadWA, sizeof(analogReadWA), HIGHPRIO, adctodac, NULL);
	chThdWait(tp);

	return 0;
}

/*
 * Application entry point.
 */
int main(void) {

  /*
   * System initializations.
   * - HAL initialization, this also initializes the configured device drivers
   *   and performs the board-specific initializations.
   * - Kernel initialization, the main() function becomes a thread and the
   *   RTOS is active.
   */
  halInit();
  chSysInit();

  /*
   * Shell manager initialization.
   */
  shellInit();

  /*
   * Activates the serial driver 6 using the driver default configuration.
   */
  sdStart(&SD6, NULL);

  /*
   * Initializes the SDIO drivers.
   */
  sdcStart(&SDCD1, &sdccfg);

  /*
   * Creates the blinker thread.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

  /*
   * Normal main() thread activity, spawning shells.
   */
  while (true) {
    thread_t *shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
                                            "shell", NORMALPRIO + 1,
                                            shellThread, (void *)&shell_cfg1);
    chThdWait(shelltp);               /* Waiting termination.             */
    chThdSleepMilliseconds(1000);
  }
}

/*
 * Stop a event loop thread
 */
void sc_event_loop_stop(void)
{
  msg_t nop;

  chDbgAssert(event_thread != NULL, "Event thread not running", "#1");

  chThdTerminate(event_thread);

  // Send an event to wake up the thread
  nop = sc_event_msg_create_type(SC_EVENT_TYPE_NOP);
  sc_event_msg_post(nop, SC_EVENT_MSG_POST_FROM_NORMAL);

  chThdWait(event_thread);
  event_thread = NULL;

  // Clear all handlers
#if HAL_USE_UART
  cb_handle_byte = NULL;
#endif
#if HAL_USE_EXT
  {
    uint8_t i;
    for (i = 0; i < EXT_MAX_CHANNELS; ++i) {
      cb_extint[i] = NULL;
    }
  }
#endif

  cb_gsm_state_changed = NULL;
  cb_gsm_cmd_done = NULL;

#if HAL_USE_ADC
  cb_adc_available = NULL;
#endif
  cb_temp_available = NULL;
  cb_9dof_available = NULL;
  cb_blob_available = NULL;
  cb_ahrs_available = NULL;
}

static void bridge(t_hydra_console *con)
{
	uint8_t tx_data[UART_BRIDGE_BUFF_SIZE];
	uint8_t bytes_read;
	//uint8_t bytes_read;
	mode_config_proto_t* proto = &con->mode->proto;

	cprintf(con, "Interrupt by pressing user button.\r\n");
	cprint(con, "\r\n", 2);

	thread_t *bthread = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, "bridge_thread",
						LOWPRIO, bridge_thread, con);
	while(!USER_BUTTON) {
		bytes_read = chnReadTimeout(con->sdu, tx_data,
					    UART_BRIDGE_BUFF_SIZE, US2ST(100));
		if(bytes_read > 0) {
			bsp_uart_write_u8(proto->dev_num, tx_data, bytes_read);
		}
	}
	chThdTerminate(bthread);
	chThdWait(bthread);
}

static msg_t ShellThread(void *arg){
  chRegSetThreadName("Shell");
  chThdSleepMilliseconds(1000);

  /* init static pointer for serial driver with received pointer */
  shell_sdp = (SerialUSBDriver *)arg;

  // create and init microrl object
  microrl_t microrl_shell;
//  cli_print("@@*** Super cool device, version 1.2.3, for help type help... ***@@\r\n");
  microrl_init(&microrl_shell, cli_print);

  // set callback for execute
  microrl_set_execute_callback(&microrl_shell, execute);

  // set callback for completion (optionally)
  microrl_set_complete_callback(&microrl_shell, complete);

  // set callback for ctrl+c handling (optionally)
  microrl_set_sigint_callback(&microrl_shell, sigint);

  while (TRUE){
    // put received char from stdin to microrl lib
    msg_t c = sdGetTimeout(shell_sdp, MS2ST(50));
    if (c != Q_TIMEOUT)
      microrl_insert_char(&microrl_shell, (char)c);

    /* умираем по всем правилам, не забываем убить потомков */
    if (chThdShouldTerminate()){
      if ((current_cmd_tp != NULL) && (current_cmd_tp->p_state != THD_STATE_FINAL)){
        chThdTerminate(current_cmd_tp);
        chThdWait(current_cmd_tp);
      }
      chThdExit(0);
    }
  }
  return 0;
}

static void cmd_benchmark( int argc, char *argv[] )
{
    thread_t *tp;

    (void)argv;
    if( argc > 0 )
    {
        usage( "benchmark" );
        return;
    }

    tp = chThdCreateFromHeap( NULL,
                              TEST_WA_SIZE,
                              chThdGetPriorityX(),
                              TestThread,
                              &SDU2 );
    if( tp == NULL )
    {
        chprint( "out of memory\r\n" );
        return;
    }

    chThdWait( tp );
}

/* Terminate the core thread, wait for control release */
void ncoreTerminateDrawThread(void) {
  chThdTerminate(nThd);
  chThdWait(nThd);
}

/*
 * Application entry point.
 */
int main(void) {

  /*
   * System initializations.
   * - HAL initialization, this also initializes the configured device drivers
   *   and performs the board-specific initializations.
   * - Kernel initialization, the main() function becomes a thread and the
   *   RTOS is active.
   */
  halInit();
  chSysInit();

  /*
   * Initializes a serial-over-USB CDC driver.
   */
  sduObjectInit(&SDU2);
  sduStart(&SDU2, &serusbcfg);

  /*
   * GPIOI1 is programmed as output (board LED).
   */
  palClearLine(LINE_ARD_D13);
  palSetLineMode(LINE_ARD_D13, PAL_MODE_OUTPUT_PUSHPULL);

  /*
   * Activates the USB driver and then the USB bus pull-up on D+.
   * Note, a delay is inserted in order to not have to disconnect the cable
   * after a reset.
   */
  usbDisconnectBus(serusbcfg.usbp);
  chThdSleepMilliseconds(1500);
  usbStart(serusbcfg.usbp, &usbcfg);
  usbConnectBus(serusbcfg.usbp);

  /*
   * Shell manager initialization.
   */
  shellInit();

  /*
   * Creates the blinker thread.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

  /*
   * Normal main() thread activity, spawning shells.
   */
  while (true) {
    if (SDU2.config->usbp->state == USB_ACTIVE) {
      thread_t *shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
                                              "shell", NORMALPRIO + 1,
                                              shellThread, (void *)&shell_cfg1);
      chThdWait(shelltp);
    }
#if 0
    if (palReadPad(GPIOI, GPIOI_BUTTON_USER)) {
      usbDisconnectBus(serusbcfg.usbp);
      usbStop(serusbcfg.usbp);
      chThdSleepMilliseconds(1500);
      usbStart(serusbcfg.usbp, &usbcfg);
      usbConnectBus(serusbcfg.usbp);
    }
#endif
    chThdSleepMilliseconds(1000);
  }
}

  msg_t BaseThread::Wait(void) {

    return chThdWait(thread_ref);
  }

//.........这里部分代码省略.........

  /* Global configuration pointer.*/
  config = cfg;

  /* Starting timers using the stored configurations.*/
  gptStart(cfg->gpt1p, cfg->gptcfg1p);
  gptStart(cfg->gpt2p, cfg->gptcfg2p);

  /*
   * Initializes the mailboxes and creates the worker threads.
   */
  for (i = 0; i < IRQ_STORM_CFG_NUM_THREADS; i++) {
    chMBObjectInit(&mb[i], b[i], IRQ_STORM_CFG_MAILBOX_SIZE);
    threads[i] = chThdCreateStatic(irq_storm_thread_wa[i],
                                   sizeof irq_storm_thread_wa[i],
                                   IRQ_STORM_CFG_THREADS_PRIORITY,
                                   irq_storm_thread,
                                   (void *)i);
  }

  /* Printing environment information.*/
  chprintf(cfg->out, "");
  chprintf(cfg->out, "\r\n*** ChibiOS/RT IRQ-STORM long duration test\r\n***\r\n");
  chprintf(cfg->out, "*** Kernel:       %s\r\n", CH_KERNEL_VERSION);
  chprintf(cfg->out, "*** Compiled:     %s\r\n", __DATE__ " - " __TIME__);
#ifdef PORT_COMPILER_NAME
  chprintf(cfg->out, "*** Compiler:     %s\r\n", PORT_COMPILER_NAME);
#endif
  chprintf(cfg->out, "*** Architecture: %s\r\n", PORT_ARCHITECTURE_NAME);
#ifdef PORT_CORE_VARIANT_NAME
  chprintf(cfg->out, "*** Core Variant: %s\r\n", PORT_CORE_VARIANT_NAME);
#endif
  chprintf(cfg->out, "*** System Clock: %d\r\n", cfg->sysclk);
#ifdef PORT_INFO
  chprintf(cfg->out, "*** Port Info:    %s\r\n", PORT_INFO);
#endif
#ifdef PLATFORM_NAME
  chprintf(cfg->out, "*** Platform:     %s\r\n", PLATFORM_NAME);
#endif
#ifdef BOARD_NAME
  chprintf(cfg->out, "*** Test Board:   %s\r\n", BOARD_NAME);
#endif
  chprintf(cfg->out, "***\r\n");
  chprintf(cfg->out, "*** Iterations:   %d\r\n", IRQ_STORM_CFG_ITERATIONS);
  chprintf(cfg->out, "*** Randomize:    %d\r\n", IRQ_STORM_CFG_RANDOMIZE);
  chprintf(cfg->out, "*** Threads:      %d\r\n", IRQ_STORM_CFG_NUM_THREADS);
  chprintf(cfg->out, "*** Mailbox size: %d\r\n\r\n", IRQ_STORM_CFG_MAILBOX_SIZE);

  /* Test loop.*/
  worst = 0;
  for (i = 1; i <= IRQ_STORM_CFG_ITERATIONS; i++){

    chprintf(cfg->out, "Iteration %d\r\n", i);
    saturated = false;
    threshold = 0;

    /* Timer intervals starting at 2mS then decreased by 10% after each
       cycle.*/
    for (interval = 2000; interval >= 2; interval -= (interval + 9) / 10) {

      /* Timers programmed slightly out of phase each other.*/
      gptStartContinuous(cfg->gpt1p, interval - 1); /* Slightly out of phase.*/
      gptStartContinuous(cfg->gpt2p, interval + 1); /* Slightly out of phase.*/

      /* Storming for one second.*/
      chThdSleepMilliseconds(1000);

      /* Timers stopped.*/
      gptStopTimer(cfg->gpt1p);
      gptStopTimer(cfg->gpt2p);

      /* Did the storm saturate the threads chain?*/
      if (!saturated)
        chprintf(cfg->out, ".");
      else {
        chprintf(cfg->out, "#");
        if (threshold == 0)
          threshold = interval;
        break;
      }
    }
    /* Gives threads a chance to empty the mailboxes before next cycle.*/
    chThdSleepMilliseconds(20);
    chprintf(cfg->out, "\r\nSaturated at %d uS\r\n\r\n", threshold);
    if (threshold > worst)
      worst = threshold;
  }
  gptStopTimer(cfg->gpt1p);
  gptStopTimer(cfg->gpt2p);

  chprintf(cfg->out, "Worst case at %d uS\r\n", worst);
  chprintf(cfg->out, "\r\nTest Complete\r\n");

  /* Terminating threads and cleaning up.*/
  for (i = 0; i < IRQ_STORM_CFG_NUM_THREADS; i++) {
    chThdTerminate(threads[i]);
    chThdWait(threads[i]);
    threads[i] = NULL;
  }
}

void slcan(t_hydra_console *con) {
	uint8_t buff[SLCAN_BUFF_LEN];
	can_tx_frame tx_msg;
	mode_config_proto_t* proto = &con->mode->proto;
	thread_t *rthread = NULL;

	while (!USER_BUTTON) {
		slcan_read_command(con, buff);
		switch (buff[0]) {
		case 'S':
			/*CAN speed*/
			switch(buff[1]) {
			case '0':
				proto->config.can.dev_speed = 10000;
				break;
			case '1':
				proto->config.can.dev_speed = 20000;
				break;
			case '2':
				proto->config.can.dev_speed = 50000;
				break;
			case '3':
				proto->config.can.dev_speed = 100000;
				break;
			case '4':
				proto->config.can.dev_speed = 125000;
				break;
			case '5':
				proto->config.can.dev_speed = 250000;
				break;
			case '6':
				proto->config.can.dev_speed = 500000;
				break;
			case '7':
				proto->config.can.dev_speed = 800000;
				break;
			case '8':
				proto->config.can.dev_speed = 1000000;
				break;
			case '9':
				proto->config.can.dev_speed = 2000000;
				break;
			}

			if(bsp_can_set_speed(proto->dev_num, proto->config.can.dev_speed) == BSP_OK) {
				cprint(con, "\r", 1);
			}else {
				cprint(con, "\x07", 1);
			}
			break;
		case 's':
			/*BTR value*/
			/*Not implemented*/
			cprint(con, "\x07", 1);
			break;
		case 'O':
			/*Open channel*/
			if(rthread == NULL) {
				rthread = chThdCreateFromHeap(NULL,
							      CONSOLE_WA_SIZE,
							      "SLCAN reader",
							      LOWPRIO,
							      can_reader_thread,
							      con);
				cprint(con, "\r", 1);
			} else {
				cprint(con, "\x07", 1);
			}
			break;
		case 'C':
			/*Close channel*/
			if(rthread != NULL) {
				chThdTerminate(rthread);
				chThdWait(rthread);
				rthread = NULL;
			}
			cprint(con, "\r", 1);
			break;
		case 't':
		case 'T':
		case 'r':
		case 'R':
			/*Transmit*/
			if(can_slcan_in(buff, &tx_msg) == BSP_OK) {
				chSysLock();
				if(bsp_can_write(proto->dev_num, &tx_msg) == BSP_OK) {
					cprint(con, "\r", 1);
				}else {
					cprint(con, "\x07", 1);
				}
				chSysUnlock();
			} else {
				cprint(con, "\x07", 1);
			}

			break;
		case 'F':
			/*status*/
			break;
		case 'M':
//.........这里部分代码省略.........

//.........这里部分代码省略.........
   */
  sduObjectInit(&OUTPUT_CHANNEL);
  sduStart(&OUTPUT_CHANNEL, &serusbcfg);

  /*
   * Activates the USB driver and then the USB bus pull-up on D+.
   * Note, a delay is inserted in order to not have to disconnect the cable
   * after a reset.
   */
  usbDisconnectBus(serusbcfg.usbp);
  chThdSleepMilliseconds(1000);
  usbStart(serusbcfg.usbp, &usbcfg);
  usbConnectBus(serusbcfg.usbp);

  led_blink = 1;

  /*
   * Shell manager initialization.
   */
  shellInit();

  /*
   * The main loop.
   */
  /*
  while(true) {
    if((palReadPad(BUTTON_GPIO, BUTTON_PIN) == BUTTON_ACTIVE) && (OUTPUT_CHANNEL.config->usbp->state == USB_ACTIVE)) {
      // sdWrite(&OUTPUT_CHANNEL, (uint8_t *)"hello world\r\n", 13);
      // chprintf((BaseSequentialStream *)&OUTPUT_CHANNEL, "Hello world\r\n");
      chnPutTimeout(&OUTPUT_CHANNEL, 'W', TIME_IMMEDIATE);
      // wieg_send(wieg_test_buf, 26);
      led_blink = 1;
      chThdSleepMilliseconds(200);
      // chnWrite((BaseChannel *)&OUTPUT_CHANNEL, (uint8_t *)"Hello, world\r\n", 14);
    }

    msg_t charbuf;
    charbuf = chnGetTimeout(&OUTPUT_CHANNEL, TIME_IMMEDIATE);
    if(charbuf != Q_TIMEOUT) {
      switch(charbuf) {
        default:
          chprintf((BaseSequentialStream *)&OUTPUT_CHANNEL, "Haloooo\r\n");
          break;
      }
      // chnPutTimeout(&OUTPUT_CHANNEL, (uint8_t)charbuf, TIME_IMMEDIATE);
      // if(charbuf == '\r') {
        // chnPutTimeout(&OUTPUT_CHANNEL, '\n', TIME_IMMEDIATE);          
      // }
    }

    chThdSleepMilliseconds(50);
  }
  */

  /*
   * Normal main() thread activity, spawning shells.
   */
  while (true) {
    msg_t c;
    if(serusbcfg.usbp->state == USB_ACTIVE) {
      c = chnGetTimeout(&OUTPUT_CHANNEL, TIME_IMMEDIATE);
      if(c != Q_TIMEOUT) {
        switch(c) {
          case 'k':
            if(unlock_pw_recved==0) {
              unlock_pw_recved++;
              led_blink = 1;
            }
            break;
          case 'o':
            if(unlock_pw_recved==1) {
              unlock_pw_recved++;
              led_blink = 1;
            }
            break;
          case 'j':
            if(unlock_pw_recved==2) {
              unlock_pw_recved++;
              led_blink = 1;
            }
            break;
          case 'a':
            if(unlock_pw_recved==3) {
              unlock_pw_recved++;
              led_blink = 1;
            }
            break;
        }
      }
      if(unlock_pw_recved == 4) {
        unlock_pw_recved = 0;      
        thread_t *shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
                                                "shell", NORMALPRIO + 1,
                                                shellThread, (void *)&shell_cfg1);
        chThdWait(shelltp);               /* Waiting termination.             */
      }
    }
    chThdSleepMilliseconds(50);
  }
}

static msg_t     thdUsbStorage(void *arg)
{
  (void) arg; // unused
  chRegSetThreadName("UsbStorage:polling");
  uint antiBounce=5;
  EventListener connected;

  // Should use EXTI interrupt instead of active polling,
  // but in the chibios_opencm3 implementation, since EXTI is
  // used via libopencm3, ISR are routed on pprz/opencm3 and cannot
  // be used concurrently by chibios api
  // Should be fixed when using chibios-rt branch
  while (!chThdShouldTerminate() && antiBounce) {
    const bool_t usbConnected = palReadPad (GPIOA, GPIOA_OTG_FS_VBUS);
    if (usbConnected)
      antiBounce--;
    else
      antiBounce=5;

    chThdSleepMilliseconds(20);
  }
  isRunning = true;
  chRegSetThreadName("UsbStorage:connected");

  /* Stop the logs*/
  chibios_logFinish (true);


  /* connect sdcard sdc interface sdio */
  if (sdioConnect () == false)
    chThdExit (RDY_TIMEOUT);

  /* initialize the USB mass storage driver */
  msdInit(&UMSD1);

  /* start the USB mass storage service */
  msdStart(&UMSD1, &msdConfig);

  /* start the USB driver */
  usbDisconnectBus(&USBD1);
  chThdSleepMilliseconds(1000);
  usbStart(&USBD1, &usbConfig);
  usbConnectBus(&USBD1);

  /* wait for a real usb storage connexion before shutting down autopilot */
  chEvtRegisterMask(&UMSD1.evt_connected, &connected, EVENT_MASK(1));
  chEvtWaitOne(EVENT_MASK(1));

  /* stop autopilot */
  if (pprzThdPtr != NULL) {
    chThdTerminate (pprzThdPtr);
    chThdWait (pprzThdPtr);
    pprzThdPtr = NULL;
  }

  /* wait until usb-storage is unmount and usb cable is unplugged*/
  while (!chThdShouldTerminate() && palReadPad (GPIOA, GPIOA_OTG_FS_VBUS)) {
    chThdSleepMilliseconds(10);
  }


  /* then close open descriptors and reboot autopilot */
  usbDisconnectBus(&USBD1);
  chThdSleepMilliseconds(500);
  msdStop(&UMSD1);
  sdioDisconnect ();

  MCU_RESTART();
  return RDY_OK;
}