static void cmd_led(BaseSequentialStream *chp, int argc, char *argv[]) {
    const char led_usage[] = "usage: led <led_state>\n\r\tled_state: 0=OFF 1=ON t=TOGGLE\n\r";
    const char led_toggle_text[] = "led TOGGLE\n\r";
    if((argc > 0) && (argc < 2)){
        if(argv[0][0] == '1'){
            palSetPad(GPIOA,3);
            chprintf(chp, "led ON\n\r");
        } else if(argv[0][0] == '0'){
            palClearPad(GPIOA,3);
            chprintf(chp, "led OFF\n\r");
        } else if(argv[0][0] == 't'){
            palTogglePad(GPIOA,3);
            chprintf(chp, led_toggle_text);
        } else if(strcmp("-h",argv[0])==0){
            chprintf(chp, led_usage);
        }else {
            chprintf(chp, "%c is not a valid value for led!\n\r",argv[1][0]);
            chprintf(chp, led_usage);
        }
    } else if(argc == 0){
        palTogglePad(GPIOA,3);
        chprintf(chp, led_toggle_text);
    }else {
        if(argc >= 2){chprintf(chp, "too many arguments!\n\r");}
        //chprintf(chp, led_usage);
    }
}

static msg_t ledsThread( void *arg )
{
    (void)arg;
    chRegSetThreadName( "ld" );
    while ( 1 )
    {
        static uint32_t arg;
    	chMtxLock( &mutex );
            arg = value;
            if ( arg & 1 )
                palTogglePad( LED_0_PORT, LED_0_PIN );
            else
            	palClearPad( LED_0_PORT, LED_0_PIN );

            if ( arg & 2 )
                palTogglePad( LED_1_PORT, LED_1_PIN );
            else
            	palClearPad( LED_1_PORT, LED_1_PIN );
        chMtxUnlock();
        chThdSleepMilliseconds( DURATION_MS );
    	processDfu( DURATION_MS );
    }

    return 0;
}

/*
 * data Received Callback
 * It toggles an LED based on the first received character.
 */
void dataReceived(USBDriver *usbp, usbep_t ep){
    USBOutEndpointState *osp = usbp->epc[ep]->out_state;
    (void) usbp;
    (void) ep;

    if(osp->rxcnt){
        switch(receiveBuf[0]){
            case '1':
                palTogglePad(GPIOD, GPIOD_LED3);
                break;
            case '2':
                palTogglePad(GPIOD, GPIOD_LED4);
                break;
            case '3':
                palTogglePad(GPIOD, GPIOD_LED5);
                break;
            case '4':
                palTogglePad(GPIOD, GPIOD_LED6);
                break;

        }
    }

    /*
     * Initiate next receive
     */
    usbPrepareReceive(usbp, EP_OUT, receiveBuf, OUT_PACKETSIZE);

    chSysLockFromIsr();
    usbStartReceiveI(usbp, EP_OUT);
    chSysUnlockFromIsr();
}

/*
 * Handles the USB driver global events.
 */
static void usb_event(USBDriver *usbp, usbevent_t event) {
  (void) usbp;
  switch (event) {
  case USB_EVENT_RESET:
    palTogglePad(GPIOD, GPIOD_LED6);
    return;
  case USB_EVENT_ADDRESS:
    return;
  case USB_EVENT_CONFIGURED:

    /* Enables the endpoints specified into the configuration.
       Note, this callback is invoked from an ISR so I-Class functions
       must be used.*/
    chSysLockFromIsr();
    usbInitEndpointI(usbp, 1, &ep1config);
    usbInitEndpointI(usbp, 2, &ep2config);
    chSysUnlockFromIsr();
    //allow the main thread to init the transfers
    initUSB =1;
    return;
  case USB_EVENT_SUSPEND:
    return;
  case USB_EVENT_WAKEUP:
    return;
  case USB_EVENT_STALLED:
    return;
  }
  palTogglePad(GPIOD, GPIOD_LED5);
  return;
}

void toggleLedsI( uint32_t arg )
{
    if ( arg & 1 )
        palTogglePad( LED_0_PORT, LED_0_PIN );
    if ( arg & 2 )
        palTogglePad( LED_1_PORT, LED_1_PIN );
}

void toggleLedsImmediate( uint32_t val )
{
    if ( val & 1 )
        palTogglePad( LED_0_PORT, LED_0_PIN );
    if ( val & 2 )
        palTogglePad( LED_1_PORT, LED_1_PIN );
}

static msg_t can_rx(void *p) {
  struct can_instance *cip = p;
  EventListener el;
  CANRxFrame rxmsg;
  (void)p;
  chRegSetThreadName("receiver");
  chEvtRegister(&cip->canp->rxfull_event, &el, 0);
#if SPC5_CAN_USE_FILTERS
  rxFlag = chEvtGetAndClearFlagsI(&el);
#endif
  while(!chThdShouldTerminate()) {
    if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
      continue;
#if !SPC5_CAN_USE_FILTERS
    while (canReceive(cip->canp, CAN_ANY_MAILBOX,
                      &rxmsg, TIME_IMMEDIATE) == RDY_OK) {
      /* Process message.*/
      palTogglePad(PORT_D, cip->led);
    }
#else
    while (canReceive(cip->canp, rxFlag,
                       &rxmsg, TIME_IMMEDIATE) == RDY_OK) {
      /* Process message.*/
      palTogglePad(PORT_D, cip->led);
    }
#endif
  }
  chEvtUnregister(&CAND1.rxfull_event, &el);
  return 0;
}

//-----------------------------------------------------------------------------
void
kuroBox_panic(int msg)
{
	(void)msg;
// this function is doing more harm than good...
#if 0 
	switch( msg )
	{
	case unknown_panic:
	default:
		{
			while(1)
			{
				palTogglePad(GPIOA, GPIOA_LED3);
				chThdSleepMilliseconds(50);async_vn_msg_t
			}			
		}
	case no_panic:
		{
			while(1)
			{
				palTogglePad(GPIOA, GPIOA_LED3);
				chThdSleepMilliseconds(1000);
			}			
		}
	}
#endif
}

/**
 * @brief   Toggle LED
 * @details Toggles one of the two on-board user LEDs (0 or 1).
 *
 * @param[in] num    Which LED to toggle. Either 0 or 1.
 */
void led_toggle(int num)
{
   if (num == 0)
   {
      palTogglePad(GPIOE, GPIOE_LED0);
   }
   else if (num == 1)
   {
      palTogglePad(GPIOD, GPIOD_LED1);
   }
}

void led_toggle(unsigned int led)
{
    if (led & STATUS_LED) {
        palTogglePad(GPIOB, GPIOB_STATUS_LED);
    }
    if (led & CAN1_STATUS_LED) {
        palTogglePad(GPIOB, GPIOB_CAN1_STATUS_LED);
    }
    if (led & CAN1_PWR_LED) {
        palTogglePad(GPIOA, GPIOA_CAN1_PWR_LED);
    }
}

static msg_t heartBeat (void*Arg)
{
	palSetPad (GPIOB, 8);
	palClearPad (GPIOB, 7);
	printf ("\n\rHello world!\n\r");

	while (true)
	{
    	palTogglePad (GPIOB, 8);
		palTogglePad (GPIOB, 7);
		chThdSleepMilliseconds (250);
	}
}

void led_toggle(int num) {
	switch (num) {
	case LED_RED:
		palTogglePad(GPIOE, 0);
		break;

	case LED_GREEN:
		palTogglePad(GPIOE, 1);
		break;

	default:
		break;
	}
}

static msg_t Thread1(void *arg) {

  (void)arg;

  chRegSetThreadName("blinker");
  while (TRUE) {
    palTogglePad(LED_GPIO, LED1);
    palTogglePad(LED_GPIO, LED2);
    palTogglePad(LED_GPIO, LED3);
    palTogglePad(LED_GPIO, LED4);
    chThdSleepMilliseconds(500);
  }
  return 0;
}

static THD_FUNCTION(Thread1, arg) {

  (void)arg;
  chRegSetThreadName("Blinker");
  while(true) {
    if(i2cOk) {
      palSetPad(GPIO_LED_RED, PIN_LED_RED); /* Off red */
      palTogglePad(GPIO_LED_GREEN, PIN_LED_GREEN); /* Blink green */
    } else {
      palSetPad(GPIO_LED_GREEN, PIN_LED_GREEN); /* Off green */
      palTogglePad(GPIO_LED_RED, PIN_LED_RED); /* Blink red */
    }
    chThdSleepMilliseconds(500);
  }
}

THD_FUNCTION(Thread0, arg)
{
    (void)arg;
    if (RCC->CSR & RCC_CSR_WWDGRSTF)
    {
        /* WWDGRST flag set */
        serDbg("\r\n**WWDG Reset!**\r\n\r\n");

        /* Clear reset flags */
        RCC->CSR |= RCC_CSR_RMVF;
    }

    /* WWDG clock counter = (PCLK1 (48MHz)/4096)/8 = 1464Hz (~683 us)  */
    WWDG_SetPrescaler(WWDG_Prescaler_8);

    /* Set Window value to 126; WWDG counter should be refreshed only when the counter
    is below 126 (and greater than 64) otherwise a reset will be generated */
    WWDG_SetWindowValue(126);

    /* Freeze WWDG while core is stopped */
    DBGMCU->APB1FZ |= DBGMCU_APB1_FZ_DBG_WWDG_STOP;

    /* Enable WWDG and set counter value to 127, WWDG timeout = ~683 us * 64 = 43.7 ms
    In this case the refresh window is: ~683 * (127-126)= 0.683ms < refresh window < ~683 * 64 = 43.7ms
    */
    WWDG_Enable(127);
    serDbg("WWDG Started\r\n");
    while (true)
    {
        chThdSleepMilliseconds(25);
        palTogglePad(GPIOC, GPIOC_LED3); /* Watchdog heartbeat */
        WWDG_SetCounter(127);
    }
}

/*
 * Heartbeat thread
 */
static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg)
{
  (void) arg;
  chRegSetThreadName("pprz heartbeat");

  chThdSleepSeconds (SDLOG_START_DELAY);
  if (usbStorageIsItRunning ())
    chThdSleepSeconds (20000); // stuck here for hours
  else
    sdOk = chibios_logInit();

  while (TRUE) {
    palTogglePad (GPIOC, GPIOC_LED3);
    chThdSleepMilliseconds (sdOk == TRUE ? 1000 : 200);
    static uint32_t timestamp = 0;


    // we sync gps time to rtc every 5 seconds
    if (chTimeNow() - timestamp > 5000) {
      timestamp = chTimeNow();
      if (getGpsTimeOfWeek() != 0) {
        setRtcFromGps (getGpsWeek(), getGpsTimeOfWeek());
      }
    }

  }
}

int
main(void)
{
  halInit();
  chSysInit();
  static const evhandler_t evhndl[] = { };

  // CDC.
  sduObjectInit(&SDU1);
  sduStart(&SDU1, &serusbcfg);
  usbDisconnectBus(serusbcfg.usbp);
  chThdSleepMilliseconds(1500);
  usbStart(serusbcfg.usbp, &usbcfg);
  usbConnectBus(serusbcfg.usbp);

  chThdSleepMilliseconds(4000);

  uart_init();
  chThdCreateStatic(uart_thread_wa, sizeof(uart_thread_wa), NORMALPRIO, uart_thread, NULL);

  while (true)
  {
    palTogglePad(GPIOC, GPIOC_LED);
    chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(100)));
  }
}

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

  /* Goal of this testhal */
  if (!check_watermark()){
    write_watermark();
  }

  /* Normal main thread loop. */
  while (TRUE){
    chThdSleepMilliseconds(200);
    palTogglePad(GPIOB, GPIOB_LED_B);
  }

  return 0;
}

static void led(void) {

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

/*
 * 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 serial 1 (UART0) using the driver default configuration.
   */
  sdStart(&SD1, &s0cfg);
  sdPut(&SD1,'B');

  chThdCreateStatic(waSerEcho, sizeof(waSerEcho), NORMALPRIO, thSerEcho, NULL);

  while (!chThdShouldTerminateX()) {
    chThdSleepMilliseconds(1000);
    palTogglePad(TEENSY_PIN13_IOPORT, TEENSY_PIN13);
    sdPut(&SD1,'B');
  }

  return 0;
}