bool_t gdispInit(void) {
		bool_t		res;
		unsigned	i;

		/* Mark all the Messages as free */
		for(i=0; i < GDISP_QUEUE_SIZE; i++)
			gdispMsgs[i].action = GDISP_LLD_MSG_NOP;

		/* Initialise our Mailbox, Mutex's and Counting Semaphore.
		 * 	A Mutex is required as well as the Mailbox and Thread because some calls have to be synchronous.
		 *	Synchronous calls get handled by the calling thread, asynchronous by our worker thread.
		 */
		chMBInit(&gdispMailbox, gdispMailboxQueue, sizeof(gdispMailboxQueue)/sizeof(gdispMailboxQueue[0]));
		chMtxInit(&gdispMutex);
		chMtxInit(&gdispMsgsMutex);
		chSemInit(&gdispMsgsSem, GDISP_QUEUE_SIZE);

		lldThread = chThdCreateStatic(waGDISPThread, sizeof(waGDISPThread), NORMALPRIO, GDISPThreadHandler, NULL);

		/* Initialise driver - synchronous */
		chMtxLock(&gdispMutex);
		res = gdisp_lld_init();
		chMtxUnlock();

		return res;
	}

extern "C" void mainFunc(){
    Serial.begin(38400);
    Serial1.begin(38400, SERIAL_8N1);
    delay(3000);
    Serial.printf("mainFunc\r\n");

    chMtxInit(&kalman_st_mut);
    chMtxInit(&control_mut);

    //chThdCreateStatic(wa_led_thread,           sizeof(wa_led_thread),           NORMALPRIO+3, led_thread,           NULL);
    chThdCreateStatic(wa_flash_thread,         sizeof(wa_flash_thread),         NORMALPRIO+4, flash_thread,         NULL);
    chThdCreateStatic(wa_meas_thread,         sizeof(wa_meas_thread),         NORMALPRIO+3, meas_thread,         NULL);
    chThdCreateStatic(wa_communication_thread, sizeof(wa_communication_thread), NORMALPRIO+2, communication_thread, NULL);
    chThdCreateStatic(wa_receive_thread,       sizeof(wa_receive_thread),       NORMALPRIO+1, receive_thread,       NULL);
    chThdCreateStatic(wa_orientation_thread,   sizeof(wa_orientation_thread),   NORMALPRIO,   orientation_thread,   NULL);
}

FrameBuffer< pages, columns >::FrameBuffer(){
	limits.x_min = columns - 1;
	limits.y_min = pages - 1;
	limits.x_max = 0;
	limits.y_max = 0;
	chMtxInit(&mutex);
}

/**
 * @brief   Initializes the standard part of a @p I2CDriver structure.
 *
 * @param[out] i2cp     pointer to the @p I2CDriver object
 *
 * @init
 */
void i2cObjectInit(I2CDriver *i2cp) {

    i2cp->id_state  = I2C_STOP;
    i2cp->id_config = NULL;
    i2cp->rxbuff_p = NULL;
    i2cp->txbuff_p = NULL;
    i2cp->rxbuf = NULL;
    i2cp->txbuf = NULL;
    i2cp->id_slave_config = NULL;

#if I2C_USE_WAIT
    i2cp->id_thread   = NULL;
#endif /* I2C_USE_WAIT */

#if I2C_USE_MUTUAL_EXCLUSION
#if CH_USE_MUTEXES
    chMtxInit(&i2cp->id_mutex);
#else
    chSemInit(&i2cp->id_semaphore, 1);
#endif /* CH_USE_MUTEXES */
#endif /* I2C_USE_MUTUAL_EXCLUSION */

#if defined(I2C_DRIVER_EXT_INIT_HOOK)
    I2C_DRIVER_EXT_INIT_HOOK(i2cp);
#endif
}

int motor_init(void)
{
	_watchdog_id = watchdog_create(WATCHDOG_TIMEOUT_MSEC);
	if (_watchdog_id < 0) {
		return _watchdog_id;
	}

	int ret = motor_rtctl_init();
	if (ret) {
		return ret;
	}

	chMtxInit(&_mutex);
	chEvtInit(&_setpoint_update_event);

	configure();

	init_filters();
	if (_state.input_voltage < MIN_VALID_INPUT_VOLTAGE || _state.input_voltage > MAX_VALID_INPUT_VOLTAGE) {
		lowsyslog("Motor: Invalid input voltage: %f\n", _state.input_voltage);
		return -1;
	}

	ret = rpmctl_init();
	if (ret) {
		return ret;
	}

	motor_rtctl_stop();

	assert_always(chThdCreateStatic(_wa_control_thread, sizeof(_wa_control_thread),
	                                HIGHPRIO, control_thread, NULL));
	return 0;
}

void oledInit (oledConfig *oledConfig,  struct SerialDriver *oled, const uint32_t baud,
	       GPIO_TypeDef *rstGpio, uint32_t rstPin, enum OledConfig_Device dev)
{
  oledConfig->rstGpio = rstGpio;
  oledConfig->rstPin = rstPin;
  oledConfig->deviceType = dev;

  oledHardReset (oledConfig);


  oledPreInit (oledConfig, 9600);
  oledConfig->serial = (BaseSequentialStream *) oled;
  chMtxInit(&(oledConfig->omutex));
  sdStart(oled, &(oledConfig->serialConfig));
  chThdSleepMilliseconds(10);

  // opaque background
  if (oledConfig->deviceType != TERM_VT100) {
    OLED_KOF (KOF_INT16, "%c%c%c%c", 0xff, 0xdf, 0x00, 0x01); 
  }

  oledClearScreen (oledConfig);
  
  // use greater speed
  if (baud != 9600) 
    oledSetBaud (oledConfig, baud);
}

	MessageQueue(
		uint8_t* const data,
		size_t k
	) : fifo { data, k }
	{
		chMtxInit(&mutex_write);
	}

void errorLedInit(void)
{
#if PROTECT_DATA_WITH_MUTEX
  chMtxInit(&mutex);
#endif
  chThdCreateStatic(waErrorLedThd, sizeof(waErrorLedThd), NORMALPRIO-1, errorLedThd, 0);  
}

void initI2c( void )
{
    // Address pins
    palSetPadMode( ADDR_PORT, ADDR_0_PIN, PAL_MODE_INPUT );
    palSetPadMode( ADDR_PORT, ADDR_1_PIN, PAL_MODE_INPUT );
    palSetPadMode( ADDR_PORT, ADDR_2_PIN, PAL_MODE_INPUT );
    palSetPadMode( GPIOB, 6, PAL_MODE_STM32_ALTERNATE_OPENDRAIN );
    palSetPadMode( GPIOB, 7, PAL_MODE_STM32_ALTERNATE_OPENDRAIN );
    chThdSleepMilliseconds( 100 );

    i2cInit();
    //chThdSleepMilliseconds( 100 );
    i2cStart( &I2CD1, &i2cfg1 );
    //chThdSleepMilliseconds( 200 );

    // Initial values for IOs.
    int16_t i;
    for ( i=0; i<I2C_SLAVES_CNT; i++ )
    {
        outs[i]     = 0;
        pendOuts[i] = 0;
        ins[i]      = 0;
    }

    // Initializing mutex.
    chMtxInit( &mutex );
    // Creating thread.
    chThdCreateStatic( waI2c, sizeof(waI2c), NORMALPRIO, i2cThread, NULL );
}

void initPower( void )
{
    chMtxInit( &g_mutex );
    setPower( 0 );

    chThdCreateStatic( waPower, sizeof(waPower), NORMALPRIO, Power, NULL );
}

static clarityError clarityMgmtInit(clarityAccessPointInformation * apInfo,
                                    clarityUnresponsiveCallback cb)
{

    memset(&mgmtData, 0, sizeof(mgmtData));
    chMtxInit(&mgmtData.mutex);

    clarityMgmtMtxLock();
    mgmtData.ap = apInfo;
    clarityMgmtMtxUnlock();

#if 0
    connectivityMonThd = chThdCreateStatic(connectivityMonThdWorkingArea,
                                sizeof(connectivityMonThdWorkingArea),
                                NORMALPRIO + 1,
                                clarityMgmtConnectivityMonitoringThd,          
                                NULL);         
#endif
    if (cb != NULL)
    {
        unresponsiveCb = cb;
        responseMonThd = chThdCreateStatic(responseMonThdWorkingArea,
                                sizeof(responseMonThdWorkingArea),
                                HIGHPRIO-1,
                                clarityMgmtResponseMonitoringThd,          
                                NULL);       
    }
    return CLARITY_SUCCESS;
}

bool_t osCreateMutex(OsMutex *mutex)
{
   //Initialize the mutex object
   chMtxInit(mutex);

   //Mutex successfully created
   return TRUE;
}

void _heap_init(void) {

#if CH_USE_MUTEXES
  chMtxInit(&hmtx);
#else
  chSemInit(&hsem, 1);
#endif
}

void initRead( void )
{
    palSetPadMode( SEN_PORT, SEN_1, PAL_MODE_INPUT );
    palSetPadMode( SEN_PORT, SEN_2, PAL_MODE_INPUT );
    // Initializing mutex.
    chMtxInit( &mutex );
    // Creating thread.
    chThdCreateStatic( waRead, sizeof(waRead), NORMALPRIO, readThread, NULL );
}

void comm_init(void) {
	myUSBinit();
	packet_init(send_packet, process_packet);

	chMtxInit(&send_mutex);

	// Threads
	chThdCreateStatic(serial_read_thread_wa, sizeof(serial_read_thread_wa), NORMALPRIO, serial_read_thread, NULL);
	chThdCreateStatic(serial_process_thread_wa, sizeof(serial_process_thread_wa), NORMALPRIO, serial_process_thread, NULL);
	chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
}

/**
 *
 * @brief   Creates a non recursive mutex.
 *
 * @returns instance of @p struct pios_mutex or NULL on failure
 *
 */
struct pios_mutex *PIOS_Mutex_Create(void)
{
	struct pios_mutex *mtx = PIOS_malloc(sizeof(struct pios_mutex));

	if (mtx == NULL)
		return NULL;

	chMtxInit(&mtx->mtx);

	return mtx;
}

void initLed( void )
{
	palClearPad( LED_0_PORT,   LED_0_PIN );
	palClearPad( LED_1_PORT,   LED_1_PIN );
	palSetPadMode( LED_0_PORT, LED_0_PIN, PAL_MODE_OUTPUT_PUSHPULL );
	palSetPadMode( LED_1_PORT, LED_1_PIN, PAL_MODE_OUTPUT_PUSHPULL );

	// Initializing mutex.
	chMtxInit( &mutex );
	// Creating thread.
	chThdCreateStatic( waLeds, sizeof(waLeds), NORMALPRIO, ledsThread, NULL );
}

void rawd_init(void)
{
	chMtxInit(&rawd_mutex);

	memset(ncs, sizeof(ncs), 0);

	rawd_th = chThdCreateStatic(rawd_wa, sizeof(rawd_wa),
			LOWPRIO + 2,
			rawd_loop, NULL);

	rawd_push_th = chThdCreateStatic(rawd_push_wa, sizeof(rawd_push_wa),
			LOWPRIO + 2,
			rawd_push_loop, NULL);
}

void mySPIinit(void){
  chMtxInit(&accelMtx);

  // Initializes the SPI driver 1 in order to access the MEMS. The signals are already initialized in the board file.
  spiStart(&SPID1, &spi1cfg);

  // LIS302DL initialization.
  lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG1, 0x43);
  lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG2, 0x00);
  lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG3, 0x00);

  // Start accelerator reading thread.
  chThdCreateStatic(waThread1, sizeof(waThread1),
                    NORMALPRIO + 10, Thread1, NULL);
}

bool oscUsbEnable(bool on)
{
  if (on && osc.usbThd == 0) {
    chMtxInit(&osc.usb.lock);
    osc.usb.sendMessage = usbserialWriteSlip;
    osc.usbThd = chThdCreateStatic(waUsbThd, sizeof(waUsbThd), NORMALPRIO, OscUsbSerialThread, NULL);
    return true;
  }
  if (!on && osc.usbThd != 0) {
    chThdTerminate(osc.usbThd);
    osc.usbThd = 0;
    return true;
  }
  return false;
}