uint8_t i2c_t::CmdWriteRead(uint8_t Addr,
        uint8_t *WPtr, uint8_t WLength,
        uint8_t *RPtr, uint8_t RLength) {
    if(IBusyWait() != OK) return FAILURE;
    // Clear flags
    ii2c->SR1 = 0;
    while(RxIsNotEmpty()) (void)ii2c->DR;   // Read DR until it empty
    ClearAddrFlag();
    // Start transmission
    SendStart();
    if(WaitEv5() != OK) return FAILURE;
    SendAddrWithWrite(Addr);
    if(WaitEv6() != OK) { SendStop(); return FAILURE; }
    // Start TX DMA if needed
    if(WLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        dmaStreamSetMemory0(PDmaTx, WPtr);
        dmaStreamSetTransactionSize(PDmaTx, WLength);
        PRequestingThread = chThdSelf();
        dmaStreamEnable(PDmaTx);
        chSysLock();
        chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
        chSysUnlock();
        dmaStreamDisable(PDmaTx);
    }
    // Read if needed
    if(RLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        // Send repeated start
        SendStart();
        if(WaitEv5() != OK) return FAILURE;
        SendAddrWithRead(Addr);
        if(WaitEv6() != OK) { SendStop(); return FAILURE; }
        // If number of data to be read is 1, then DMA cannot be used
        if(RLength == 1) {
            AckDisable();
            SendStop();
            if(WaitRx() != OK) return FAILURE;
            *RPtr = ReceiveData();
            if(WaitStop() != OK) return FAILURE;
            return OK;
        }
        else {  // more than 1 byte, use DMA
            AckEnable();
            dmaStreamSetMemory0(PDmaRx, RPtr);
            dmaStreamSetTransactionSize(PDmaRx, RLength);
            DmaLastTransferSet(); // Inform DMA that this is last transfer => do not ACK last byte
            PRequestingThread = chThdSelf();
            dmaStreamEnable(PDmaRx);
            chSysLock();
            chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
            chSysUnlock();
            dmaStreamDisable(PDmaRx);
        } // if lng==1
    } // if != 0
    SendStop();
    return OK;
}

uint8_t i2c_t::CmdWriteRead(uint8_t Addr,
        uint8_t *WPtr, uint8_t WLength,
        uint8_t *RPtr, uint8_t RLength) {
    if(IBusyWait() != OK) return FAILURE;
    // Clear flags
    ii2c->SR1 = 0;
    while(RxIsNotEmpty()) (void)ii2c->DR;   // Read DR until it empty
    ClearAddrFlag();
    // Start transmission
    SendStart();
    if(WaitEv5() != OK) return FAILURE;
    SendAddrWithWrite(Addr);
    if(WaitEv6() != OK) { SendStop(); return FAILURE; }
    ClearAddrFlag();
    // Start TX DMA if needed
    if(WLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        dmaStreamSetMemory0(PDmaTx, WPtr);
        dmaStreamSetMode   (PDmaTx, I2C_DMATX_MODE);
        dmaStreamSetTransactionSize(PDmaTx, WLength);
        chSysLock();
        PRequestingThread = chThdSelf();
        dmaStreamEnable(PDmaTx);
        chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
        chSysUnlock();
        dmaStreamDisable(PDmaTx);
    }
    // Read if needed
    if(RLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        // Send repeated start
        SendStart();
        if(WaitEv5() != OK) return FAILURE;
        SendAddrWithRead(Addr);
        if(WaitEv6() != OK) { SendStop(); return FAILURE; }
        // If single byte is to be received, disable ACK before clearing ADDR flag
        if(RLength == 1) AckDisable();
        else AckEnable();
        ClearAddrFlag();
        dmaStreamSetMemory0(PDmaRx, RPtr);
        dmaStreamSetMode   (PDmaRx, I2C_DMARX_MODE);
        dmaStreamSetTransactionSize(PDmaRx, RLength);
        DmaLastTransferSet(); // Inform DMA that this is last transfer => do not ACK last byte
        chSysLock();
        PRequestingThread = chThdSelf();
        dmaStreamEnable(PDmaRx);
        chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
        chSysUnlock();
        dmaStreamDisable(PDmaRx);
    } // if != 0
    else WaitBTF(); // if nothing to read, just stop
    SendStop();
    return OK;
}

/**
 * @brief   Puts the current thread to sleep into the specified state with
 *          timeout specification.
 * @details The thread goes into a sleeping state, if it is not awakened
 *          explicitly within the specified timeout then it is forcibly
 *          awakened with a @p RDY_TIMEOUT low level message. The possible
 *          @ref thread_states are defined into @p threads.h.
 *
 * @param[in] newstate  the new thread state
 * @param[in] time      the number of ticks before the operation timeouts, the
 *                      special values are handled as follow:
 *                      - @a TIME_INFINITE the thread enters an infinite sleep
 *                        state, this is equivalent to invoking
 *                        @p chSchGoSleepS() but, of course, less efficient.
 *                      - @a TIME_IMMEDIATE this value is not allowed.
 *                      .
 * @return              The wakeup message.
 * @retval RDY_TIMEOUT if a timeout occurs.
 *
 * @sclass
 */
msg_t chSchGoSleepTimeoutS(tstate_t newstate, systime_t time) {

  if (TIME_INFINITE != time) {
    VirtualTimer vt;

    chVTSetI(&vt, time, wakeup, currp);
    chSchGoSleepS(newstate);
    if (chVTIsArmedI(&vt))
      chVTResetI(&vt);
  }
  else
    chSchGoSleepS(newstate);
  return currp->p_u.rdymsg;
}

static msg_t lsm303_acc_update_thread(void *p) {
  I2CDriver *i2cp = (I2CDriver *)p;

  while (TRUE) {
    msg_t msg;

    /* Waiting for the IRQ to happen.*/

    chSysLock();
    acctp = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);
    msg = chThdSelf()->p_u.rdymsg;
    chSysUnlock();

    /* If data ready, update all axis.*/
    if (msg == LSM303_ACC_DATA_READY) {
      lsm303_acc_update(i2cp);
      while (status_a != 0x00) {
        lsm303_acc_update(i2cp);
      }
//      chprintf((BaseChannel *)&SERIAL_DRIVER, "ACC T: %d X: %d Y: %d Z: %d\r\n",
//                 acc_update_time, acc_data.x, acc_data.y, acc_data.z);
    }
  }

  return RDY_OK;
}

/**
 * @brief   Performs atomic signal and wait operations on two semaphores.
 *
 * @param[in] sps       pointer to a @p semaphore_t structure to be signaled
 * @param[in] spw       pointer to a @p semaphore_t structure to wait on
 * @return              A message specifying how the invoking thread has been
 *                      released from the semaphore.
 * @retval MSG_OK       if the thread has not stopped on the semaphore or the
 *                      semaphore has been signaled.
 * @retval MSG_RESET    if the semaphore has been reset using @p chSemReset().
 *
 * @api
 */
msg_t chSemSignalWait(semaphore_t *sps, semaphore_t *spw) {
  msg_t msg;

  chDbgCheck((sps != NULL) && (spw != NULL));
  chDbgAssert(((sps->s_cnt >= (cnt_t)0) && queue_isempty(&sps->s_queue)) ||
              ((sps->s_cnt < (cnt_t)0) && queue_notempty(&sps->s_queue)),
              "inconsistent semaphore");
  chDbgAssert(((spw->s_cnt >= (cnt_t)0) && queue_isempty(&spw->s_queue)) ||
              ((spw->s_cnt < (cnt_t)0) && queue_notempty(&spw->s_queue)),
              "inconsistent semaphore");

  chSysLock();
  if (++sps->s_cnt <= (cnt_t)0) {
    chSchReadyI(queue_fifo_remove(&sps->s_queue))->p_u.rdymsg = MSG_OK;
  }
  if (--spw->s_cnt < (cnt_t)0) {
    thread_t *ctp = currp;
    sem_insert(ctp, &spw->s_queue);
    ctp->p_u.wtsemp = spw;
    chSchGoSleepS(CH_STATE_WTSEM);
    msg = ctp->p_u.rdymsg;
  }
  else {
    chSchRescheduleS();
    msg = MSG_OK;
  }
  chSysUnlock();

  return msg;
}

static THD_FUNCTION(txWrite, arg)
{
	(void)(arg);
	while(!0){
            chSysLock();
            tp_W = chThdGetSelfX();
            chSchGoSleepS(CH_STATE_SUSPENDED);
            chSysUnlock();

            sdWrite(&SD3, &temda, strlen(temda));
		/* Get contents of next mail in mailbox pointed by arg1 to arg2
		 * and wait mail arg3 mSeconds if there is no message is present.
		 *
		 * Returns MSG_OK if succesfully a mail fetched from mailbox*/
		//if(chMBFetch(&serialMbox, (msg_t *)&toSend, TIME_INFINITE) == MSG_OK)
	//	{
			/* Write arg3 bytes from arg2 to device pointed by arg1(SD3 for this example).
			 * Type of arg2 should be (uint8_t *) otherwise only first 8 bit will be send.*/
		//	sdWrite(&SD3, &toSend,1);

		//}
		//wifiInit();
		chThdSleepMilliseconds(1000);
	}
}

void LedRGB_t::Task(void) {
    if(ICurrentColor == INeededColor) {
        chSysLock();
        IsSleeping = true;
        chSchGoSleepS(THD_STATE_SUSPENDED);
        chSysUnlock();
    }
    else {
        chThdSleepMilliseconds(LED_SETUP_DELAY_MS);
        // Red channel
        if (ICurrentColor.Red != INeededColor.Red) {
            if(INeededColor.Red < ICurrentColor.Red) ICurrentColor.Red--;
            else ICurrentColor.Red++;
        }
        // Green channel
        if (ICurrentColor.Green != INeededColor.Green) {
            if(INeededColor.Green < ICurrentColor.Green) ICurrentColor.Green--;
            else ICurrentColor.Green++;
        }
        // Blue channel
        if (ICurrentColor.Blue != INeededColor.Blue) {
            if(INeededColor.Blue < ICurrentColor.Blue) ICurrentColor.Blue--;
            else ICurrentColor.Blue++;
        }
        SetColor(ICurrentColor);
    }
}

/**
 * @brief   Performs atomic signal and wait operations on two semaphores.
 * @pre     The configuration option @p CH_USE_SEMSW must be enabled in order
 *          to use this function.
 *
 * @param[in] sps       pointer to a @p Semaphore structure to be signaled
 * @param[in] spw       pointer to a @p Semaphore structure to be wait on
 * @return              A message specifying how the invoking thread has been
 *                      released from the semaphore.
 * @retval RDY_OK       if the thread has not stopped on the semaphore or the
 *                      semaphore has been signaled.
 * @retval RDY_RESET    if the semaphore has been reset using @p chSemReset().
 *
 * @api
 */
msg_t chSemSignalWait(Semaphore *sps, Semaphore *spw) {
  msg_t msg;

  chDbgCheck((sps != NULL) && (spw != NULL), "chSemSignalWait");

  chDbgAssert(((sps->s_cnt >= 0) && isempty(&sps->s_queue)) ||
              ((sps->s_cnt < 0) && notempty(&sps->s_queue)),
              "chSemSignalWait(), #1",
              "inconsistent semaphore");

  chDbgAssert(((spw->s_cnt >= 0) && isempty(&spw->s_queue)) ||
              ((spw->s_cnt < 0) && notempty(&spw->s_queue)),
              "chSemSignalWait(), #2",
              "inconsistent semaphore");

  chSysLock();
  if (++sps->s_cnt <= 0)
    chSchReadyI(fifo_remove(&sps->s_queue))->p_u.rdymsg = RDY_OK;
  if (--spw->s_cnt < 0) {
    Thread *ctp = currp;
    sem_insert(ctp, &spw->s_queue);
    ctp->p_u.wtobjp = spw;
    chSchGoSleepS(THD_STATE_WTSEM);
    msg = ctp->p_u.rdymsg;
  }
  else {
    chSchRescheduleS();
    msg = RDY_OK;
  }
  chSysUnlock();
  return msg;
}

void Uart_t::IRxTask() {
    while(true) {
        chThdSleepMilliseconds(UART_RX_POLLING_MS);
        // Get number of bytes to process
#if defined STM32F2XX || defined STM32F4XX
        int32_t Sz = UART_RXBUF_SZ - UART_DMA_RX->stream->NDTR;   // Number of bytes copied to buffer since restart
#else
        int32_t Sz = UART_RXBUF_SZ - UART_DMA_RX->channel->CNDTR;   // Number of bytes copied to buffer since restart
#endif
        if(Sz != SzOld) {
            int32_t ByteCnt = Sz - SzOld;
            if(ByteCnt < 0) ByteCnt += UART_RXBUF_SZ;   // Handle buffer circulation
            SzOld = Sz;
            // Iterate received bytes
            for(int32_t i=0; i<ByteCnt; i++) {
                char c = IRxBuf[RIndx++];
                if(RIndx >= UART_RXBUF_SZ) RIndx = 0;
                if(Cmd.PutChar(c) == pdrNewCmd) {
                    chSysLock();
                    App.SignalEvtI(EVTMSK_UART_NEW_CMD);
                    chSchGoSleepS(CH_STATE_SUSPENDED); // Wait until cmd processed
                    chSysUnlock();  // Will be here when application signals that cmd processed
                }
            } // for
        } // if sz
    } // while true
}

void Usb_t::ITask() {
    while(1) {
        chSysLock();
        if(!Ep[0].IsTxPending) {
            chSchGoSleepS(THD_STATE_SUSPENDED);     // Will wake up by IRQ
        }
        chSysUnlock();

        // Handle RX
//        while(OTG_FS->GINTSTS & GINTSTS_RXFLVL) RxHandler();

        // Handle TX
        for(uint8_t i=0; i<USB_EP_CNT; i++) {
            if(Ep[i].IsTxPending) {
                // Block IRQ for not to interfere in FIFO filling operation
                chSysLock();
                DisableIrqs();
                Ep[i].IsTxPending = false;
                //chSysUnlock();
                bool Done = Ep[i].TxFifoHandler();
                //Uart.Printf("Done: %u\r\n", Done);
                // Unblock IRQs back
                //chSysLock();
                EnableIrqs();
                if(!Done) Ep[i].EnableInFifoEmptyIRQ();
                chSysUnlock();
            } // if pending
        } // for
    } // while 1
}

static msg_t lsm303_mag_update_thread(void *p) {
  I2CDriver *i2cp = (I2CDriver *)p;

  while (TRUE) {
    msg_t msg;

    /* Waiting for the IRQ to happen.*/

    chSysLock();
    magtp = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);
    msg = chThdSelf()->p_u.rdymsg;
    chSysUnlock();

    /* If data ready, update all axis.*/
    if (msg == LSM303_MAG_DATA_READY) {
      lsm303_mag_update(i2cp);
//      if (status_m != 0x00) {
//        lsm303_mag_update(i2cp);
//      }
    }
  }

  return RDY_OK;
}

/**
 * @brief   Locks the specified mutex.
 *
 * @param[in] mp        pointer to the @p Mutex structure
 */
void chMtxLockS(Mutex *mp) {
  Thread *ctp = currp;

  chDbgCheck(mp != NULL, "chMtxLockS");

  /* Ia the mutex already locked? */
  if (mp->m_owner != NULL) {
    /* Priority inheritance protocol; explores the thread-mutex dependencies
       boosting the priority of all the affected threads to equal the priority
       of the running thread requesting the mutex.*/
    Thread *tp = mp->m_owner;
    /* Does the running thread have higher priority than the mutex
       ownning thread? */
    while (tp->p_prio < ctp->p_prio) {
      /* Make priority of thread tp match the running thread's priority.*/
      tp->p_prio = ctp->p_prio;
      /* The following states need priority queues reordering.*/
      switch (tp->p_state) {
      case THD_STATE_WTMTX:
        /* Re-enqueues the mutex owner with its new priority.*/
        prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
        tp = ((Mutex *)tp->p_u.wtobjp)->m_owner;
        continue;
#if CH_USE_CONDVARS | CH_USE_SEMAPHORES_PRIORITY | CH_USE_MESSAGES_PRIORITY
#if CH_USE_CONDVARS
      case THD_STATE_WTCOND:
#endif
#if CH_USE_SEMAPHORES_PRIORITY
      case THD_STATE_WTSEM:
#endif
#if CH_USE_MESSAGES_PRIORITY
      case THD_STATE_SNDMSG:
#endif
        /* Re-enqueues tp with its new priority on the queue.*/
        prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
        break;
#endif
      case THD_STATE_READY:
        /* Re-enqueues  tp with its new priority on the ready list.*/
        chSchReadyI(dequeue(tp));
      }
      break;
    }
    /* Sleep on the mutex.*/
    prio_insert(ctp, &mp->m_queue);
    ctp->p_u.wtobjp = mp;
    chSchGoSleepS(THD_STATE_WTMTX);
    /* It is assumed that the thread performing the unlock operation assigns
       the mutex to this thread.*/
    chDbgAssert(mp->m_owner == ctp, "chMtxLockS(), #1", "not owner");
    chDbgAssert(ctp->p_mtxlist == mp, "chMtxLockS(), #2", "not owned");
  }
  else {
    /* It was not owned, inserted in the owned mutexes list.*/
    mp->m_owner = ctp;
    mp->m_next = ctp->p_mtxlist;
    ctp->p_mtxlist = mp;
  }
}

int ThreadWait::sleep() {
	chSysLock();
	thread_to_wake = chThdSelf();
	chSchGoSleepS(THD_STATE_SUSPENDED);
	const auto result = chThdSelf()->p_u.rdymsg;
	chSysUnlock();
	return result;
}

static msg_t usb_lld_pump(void *p) {
  USBDriver *usbp = (USBDriver *)p;
  stm32_otg_t *otgp = usbp->otg;

  chRegSetThreadName("usb_lld_pump");
  chSysLock();
  while (TRUE) {
    usbep_t ep;
    uint32_t epmask;

    /* Nothing to do, going to sleep.*/
    if ((usbp->state == USB_STOP) ||
        ((usbp->txpending == 0) && !(otgp->GINTSTS & GINTSTS_RXFLVL))) {
      otgp->GINTMSK |= GINTMSK_RXFLVLM;
      usbp->thd_wait = chThdSelf();
      chSchGoSleepS(THD_STATE_SUSPENDED);
    }
    chSysUnlock();

    /* Checks if there are TXFIFOs to be filled.*/
    for (ep = 0; ep <= usbp->otgparams->num_endpoints; ep++) {

      /* Empties the RX FIFO.*/
      while (otgp->GINTSTS & GINTSTS_RXFLVL) {
        otg_rxfifo_handler(usbp);
      }

      epmask = (1 << ep);
      if (usbp->txpending & epmask) {
        bool_t done;

        chSysLock();
        /* USB interrupts are globally *suspended* because the peripheral
           does not allow any interference during the TX FIFO filling
           operation.
           Synopsys document: DesignWare Cores USB 2.0 Hi-Speed On-The-Go (OTG)
             "The application has to finish writing one complete packet before
              switching to a different channel/endpoint FIFO. Violating this
              rule results in an error.".*/
        otgp->GAHBCFG &= ~GAHBCFG_GINTMSK;
        usbp->txpending &= ~epmask;
        chSysUnlock();

        done = otg_txfifo_handler(usbp, ep);

        chSysLock();
        otgp->GAHBCFG |= GAHBCFG_GINTMSK;
        if (!done)
          otgp->DIEPEMPMSK |= epmask;
        chSysUnlock();
      }
    }
    chSysLock();
  }
  chSysUnlock();
  return 0;
}

  msg_t ThreadReference::suspendS(void) {

    chDbgAssert(thread_ref == NULL,
                "already referenced");

    thread_ref = chThdGetSelfX();
    chSchGoSleepS(CH_STATE_SUSPENDED);
    return thread_ref->p_u.rdymsg;
  }

/**
 * @brief   Puts the current thread to sleep into the specified state with
 *          timeout specification.
 * @details The thread goes into a sleeping state, if it is not awakened
 *          explicitly within the specified timeout then it is forcibly
 *          awakened with a @p MSG_TIMEOUT low level message. The possible
 *          @ref thread_states are defined into @p threads.h.
 *
 * @param[in] newstate  the new thread state
 * @param[in] time      the number of ticks before the operation timeouts, the
 *                      special values are handled as follow:
 *                      - @a TIME_INFINITE the thread enters an infinite sleep
 *                        state, this is equivalent to invoking
 *                        @p chSchGoSleepS() but, of course, less efficient.
 *                      - @a TIME_IMMEDIATE this value is not allowed.
 *                      .
 * @return              The wakeup message.
 * @retval MSG_TIMEOUT  if a timeout occurs.
 *
 * @sclass
 */
msg_t chSchGoSleepTimeoutS(tstate_t newstate, systime_t time) {

  chDbgCheckClassS();

  if (TIME_INFINITE != time) {
    virtual_timer_t vt;

    chVTDoSetI(&vt, time, wakeup, currp);
    chSchGoSleepS(newstate);
    if (chVTIsArmedI(&vt))
      chVTDoResetI(&vt);
  }
  else {
    chSchGoSleepS(newstate);
  }

  return currp->p_u.rdymsg;
}

uint8_t i2c_t::CmdWriteWrite(uint8_t Addr,
        uint8_t *WPtr1, uint8_t WLength1,
        uint8_t *WPtr2, uint8_t WLength2) {
    if(IBusyWait() != OK) return FAILURE;
    // Clear flags
    ii2c->SR1 = 0;
    while(RxIsNotEmpty()) (void)ii2c->DR;   // Read DR until it empty
    ClearAddrFlag();
    // Start transmission
    SendStart();
    if(WaitEv5() != OK) return FAILURE;
    SendAddrWithWrite(Addr);
    if(WaitEv6() != OK) { SendStop(); return FAILURE; }
    ClearAddrFlag();
    // Start TX DMA if needed
    if(WLength1 != 0) {
        if(WaitEv8() != OK) return FAILURE;
        dmaStreamSetMemory0(PDmaTx, WPtr1);
        dmaStreamSetMode   (PDmaTx, I2C_DMATX_MODE);
        dmaStreamSetTransactionSize(PDmaTx, WLength1);
        chSysLock();
        PRequestingThread = chThdSelf();
        dmaStreamEnable(PDmaTx);
        chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
        chSysUnlock();
        dmaStreamDisable(PDmaTx);
    }
    if(WLength2 != 0) {
        if(WaitEv8() != OK) return FAILURE;
        dmaStreamSetMemory0(PDmaTx, WPtr2);
        dmaStreamSetMode   (PDmaTx, I2C_DMATX_MODE);
        dmaStreamSetTransactionSize(PDmaTx, WLength2);
        chSysLock();
        PRequestingThread = chThdSelf();
        dmaStreamEnable(PDmaTx);
        chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
        chSysUnlock();
        dmaStreamDisable(PDmaTx);
    }
    WaitBTF();
    SendStop();
    return OK;
}

/**
 * @brief   Sends the current thread sleeping and sets a reference variable.
 * @note    This function must reschedule, it can only be called from thread
 *          context.
 *
 * @param[in] trp       a pointer to a thread reference object
 * @return              The wake up message.
 *
 * @sclass
 */
msg_t chThdSuspendS(thread_reference_t *trp) {
  thread_t *tp = chThdGetSelfX();

  chDbgAssert(*trp == NULL, "not NULL");

  *trp = tp;
  tp->p_u.wtobjp = &trp;
  chSchGoSleepS(CH_STATE_SUSPENDED);
  return chThdGetSelfX()->p_u.rdymsg;
}

static THD_FUNCTION(thEDisplay, arg)
{
	(void)arg;
	chRegSetThreadName("eDisplay");

	kbb_display.header.preamble = KBB_PREAMBLE;
	kbb_display.header.msg_class = KBB_CLASS_EXTERNAL;
	kbb_display.header.msg_subclass = KBB_SUBCLASS_EXTERNAL_01;
	kbb_display.header.msg_size = KBB_DISPLAY_SIZE;

	while( !chThdShouldTerminateX() )
	{
		chSysLock();
			eDisplayThreadForSleep = chThdGetSelfX();
		    chSchGoSleepS(CH_STATE_SUSPENDED);
		chSysUnlock();

		if ( !ed_serialPort )
			continue;

		if ( ed_serialPort == &Serial1 )
			if ( kbse_getBaudSerial1() < 115200 )
				continue;
		if ( ed_serialPort == &Serial2 )
			if ( kbse_getBaudSerial2() < 115200 )
				continue;

		const kbb_current_msg_t * msg = kbw_getCurrentMsg();
		memcpy(&kbb_display.ltc_frame, 		&msg->ltc_frame, 	sizeof(kbb_display.ltc_frame));
		memcpy(&kbb_display.smpte_time, 	&msg->smpte_time, 	sizeof(kbb_display.smpte_time));
		kbb_display.ecef[0] = msg->nav_sol.ecefX;
		kbb_display.ecef[1] = msg->nav_sol.ecefY;
		kbb_display.ecef[2] = msg->nav_sol.ecefZ;
		memcpy(&kbb_display.vnav, 			&msg->vnav, 		sizeof(kbb_display.vnav));
		kbb_display.temperature = msg->temperature;
		int32_t * i32buf = (int32_t*)kbb_display.__pad;

		i32buf[0] = AA;
		i32buf[1] = BB;
		i32buf[2] = CC;

		uint8_t * buf = (uint8_t*) &kbb_display;
		kbb_display.header.checksum = calc_checksum_16(buf+KBB_CHECKSUM_START,
				KBB_DISPLAY_SIZE-KBB_CHECKSUM_START);

		// display!!!
		int size_written = sdWrite(ed_serialPort, buf, KBB_DISPLAY_SIZE);
		if ( size_written != KBB_DISPLAY_SIZE )
		{
			kbg_toggleLED3();
			ASSERT(size_written==KBB_DISPLAY_SIZE,"thEDisplay", "Wrong sent data!!");
		}
	}
	return;
}

/**
 * @brief   Transmits data via the I2C bus as master.
 * @details When performing reading through write you can not write more than
 *          3 bytes of data to I2C slave. This is SAM7 platform limitation.
 *
 * @param[in] i2cp      pointer to the @p I2CDriver object
 * @param[in] addr      slave device address
 * @param[in] txbuf     pointer to the transmit buffer
 * @param[in] txbytes   number of bytes to be transmitted
 * @param[out] rxbuf    pointer to the receive buffer
 * @param[in] rxbytes   number of bytes to be received
 * @param[in] timeout   this value is ignored on SAM7 platform.
 *                      .
 * @return              The operation status.
 * @retval RDY_OK       if the function succeeded.
 * @retval RDY_RESET    if one or more I2C errors occurred, the errors can
 *                      be retrieved using @p i2cGetErrors().
 *
 * @notapi
 */
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
                                      const uint8_t *txbuf, size_t txbytes,
                                      uint8_t *rxbuf, size_t rxbytes,
                                      systime_t timeout) {
  (void)timeout;

  /* SAM7 specific check */
  chDbgCheck(((rxbytes == 0) || ((txbytes > 0) && (txbytes < 4) && (rxbuf != NULL))),
             "i2c_lld_master_transmit_timeout");

  /* prepare to read through write operation */
  if (rxbytes > 0){
    AT91C_BASE_TWI->TWI_MMR |= txbytes << 8;

    /* store internal slave address in TWI_IADR registers */
    AT91C_BASE_TWI->TWI_IADR = 0;
    while (txbytes > 0){
      AT91C_BASE_TWI->TWI_IADR = (AT91C_BASE_TWI->TWI_IADR << 8);
      AT91C_BASE_TWI->TWI_IADR |= *(txbuf++);
      txbytes--;
    }
    /* Internal address of I2C slave was set in special Atmel registers.
     * Now we must call read function. The I2C cell automatically sends
     * bytes from IADR register to bus and issues repeated start. */
    return i2c_lld_master_receive_timeout(i2cp, addr, rxbuf, rxbytes, timeout);
  }
  else{
    if (txbytes == 1){
      /* In single data byte master read or write, the START and STOP
       * must both be set. */
      AT91C_BASE_TWI->TWI_CR |= AT91C_TWI_STOP;
    }
    AT91C_BASE_TWI->TWI_MMR  = 0;
    AT91C_BASE_TWI->TWI_MMR |= addr << 16;

    /* enable just needed interrupts */
    AT91C_BASE_TWI->TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;

    /* correct size and pointer because first byte will be written
     * for issue start condition */
    i2cp->txbuf   = txbuf + 1;
    i2cp->txbytes = txbytes - 1;

    /* According to datasheet there is no need to set START manually
     * we just need to write first byte in THR */
    AT91C_BASE_TWI->TWI_THR = txbuf[0];

    /* Waits for the operation completion.*/
    i2cp->thread = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);

    return chThdSelf()->p_u.rdymsg;
  }
}