void commands_init(void) {
	chThdCreateStatic(detect_thread_wa, sizeof(detect_thread_wa), NORMALPRIO, detect_thread, NULL);
}

void buttonInit(){

    extStart(&EXTD1, &extcfg);
    chThdCreateStatic(waThreadButton, sizeof(waThreadButton),
                    NORMALPRIO + 10, ThreadButton, NULL);
}

  BaseThread::BaseThread(void *workspace, size_t wsize, tprio_t prio) {

    thread_ref = chThdCreateStatic(workspace, wsize, prio, thdstart, this);
  }

//-----------------------------------------------------------------------------
int
kuroBoxInit(void)
{
	halInit();
	chSysInit();

	PWR->CSR &= ~PWR_CSR_EWUP;

	kbg_setLED1(1);
	kbg_setLED2(1);
	kbg_setLED3(1);

	// Serial
	kuroBoxSerialInit(NULL, NULL);

	// start the SPI bus, just use the LCD's SPI config since
	// it's shared with the eeprom
	spiStart(&SPID1, &lcd_cfg.spicfg);
	memset(lcd_buffer, 0, sizeof(lcd_buffer));
	st7565Start(&ST7565D1, &lcd_cfg, &SPID1, lcd_buffer);
	
	// EEPROM
	spiEepromStart(&spiEepromD1, &eeprom_cfg, &SPID1);
	
	// SDIO 
	sdcStart(&SDCD1, &sdio_cfg);
	
	// just blink to indicate we haven't crashed
#ifdef HAVE_BLINK_THREAD
	blinkerThread = chThdCreateStatic(waBlinker, sizeof(waBlinker), NORMALPRIO, thBlinker, NULL);
#endif // HAVE_BLINK_THREAD

	// read config goes here
	// @TODO: add config reading from eeprom
	
	// ADC to monitor voltage levels.
	// start all the ADC stuff!
	kuroBoxADCInit();
	
	// init the screen, this will spawn a thread to keep it updated
	kuroBoxScreenInit();

	// LTC's, interrupt driven, very quick now
	// Note, this module must be init'd after the screen
	kuroBoxTimeInit();

	// the actual logging thread
	kuroBoxWriterInit();

	// this turns on Layer 1 power, this turns on the mosfets controlling the
	// VCC rail. After this, we can start GPS, VectorNav and altimeter
	kbg_setL1PowerOn();

	// wait for it to stabilise, poweron devices, and let them start before continuing
	chThdSleepMilliseconds(500);

	// gps uart
	kuroBoxGPSInit(&SD6);

	VND1.spip = &SPID2;
	VND1.gpdp = &GPTD14;
	kuroBoxVectorNavInit(&VND1, NULL); // use the defaults

	// start I2C here, since it's a shared bus
	i2cStart(&I2CD2, &i2cfg2);

	// and the altimeter, also spawns a thread.
	kuroBoxAltimeterInit();

	// init the menu structure, and thread, if needed
	kuroBoxMenuInit();

	// set initial button state, AFTER menus!
	kuroBoxButtonsInit();

	// read all configs from bksram and load them up thoughout the system
	kuroBoxConfigInit();

	// Feature "A" - just a test feature that sends updates over serial
	kuroBoxExternalDisplayInit();

	// indicate we're ready
	chprintf(DEBG, "%s\n\r\n\r", BOARD_NAME);
	chThdSleepMilliseconds(50);
	kbg_setLED1(0);
	kbg_setLED2(0);
	kbg_setLED3(0);

	// all external interrupts, all the system should now be ready for it
	extStart(&EXTD1, &extcfg);

	return KB_OK; 
}

/*
 * Application entry point.
 */
int main(void) {
  unsigned i;
  gptcnt_t interval, threshold, worst;

  /* Enables FPU exceptions.*/
  nvicEnableVector(FPU_IRQn, 1);

  /*
   * 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();

  /*
   * Prepares the Serial driver 2 and GPT drivers 2 and 3.
   */
  sdStart(&SD2, NULL);          /* Default is 38400-8-N-1.*/
  palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
  palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
  gptStart(&GPTD4, &gpt4cfg);
  gptStart(&GPTD3, &gpt3cfg);

  /*
   * Initializes the worker threads.
   */
  chThdCreateStatic(waWorkerThread, sizeof waWorkerThread,
                    NORMALPRIO - 20, WorkerThread, NULL);
  chThdCreateStatic(waPeriodicThread, sizeof waPeriodicThread,
                    NORMALPRIO - 10, PeriodicThread, NULL);

  /*
   * Test procedure.
   */
  println("");
  println("*** ChibiOS/RT IRQ-STORM-FPU long duration test");
  println("***");
  print("*** Kernel:       ");
  println(CH_KERNEL_VERSION);
  print("*** Compiled:     ");
  println(__DATE__ " - " __TIME__);
#ifdef PORT_COMPILER_NAME
  print("*** Compiler:     ");
  println(PORT_COMPILER_NAME);
#endif
  print("*** Architecture: ");
  println(PORT_ARCHITECTURE_NAME);
#ifdef PORT_CORE_VARIANT_NAME
  print("*** Core Variant: ");
  println(PORT_CORE_VARIANT_NAME);
#endif
#ifdef PORT_INFO
  print("*** Port Info:    ");
  println(PORT_INFO);
#endif
#ifdef PLATFORM_NAME
  print("*** Platform:     ");
  println(PLATFORM_NAME);
#endif
#ifdef BOARD_NAME
  print("*** Test Board:   ");
  println(BOARD_NAME);
#endif
  println("***");
  print("*** System Clock: ");
  printn(STM32_SYSCLK);
  println("");
  print("*** Iterations:   ");
  printn(ITERATIONS);
  println("");
  print("*** Randomize:    ");
  printn(RANDOMIZE);
  println("");

  println("");
  worst = 0;
  for (i = 1; i <= ITERATIONS; i++){
    print("Iteration ");
    printn(i);
    println("");
    saturated = FALSE;
    threshold = 0;
    for (interval = 2000; interval >= 10; interval -= interval / 10) {
      gptStartContinuous(&GPTD4, interval - 1); /* Slightly out of phase.*/
      gptStartContinuous(&GPTD3, interval + 1); /* Slightly out of phase.*/
      chThdSleepMilliseconds(1000);
      gptStopTimer(&GPTD4);
      gptStopTimer(&GPTD3);
      if (!saturated)
        print(".");
      else {
        print("#");
        if (threshold == 0)
          threshold = interval;
//.........这里部分代码省略.........

/**
 * @brief   IRQ storm execution.
 *
 * @param[in] cfg       pointer to the test configuration structure
 *
 * @api
 */
void irq_storm_execute(const irq_storm_config_t *cfg) {
  unsigned i;
  gptcnt_t interval, threshold, worst;

  /* 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);

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

/*
 * Application entry point.
 */
int main(void) {
  thread_t *shelltp = NULL;

  /*
   * 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();

  /*
   * Initializes a serial-over-USB CDC driver.
   */
  sduObjectInit(&SDU1);
  sduStart(&SDU1, &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);

  /*
   * Creating the blinker threads.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1),
                    NORMALPRIO + 10, Thread1, NULL);
  chThdCreateStatic(waThread2, sizeof(waThread2),
                    NORMALPRIO + 10, Thread2, NULL);

  /*
   * Normal main() thread activity, in this demo it just performs
   * a shell respawn upon its termination.
   */
  while (true) {
    if (!shelltp) {
      if (SDU1.config->usbp->state == USB_ACTIVE) {
        /* Spawns a new shell.*/
        shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
      }
    }
    else {
      /* If the previous shell exited.*/
      if (chThdTerminatedX(shelltp)) {
        /* Recovers memory of the previous shell.*/
        chThdRelease(shelltp);
        shelltp = NULL;
      }
    }
    chThdSleepMilliseconds(500);
  }
}

void cmd_ledctrl(BaseSequentialStream *chp, int argc, char *argv[])
{
  int i;
  if (argc >= 1 && strcmp(argv[0], "test1") == 0)
    {
      chprintf(chp, "Red ...\r\n");
      for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
        {
          ledstripe_framebuffer[i].red = 255;
          ledstripe_framebuffer[i].green = 0;
          ledstripe_framebuffer[i].blue = 0;
        }
      chThdSleepMilliseconds(5000);
      chprintf(chp, "Green ...\r\n");
      for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
        {
          ledstripe_framebuffer[i].green = 255;
          ledstripe_framebuffer[i].red = 0;
          ledstripe_framebuffer[i].blue = 0;
        }
      chThdSleepMilliseconds(5000);
      chprintf(chp, "Blue ...\r\n");
      for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
        {
          ledstripe_framebuffer[i].blue = 255;
          ledstripe_framebuffer[i].red = 0;
          ledstripe_framebuffer[i].green = 0;
        }
      chThdSleepMilliseconds(5000);
    }
  else if (argc >= 1 && strcmp(argv[0], "on") == 0)
    {
      for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
        {
          ledstripe_framebuffer[i].red = 255;
          ledstripe_framebuffer[i].green = 255;
          ledstripe_framebuffer[i].blue = 255;
        }
    }
  else if (argc >= 1 && strcmp(argv[0], "off") == 0)
    {
      for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
        {
          ledstripe_framebuffer[i].red = 0;
          ledstripe_framebuffer[i].green = 0;
          ledstripe_framebuffer[i].blue = 0;
        }
    }
  else if (argc >= 4 && strcmp(argv[0], "end") == 0)
    {
      int red = atoi(argv[1]);
      int green = atoi(argv[2]);
      int blue = atoi(argv[3]);
      for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
        {
          ledstripe_framebuffer[i].red = 0;
          ledstripe_framebuffer[i].green = 0;
          ledstripe_framebuffer[i].blue = 0;
        }
      for (i = LED_END_POSTION; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
        {
          ledstripe_framebuffer[i].red = red;
          ledstripe_framebuffer[i].green = green;
          ledstripe_framebuffer[i].blue = blue;
        }
    }
  else if (argc >= 1 && strcmp(argv[0], "start") == 0)
    {
      chprintf(chp, "Start led thread ...");
      chThdCreateStatic(waLEDstripBlink, sizeof(waLEDstripBlink), NORMALPRIO,
          ledThread, NULL);
      chprintf(chp, " Done\r\n");
    }
  else if (argc >= 1 && strcmp(argv[0], "show") == 0)
    {
      chprintf(chp, "\r\n");
      for (i = 0; i < LEDSTRIPE_FRAMEBUFFER_SIZE; i++)
        {
          chprintf(chp, "%.2X%.2X%.2X ",
              ledstripe_framebuffer[i].red,
              ledstripe_framebuffer[i].green,
              ledstripe_framebuffer[i].blue);
        }
        chprintf(chp, "\r\n");
    }
  else if (argc >= 1)  /* Update the LEDs directly */
  {
	  int i,j= 0, color = 0;
	  long int number = 0;
	  int length = strlen(argv[0]);
	  char pEnd[2] = { '0', '0' };
	  for(i=0; i < length; i+=2){
		  memcpy(pEnd, argv[0] +i, 2);
		  number = strtol(pEnd, NULL, 16);
		  chprintf(chp, "%d %d\r\n", j, number);
		  switch(color)
		  {
		  case 0:
			  ledstripe_framebuffer[j].red = number;
			  break;
//.........这里部分代码省略.........

void app_gurgalof_init(void) {
	chThdCreateStatic(gurgalof_thread_wa, sizeof(gurgalof_thread_wa), NORMALPRIO, gurgalof_thread, NULL);
}

static void evt2_execute(void) {
  eventmask_t m;
  event_listener_t el1, el2;
  systime_t target_time;

  /*
   * Test on chEvtWaitOne() without wait.
   */
  chEvtAddEvents(7);
  m = chEvtWaitOne(ALL_EVENTS);
  test_assert(1, m == 1, "single event error");
  m = chEvtWaitOne(ALL_EVENTS);
  test_assert(2, m == 2, "single event error");
  m = chEvtWaitOne(ALL_EVENTS);
  test_assert(3, m == 4, "single event error");
  m = chEvtGetAndClearEvents(ALL_EVENTS);
  test_assert(4, m == 0, "stuck event");

  /*
   * Test on chEvtWaitOne() with wait.
   */
  test_wait_tick();
  target_time = chVTGetSystemTime() + MS2ST(50);
  threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
                                 thread1, chThdGetSelfX());
  m = chEvtWaitOne(ALL_EVENTS);
  test_assert_time_window(5, target_time, target_time + ALLOWED_DELAY);
  test_assert(6, m == 1, "single event error");
  m = chEvtGetAndClearEvents(ALL_EVENTS);
  test_assert(7, m == 0, "stuck event");
  test_wait_threads();

  /*
   * Test on chEvtWaitAny() without wait.
   */
  chEvtAddEvents(5);
  m = chEvtWaitAny(ALL_EVENTS);
  test_assert(8, m == 5, "unexpected pending bit");
  m = chEvtGetAndClearEvents(ALL_EVENTS);
  test_assert(9, m == 0, "stuck event");

  /*
   * Test on chEvtWaitAny() with wait.
   */
  test_wait_tick();
  target_time = chVTGetSystemTime() + MS2ST(50);
  threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
                                 thread1, chThdGetSelfX());
  m = chEvtWaitAny(ALL_EVENTS);
  test_assert_time_window(10, target_time, target_time + ALLOWED_DELAY);
  test_assert(11, m == 1, "single event error");
  m = chEvtGetAndClearEvents(ALL_EVENTS);
  test_assert(12, m == 0, "stuck event");
  test_wait_threads();

  /*
   * Test on chEvtWaitAll().
   */
  chEvtObjectInit(&es1);
  chEvtObjectInit(&es2);
  chEvtRegisterMask(&es1, &el1, 1);
  chEvtRegisterMask(&es2, &el2, 4);
  test_wait_tick();
  target_time = chVTGetSystemTime() + MS2ST(50);
  threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
                                 thread2, "A");
  m = chEvtWaitAll(5);
  test_assert_time_window(13, target_time, target_time + ALLOWED_DELAY);
  m = chEvtGetAndClearEvents(ALL_EVENTS);
  test_assert(14, m == 0, "stuck event");
  test_wait_threads();
  chEvtUnregister(&es1, &el1);
  chEvtUnregister(&es2, &el2);
  test_assert(15, !chEvtIsListeningI(&es1), "stuck listener");
  test_assert(16, !chEvtIsListeningI(&es2), "stuck listener");
}

/**
 * @brief   Application entry point.
 * @details
 */
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(&SDU1);
  sduStart(&SDU1, &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.
   */
  usbStop(serusbcfg.usbp);
  usbDisconnectBus(serusbcfg.usbp);
  chThdSleepMilliseconds(500);
  usbConnectBus(serusbcfg.usbp);
  usbStart(serusbcfg.usbp, &usbcfg);

  /* Activates the serial driver 4 using the driver's default configuration. */
  sdStart(&SD4, NULL);

  /* Activates the I2C driver 2. */
  i2cInit();
  i2cStart(&I2CD2, &i2cfg_d2);
    
  /* Enables the CRC peripheral clock. */
  rccEnableCRC(FALSE);

  /* Loads settings from external EEPROM chip. */
  if (eepromLoadSettings()) {
    g_boardStatus |= EEPROM_24C02_DETECTED;
  }

  /* Initializes the MPU6050 sensor. */
  if (mpu6050Init()) {
    g_boardStatus |= MPU6050_LOW_DETECTED;

    /* Creates a taken binary semaphore. */
    chBSemInit(&bsemNewDataReady, TRUE);

    /* Creates the MPU6050 polling thread and attitude calculation thread. */
    chThdCreateStatic(waPollMPU6050Thread, sizeof(waPollMPU6050Thread),
      NORMALPRIO + 1, PollMPU6050Thread, NULL);
    chThdCreateStatic(waAttitudeThread, sizeof(waAttitudeThread),
      HIGHPRIO, AttitudeThread, NULL);

    /* Starts motor drivers. */
    pwmOutputStart();

    /* Starts ADC and ICU input drivers. */
    mixedInputStart();
  }

  /* Creates the blinker thread. */
  chThdCreateStatic(waBlinkerThread, sizeof(waBlinkerThread),
    LOWPRIO, BlinkerThread, NULL);

  /* Normal main() thread activity. */
  while (TRUE) {
    g_chnp = serusbcfg.usbp->state == USB_ACTIVE ? (BaseChannel *)&SDU1 : (BaseChannel *)&SD4;
    telemetryReadSerialData();
    if (eepromIsDataLeft()) {
      eepromContinueSaving();  
    }
    chThdSleepMilliseconds(TELEMETRY_SLEEP_MS);
  }
  /* This point should never be reached. */
  return 0;
}

/* Use this AFTER ChibiOS threads have started */
void iwdgPostStart(void) {
	iwdg_lld_reload();
	chThdCreateStatic(wa, sizeof(wa), NORMALPRIO, wdthread, &timeout_ms);
}

void app_sten_init(void) {
	chThdCreateStatic(uart_thread_wa, sizeof(uart_thread_wa), NORMALPRIO - 1, uart_thread, NULL);
}

int main(void) {
	static const evhandler_t evhndl_main[]       = {
			extdetail_WKUP_button_handler
	};
	struct EventListener     el0;
	/*
	 * 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();

	extdetail_init();

	palSetPad(GPIOC, GPIOC_LED);

	/*!
	 * GPIO Pins for generating pulses at data input detect and data output send.
	 * Used for measuring latency timing of data
	 *
	 * \sa board.h
	 */
	palClearPad(  TIMEOUTPUT_PORT, TIMEOUTPUT_PIN);
	palSetPadMode(TIMEOUTPUT_PORT, TIMEOUTPUT_PIN, PAL_MODE_OUTPUT_PUSHPULL);
	palSetPad(    TIMEINPUT_PORT, TIMEINPUT_PIN);
	palSetPadMode(TIMEINPUT_PORT, TIMEINPUT_PIN, PAL_MODE_OUTPUT_PUSHPULL );

	/*
	 * I2C2 I/O pins setup
	 */
	palSetPadMode(si_i2c_connections.i2c_sda_port , si_i2c_connections.i2c_sda_pad,
			PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_OSPEED_HIGHEST |PAL_STM32_PUDR_FLOATING );
	palSetPadMode(si_i2c_connections.i2c_scl_port, si_i2c_connections.i2c_scl_pad,
			PAL_MODE_ALTERNATE(4) | PAL_STM32_OSPEED_HIGHEST  | PAL_STM32_PUDR_FLOATING);


	palSetPad(si_i2c_connections.i2c_scl_port,  si_i2c_connections.i2c_scl_pad );


	static const ShellCommand commands[] = {
	        {"mem", cmd_mem},
	        {"threads", cmd_threads},
	        {NULL, NULL}
	};
	usbSerialShellStart(commands);

	mpu9150_start(&I2CD2);

	i2cStart(mpu9150_driver.i2c_instance, &si_i2c_config);

	mpu9150_init(mpu9150_driver.i2c_instance);

	/* Administrative threads */
	chThdCreateStatic(waThread_blinker,      sizeof(waThread_blinker),      NORMALPRIO, Thread_blinker,      NULL);
	chThdCreateStatic(waThread_indwatchdog,  sizeof(waThread_indwatchdog),  NORMALPRIO, Thread_indwatchdog,  NULL);

	/* MAC */
	/*!
	 * Use a locally administered MAC address second LSbit of MSB of MAC should be 1
	 * Use unicast address LSbit of MSB of MAC should be 0
	 */
	data_udp_init();

	chThdCreateStatic(wa_lwip_thread            , sizeof(wa_lwip_thread)            , NORMALPRIO + 2, lwip_thread            , SENSOR_LWIP);
	chThdCreateStatic(wa_data_udp_send_thread   , sizeof(wa_data_udp_send_thread)   , NORMALPRIO    , data_udp_send_thread   , NULL);

	/* i2c MPU9150 */
	chThdCreateStatic(waThread_mpu9150_int,       sizeof(waThread_mpu9150_int)      , NORMALPRIO    , Thread_mpu9150_int,  NULL);

	/* SPI ADIS */
	//chThdCreateStatic(waThread_adis_dio1,    sizeof(waThread_adis_dio1),    NORMALPRIO, Thread_adis_dio1,    NULL);
	//chThdCreateStatic(waThread_adis_newdata, sizeof(waThread_adis_newdata), NORMALPRIO, Thread_adis_newdata, NULL);


	/*! Activates the EXT driver 1. */
	extStart(&EXTD1, &extcfg);


	chEvtRegister(&extdetail_wkup_event, &el0, 0);
	while (TRUE) {
		chEvtDispatch(evhndl_main, chEvtWaitOneTimeout((eventmask_t)1, MS2ST(500)));
	}
}

/*
 * Application entry point.
 */
int main(void) {
  thread_t *shelltp1 = NULL;
  thread_t *shelltp2 = NULL;

  /*
   * 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(&SDU1);
  sduStart(&SDU1, &serusbcfg1);
  sduObjectInit(&SDU2);
  sduStart(&SDU2, &serusbcfg2);

  /*
   * 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(serusbcfg1.usbp);
  chThdSleepMilliseconds(1500);
  usbStart(serusbcfg1.usbp, &usbcfg);
  usbConnectBus(serusbcfg1.usbp);

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

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

  /*
   * Normal main() thread activity, in this demo it does nothing except
   * sleeping in a loop and check the button state.
   */
  while (true) {
    if (!shelltp1 && (SDU1.config->usbp->state == USB_ACTIVE))
      shelltp1 = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
    else if (chThdTerminatedX(shelltp1)) {
      chThdRelease(shelltp1);   /* Recovers memory of the previous shell.   */
      shelltp1 = NULL;          /* Triggers spawning of a new shell.        */
    }
    if (!shelltp2 && (SDU2.config->usbp->state == USB_ACTIVE))
      shelltp2 = shellCreate(&shell_cfg2, SHELL_WA_SIZE, NORMALPRIO);
    else if (chThdTerminatedX(shelltp2)) {
      chThdRelease(shelltp2);   /* Recovers memory of the previous shell.   */
      shelltp2 = NULL;          /* Triggers spawning of a new shell.        */
    }
    chThdSleepMilliseconds(1000);
  }
}

/*
 * 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();

  /*
   * Activates the serial driver 2 and SDC driver 1 using default
   * configuration.
   */
  sdStart(&SD6, NULL);

  BaseSequentialStream *chp = (BaseSequentialStream *)&SD6;
  chprintf(chp, "running main()\r\n");
  chThdSleepMilliseconds(50);

#if STM32_USB_USE_OTG2
  USBDriver *usb_driver = &USBD2;
#else
  USBDriver *usb_driver = &USBD1;
#endif

  /*
   * Activates the card insertion monitor.
   */
  init_sd();
  chprintf(chp, "done starting SDC\r\n");
  const bool_t sdcConnectStatus = sdcConnect(&SDCD1);
  if( sdcConnectStatus != CH_SUCCESS ) {
    chprintf(chp, "failed to connect to SD Card, sdcConnectStatus = %u\r\n", sdcConnectStatus);
    for(;;) {
      chThdSleepMilliseconds(3000);
    }
  }

  chprintf(chp, "setting up MSD\r\n");
  const usb_msd_driver_state_t msd_driver_state = msdInit(usb_driver, (BaseBlockDevice*)&SDCD1, &UMSD1, USB_MS_DATA_EP, USB_MSD_INTERFACE_NUMBER);
  if( msd_driver_state != USB_MSD_DRIVER_OK ) {
    chprintf(chp, "Error initing USB MSD, %d %s\r\n", msd_driver_state, usb_msd_driver_state_t_to_str(msd_driver_state));
  }
  UMSD1.chp = chp;

  chprintf(chp, "Initializing SDU1...\r\n");
  serusbcfg.usbp = usb_driver;
  sduObjectInit(&SDU1);

  /*Disconnect the USB Bus*/
  usbDisconnectBus(usb_driver);
  chThdSleepMilliseconds(200);

  /*Start the useful functions*/
  sduStart(&SDU1, &serusbcfg);
  msdStart(&UMSD1);
  usbStart(usb_driver, &msd_usb_config);

  /*Connect the USB Bus*/
  usbConnectBus(usb_driver);

  /*
   * Creates the blinker thread.
   */
  chprintf(chp, "starting blinker thread\r\n");
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

  while (TRUE) {
    palTogglePad(GPIOC, GPIOC_LED);
    chThdSleepMilliseconds(500);
  }
}

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

  size_t start = 1100;
  size_t end = 1150;
  volatile int32_t adc_its = 0;
  volatile int32_t spi_its = 0;
  volatile int32_t uart_its = 0;
  volatile int32_t adc_its_idle = 0;
  volatile int32_t spi_its_idle = 0;
  volatile int32_t uart_its_idle = 0;
  volatile uint32_t idle_thread_cnt = 0;

  #if EMCNAND_USE_KILL_TEST
  size_t kill = 8000;
  #endif

  /*
   * 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();

  emcStart(&EMCD1, &emccfg);
#if !STM32_EMC_EMCNAND_USE_FSMC_INT
  extStart(&EXTD1, &extcfg);
#endif
  emcnandStart(&EMCNANDD1, &nandcfg);

  chThdSleepMilliseconds(4000);

  chThdCreateStatic(fsmcIdleThreadWA,
        sizeof(fsmcIdleThreadWA),
        NORMALPRIO - 20,
        fsmcIdleThread,
        NULL);

  nand_wp_release();

  dma_storm_adc_start();
  dma_storm_uart_start();
  dma_storm_spi_start();
  T = chVTGetSystemTimeX();
  general_test(&EMCNANDD1, start, end, 1);
  T = chVTGetSystemTimeX() - T;
  adc_its = dma_storm_adc_stop();
  uart_its = dma_storm_uart_stop();
  spi_its = dma_storm_spi_stop();
  chSysLock();
  idle_thread_cnt = IdleCnt;
  IdleCnt = 0;
  chSysUnlock();

  dma_storm_adc_start();
  dma_storm_uart_start();
  dma_storm_spi_start();
  chThdSleep(T);
  adc_its_idle = dma_storm_adc_stop();
  uart_its_idle = dma_storm_uart_stop();
  spi_its_idle = dma_storm_spi_stop();

  osalDbgCheck(idle_thread_cnt > (IdleCnt / 4));
  osalDbgCheck(abs(adc_its - adc_its_idle)   < (adc_its_idle  / 20));
  osalDbgCheck(abs(uart_its - uart_its_idle) < (uart_its_idle / 20));
  osalDbgCheck(abs(spi_its - spi_its_idle)   < (spi_its_idle  / 10));

  ecc_test(&EMCNANDD1, end);

#if EMCNAND_USE_KILL_TEST
  kill_block(&EMCNANDD1, kill);
#endif

  nand_wp_assert();

  /*
   * Normal main() thread activity, in this demo it does nothing.
   */
  while (TRUE) {
    chThdSleepMilliseconds(500);
  }
}

/**
 * @brief   Create statically allocated shell thread.
 *
 * @param[in] scp       pointer to a @p ShellConfig object
 * @param[in] wsp       pointer to a working area dedicated to the shell thread stack
 * @param[in] size      size of the shell working area
 * @param[in] prio      priority level for the new shell
 * @return              A pointer to the shell thread.
 */
Thread *shellCreateStatic(const ShellConfig *scp, void *wsp,
                          size_t size, tprio_t prio) {

  return chThdCreateStatic(wsp, size, prio, shell_thread, (void *)scp);
}

/*
 * Application entry point.
 */
int main(void) {
    color_t color = Black;
    uint16_t pen = 0;
    Thread *shelltp = NULL;

  /*
   * 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();

    gdispInit();
    ginputGetMouse(0);
    gdispSetOrientation(GDISP_ROTATE_90);

    drawScreen();

  /*
   * Initializes a serial-over-USB CDC driver.
   */
  sduObjectInit(&SDU1);
  sduStart(&SDU1, &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);

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

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

  /*
   * Normal main() thread activity, in this demo it does nothing except
   * sleeping in a loop and check the button state.
   */
  while (TRUE) {
    if (!shelltp && (SDU1.config->usbp->state == USB_ACTIVE))
      shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
    else if (chThdTerminated(shelltp)) {
      chThdRelease(shelltp);    /* Recovers memory of the previous shell.   */
      shelltp = NULL;           /* Triggers spawning of a new shell.        */
    }

    ginputGetMouseStatus(0, &ev);
    if (!(ev.current_buttons & GINPUT_MOUSE_BTN_LEFT))
    {
        chThdSleepMicroseconds(500); // Senza questo sleep l'USB non parte
        continue;
    }

    /* inside color box ? */
    if(ev.y >= OFFSET && ev.y <= COLOR_SIZE) {
      if(GET_COLOR(0))   color = Black;
      else if(GET_COLOR(1))   color = Red;
      else if(GET_COLOR(2))   color = Yellow;
      else if(GET_COLOR(3))   color = Green;
      else if(GET_COLOR(4))   color = Blue;
      else if(GET_COLOR(5))   color = White;

    /* inside pen box ? */
    } else if(ev.x >= OFFSET && ev.x <= PEN_SIZE) {
      if(GET_PEN(1))             pen = 0;
      else if(GET_PEN(2))             pen = 1;
      else if(GET_PEN(3))             pen = 2;
      else if(GET_PEN(4))             pen = 3;
      else if(GET_PEN(5))             pen = 4;

    /* inside drawing area ? */
    } else if(DRAW_AREA(ev.x, ev.y)) {
      if(pen == 0)
         gdispDrawPixel(ev.x, ev.y, color);
      else
          gdispFillCircle(ev.x, ev.y, pen, color);
    }
  }
}

/**
 * @brief Initialize the HMC5883 sensor thread
 */
void hmc5883_init(const struct hmc5883_cfg *cfg)
{
	dev_cfg = cfg;
	thdp_hmc5883 = chThdCreateStatic(wa_hmc5883, sizeof(wa_hmc5883), HIGHPRIO, thd_hmc5883, NULL);
}