int gnss_init(void)
{
	int use_geos = 0, use_1k161 = 0;
	/* Detecting receiver */
	while(!use_geos && !use_1k161) {
		if (detect_geos()) {
			pr_debug("Geostar GeoS-1M detected\r\n");
			use_geos = 1;
		} else if (detect_1k161()) {
			pr_debug("RIRV 1K-161 detected\r\n");
			use_1k161 = 1;
		}
	}
	if (use_geos) {
		chThdCreateStatic(wa_geo, sizeof(wa_geo), NORMALPRIO, geo_thread, NULL);
		chThdCreateStatic(wa_tel, sizeof(wa_tel), NORMALPRIO, tel_thread, NULL);
		chThdCreateStatic(wa_sat, sizeof(wa_sat), NORMALPRIO, sat_thread, NULL);
	}
	if (use_1k161) {
		chThdCreateFromHeap(NULL, 128, NORMALPRIO, geo_thread_1k161, NULL);
		chThdCreateFromHeap(NULL, 208, NORMALPRIO, misc_thread_1k161, NULL);
#if 0
		chThdCreateStatic(wa_geo, sizeof(wa_geo), NORMALPRIO, geo_thread_1k161, NULL);
		chThdCreateStatic(wa_tel, sizeof(wa_tel), NORMALPRIO, misc_thread_1k161, NULL);
#endif
	}
	if (use_geos)
		return 1;
	else if (use_1k161)
		return 2;
	return 0;
}

static void cmd_rms(BaseSequentialStream *chp) {

  /* 
   * Creating dynamic threads using the heap allocator
  */
  Thread *tp1 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-2, thread1, chp);
  Thread *tp2 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO, thread2, chp);
  Thread *tp3 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-1, thread3, chp);
  Thread *tp4 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-3, thread4, chp);


  chThdSleepUntil(chTimeNow() + MS2ST(500));

  /*
   * Try to kill threads
  */
  chThdTerminate(tp1);
  chThdTerminate(tp2);
  chThdTerminate(tp3);
  chThdTerminate(tp4);

  /*
   * Wait for the thread to terminate (if it has not terminated
   * already) then get the thread exit message (msg) and returns the
   * terminated thread memory to the heap.
   */
  msg_t msg = chThdWait(tp1);
  msg = chThdWait(tp2);
  msg = chThdWait(tp3);
  msg = chThdWait(tp4);
}

static void dyn1_execute(void) {
  size_t n, sz;
  void *p1;
  tprio_t prio = chThdGetPriority();

  (void)chHeapStatus(&heap1, &sz);
  /* Starting threads from the heap. */
  threads[0] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
                                   prio-1, thread, "A");
  threads[1] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
                                   prio-2, thread, "B");
  /* Allocating the whole heap in order to make the thread creation fail.*/
  (void)chHeapStatus(&heap1, &n);
  p1 = chHeapAlloc(&heap1, n);
  threads[2] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
                                   prio-3, thread, "C");
  chHeapFree(p1);

  test_assert(1, (threads[0] != NULL) &&
                 (threads[1] != NULL) &&
                 (threads[2] == NULL) &&
                 (threads[3] == NULL) &&
                 (threads[4] == NULL),
                 "thread creation failed");

  /* Claiming the memory from terminated threads. */
  test_wait_threads();
  test_assert_sequence(2, "AB");

  /* Heap status checked again.*/
  test_assert(3, chHeapStatus(&heap1, &n) == 1, "heap fragmented");
  test_assert(4, n == sz, "heap size changed");
}

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

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

  /*
   * LSM303DLHC Object Initialization
   */
  lsm303dlhcObjectInit(&LSM303DLHCD1);

  /*
   * Activates the LSM303DLHC driver.
   */
  lsm303dlhcStart(&LSM303DLHCD1, &lsm303dlhccfg);

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

  while(TRUE) {
    if (SDU1.config->usbp->state == USB_ACTIVE) {
      thread_t *shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
                                              "shell", NORMALPRIO + 1,
                                              shellThread, (void *)&shell_cfg1);
      chThdWait(shelltp);               /* Waiting termination.             */
    }
    chThdSleepMilliseconds(1000);
  }
  lsm303dlhcStop(&LSM303DLHCD1);
  return 0;
}

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

  if (argc != 1) {
    chprintf(chp, "Usage: stilt <Hz>\r\n");
    return;
  }

  period = (1000 / atoi(argv[0]));

  if (gyrotp == NULL)
    gyrotp = gyroRun(&SPI_DRIVER, NORMALPRIO);
  if (acctp == NULL)
    acctp = accRun(&I2C_DRIVER, NORMALPRIO);

  tp = chThdCreateFromHeap(NULL, WA_SIZE_1K, NORMALPRIO, stream_tilt_thread,
                           (void *)&period);

  if (tp == NULL) {
    chprintf(chp, "out of memory\r\n");
    return;
  }

  return;
}

static void dyn3_execute(void) {
  Thread *tp;
  tprio_t prio = chThdGetPriority();

  /* Testing references increase/decrease and final detach.*/
  tp = chThdCreateFromHeap(&heap1, WA_SIZE, prio-1, thread, "A");
  test_assert(1, tp->p_refs == 1, "wrong initial reference counter");
  chThdAddRef(tp);
  test_assert(2, tp->p_refs == 2, "references increase failure");
  chThdRelease(tp);
  test_assert(3, tp->p_refs == 1, "references decrease failure");

  /* Verify the new threads count.*/
  test_assert(4, regfind(tp), "thread missing from registry");
  test_assert(5, regfind(tp), "thread disappeared");

  /* Detach and let the thread execute and terminate.*/
  chThdRelease(tp);
  test_assert(6, tp->p_refs == 0, "detach failure");
  test_assert(7, tp->p_state == THD_STATE_READY, "invalid state");
  test_assert(8, regfind(tp), "thread disappeared");
  test_assert(9, regfind(tp), "thread disappeared");
  chThdSleepMilliseconds(50);           /* The thread just terminates.      */
  test_assert(10, tp->p_state == THD_STATE_FINAL, "invalid state");

  /* Clearing the zombie by scanning the registry.*/
  test_assert(11, regfind(tp), "thread disappeared");
  test_assert(12, !regfind(tp), "thread still in registry");
}

sys_thread_t sys_thread_new(const char *name, lwip_thread_fn thread,
                            void *arg, int stacksize, int prio) {
  thread_t *tp;

  tp = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(stacksize),
                           name, prio, (tfunc_t)thread, arg);
  return (sys_thread_t)tp;
}

/*-----------------------------------------------------------------------------*/
Thread *
StartTaskWithPriority(tfunc_t pf, tprio_t priority, ... )
{
    static  bool_t  init = TRUE;
    int16_t     i;
    bool_t      memfree = FALSE;

    // First time initialization
    if(init)
        {
        for(i=0;i<RC_TASKS;i++)
            {
            rcTasks[i].pf = NULL;
            rcTasks[i].tp = NULL;
            }
        // not next time
        init = FALSE;
        }

    // Check to see of this function is already in the robotc task list
    for(i=0;i<RC_TASKS;i++)
        {
        if( rcTasks[i].pf == pf )
            return(NULL);
        }

    // check to see of we have exhausted the task array
    for(i=0;i<RC_TASKS;i++)
            {
            if( rcTasks[i].pf == NULL )
                {
                memfree = TRUE;
                break;
                }
            }
    if( !memfree )
        return(NULL);

    Thread *tp;
    tp = chThdCreateFromHeap(NULL, THD_WA_SIZE(TC_THREAD_STACK), priority, vexRobotcTask, (void *)pf );

    // tp will be NULL if memory is exhausted
    if( tp != NULL )
        {
        // Save association of thread and callback
        for(i=0;i<RC_TASKS;i++)
            {
            if( rcTasks[i].tp == NULL )
                {
                rcTasks[i].tp = tp;
                rcTasks[i].pf = pf;
                break;
                }
            }
        }

    return(tp);
}

static void bridge(t_hydra_console *con)
{
    cprintf(con, "Interrupt by pressing user button.\r\n");
    cprint(con, "\r\n", 2);

    thread_t *bthread = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO, bridge_thread, con);
    chThdWait(bthread);

}

/*
 * 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(&SDU1);
  sduStart(&SDU1, &serusbcfg);

  /*
   * Configuring USB DP and DM PINs.
   */
  palSetPadMode(GPIOA, GPIOA_PIN11, PAL_MODE_INPUT);
  palSetPadMode(GPIOA, GPIOA_PIN12, PAL_MODE_INPUT);

  /*
   * 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 (SDU1.config->usbp->state == USB_ACTIVE) {
      thread_t *shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
                                              "shell", NORMALPRIO + 1,
                                              shellThread, (void *)&shell_cfg1);
      chThdWait(shelltp);               /* Waiting termination.             */
    }
    chThdSleepMilliseconds(1000);
  }
}

void
vexAudioPlaySound( int freq, int amplitude, int timems )
{
    static int current_amplitude = DEFAULT_AMPLITUDE;
    int f1, f2;

    // Not available on the Olimex eval card
#if defined (STM32F10X_HD)

    // init first time
    if( vslThread == NULL )
        {
        VSL_Init();
        // init counting semaphore with a value of 0
        chSemInit( &vslSem, 0 );
        vslThread = chThdCreateFromHeap(NULL, AUDIO_WA_SIZE, NORMALPRIO, vexAudioTask, (void *)NULL );
        }

    // try and stop pops
    // a frequency of 0 means silence
    if( freq == 0 )
        {
        freq = 1000;
        amplitude = 0;
        }

    // create waveform
    if( amplitude != current_amplitude )
        {
        VSL_CreateSineWave( amplitude );
        current_amplitude = amplitude;
        }

    // limit range of frequencies 200Hz to 10KHz
    if( freq <   200 ) freq =   200;
    if( freq > 10000 ) freq = 10000;
    
    // calculate prescale and period for the timer
    VSL_Factorize( 72000000L / (32 * freq), &f1, &f2 );

    // ReInit timer
    VSL_InitTimer(f1, f2);

    // Enable DMA for the DAC
    DAC->CR |= (DAC_CR_EN1 << DAC_Channel_1);
    
    /* TIM7 enable counter */
    TIM7->CR1 |= TIM_CR1_CEN;

    // stop after time
    VSL_Counter = timems;
    // this will wake audio thread if necessary
    chSemReset(&vslSem, 0);
#endif
}

/*------------------------------------------------------------------------*
 * Simulator main.                                                        *
 *------------------------------------------------------------------------*/
int main(void) {
  EventListener sd1fel, sd2fel, tel;

  /*
   * HAL initialization.
   */
  halInit();

  /*
   * ChibiOS/RT initialization.
   */
  chSysInit();

  /*
   * Serial ports (simulated) initialization.
   */
  sdStart(&SD1, NULL);
  sdStart(&SD2, NULL);

  /*
   * Shell manager initialization.
   */
  shellInit();
  chEvtRegister(&shell_terminated, &tel, 0);

  /*
   * Console thread started.
   */
  cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1,
                             console_thread, NULL);

  /*
   * Initializing connection/disconnection events.
   */
  cputs("Shell service started on SD1, SD2");
  cputs("  - Listening for connections on SD1");
  (void) sdGetAndClearFlags(&SD1);
  chEvtRegister(&SD1.sevent, &sd1fel, 1);
  cputs("  - Listening for connections on SD2");
  (void) sdGetAndClearFlags(&SD2);
  chEvtRegister(&SD2.sevent, &sd2fel, 2);

  /*
   * Events servicing loop.
   */
  while (!chThdShouldTerminate())
    chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));

  /*
   * Clean simulator exit.
   */
  chEvtUnregister(&SD1.sevent, &sd1fel);
  chEvtUnregister(&SD2.sevent, &sd2fel);
  return 0;
}

/*------------------------------------------------------------------------*
 * Simulator main.                                                        *
 *------------------------------------------------------------------------*/
int main(void) {
  EventListener sd1fel, sd2fel, tel;

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

  /*
   * Serial ports (simulated) initialization.
   */
  sdStart(&SD1, NULL);
  sdStart(&SD2, NULL);

  /*
   * Shell manager initialization.
   */
  shellInit();
  chEvtRegister(&shell_terminated, &tel, 0);

  /*
   * Console thread started.
   */
  cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1,
                             console_thread, NULL);

  /*
   * Initializing connection/disconnection events.
   */
  cputs("Shell service started on SD1, SD2");
  cputs("  - Listening for connections on SD1");
  (void) chIOGetAndClearFlags(&SD1);
  chEvtRegister(chIOGetEventSource(&SD1), &sd1fel, 1);
  cputs("  - Listening for connections on SD2");
  (void) chIOGetAndClearFlags(&SD2);
  chEvtRegister(chIOGetEventSource(&SD2), &sd2fel, 2);

  /*
   * Events servicing loop.
   */
  while (!chThdShouldTerminate())
    chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));

  /*
   * Clean simulator exit.
   */
  chEvtUnregister(chIOGetEventSource(&SD1), &sd1fel);
  chEvtUnregister(chIOGetEventSource(&SD2), &sd2fel);
  return 0;
}

/**
 * @brief   Creates a thread.
 */
osThreadId osThreadCreate(const osThreadDef_t *thread_def, void *argument) {
  size_t size;

  size = thread_def->stacksize == 0 ? CMSIS_CFG_DEFAULT_STACK :
                                      thread_def->stacksize;
  return (osThreadId)chThdCreateFromHeap(0,
                                         THD_WORKING_AREA_SIZE(size),
                                         NORMALPRIO+thread_def->tpriority,
                                         (tfunc_t)thread_def->pthread,
                                         argument);
}

Thread *magRun(SPIDriver *spip, tprio_t prio) {
	Thread *tp;

//	lsm303_mag_init(spip);
	chThdSleepMilliseconds(200);
	lsm303_mag_update(spip);
	tp = chThdCreateFromHeap(NULL, MAG_WA_SIZE, prio, lsm303_mag_update_thread, (void*) spip);
	extChannelEnable(&EXTD1, GPIOA_AM_INT2);

	return tp;
}

Thread *gyroRun(SPIDriver *spip, tprio_t prio) {
  Thread *tp;

  init_l3g4200d(spip);
  tp = chThdCreateFromHeap(NULL, GYRO_WA_SIZE, prio, l3g4200d_update_thread,
                           (void*)spip);
  extChannelEnable(&EXTD1, GYRO_INT2);
  l3g4200d_update(spip);

  return tp;
}

void
screen_saver_create()
{
  screen_saver_t* s = calloc(1, sizeof(screen_saver_t));

  s->screen = widget_create(NULL, &screen_saver_widget_class, s, display_rect);

  chThdCreateFromHeap(NULL, 1024, NORMALPRIO, screen_saver_thread, s);

  gui_msg_subscribe(MSG_TOUCH_INPUT, s->screen);
}

 ChibiosThread(const char *name, uint32_t stack, tprio_t prio, 
               ThreadFunc threadFunc, FinalizeFunc finalizeFunc = NULL, 
               void* param = NULL)
  : mThreadRef(NULL),
    mName(name),
    mThreadFunc(threadFunc),
    mFinalizeFunc(finalizeFunc),
    mParam(param)
 {
     mThreadRef = chThdCreateFromHeap(NULL, stack, prio, thdStart, this);
 } 

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

  palSetLineMode(LINE_ARD_D15, PAL_MODE_ALTERNATE(4) |
                 PAL_STM32_OSPEED_HIGH);
  palSetLineMode(LINE_ARD_D14, PAL_MODE_ALTERNATE(4) |
                 PAL_STM32_OSPEED_HIGH);

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

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

  /*
   * LSM6DS0 Object Initialization
   */
  lsm6ds0ObjectInit(&LSM6DS0D1);

  /*
   * Activates the LSM6DS0 driver.
   */
  lsm6ds0Start(&LSM6DS0D1, &lsm6ds0cfg);

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

  while(TRUE) {

    thread_t *shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
                                            "shell", NORMALPRIO + 1,
                                            shellThread, (void *)&shell_cfg1);
    chThdWait(shelltp);                  /* Waiting termination.             */
    chThdSleepMilliseconds(1000);
  }
  lsm6ds0Stop(&LSM6DS0D1);
  return 0;
}

Thread *magRun(I2CDriver *i2cp, tprio_t prio) {
  Thread *tp;

  lsm303_mag_init(i2cp);
  chThdSleepMilliseconds(200);
  lsm303_mag_update(i2cp);
  tp = chThdCreateFromHeap(NULL, MAG_WA_SIZE, prio, lsm303_mag_update_thread,
                           (void*)i2cp);
  extChannelEnable(&EXTD1, AM_DRDY);

  return tp;
}