void blinkLED (void* pdata){
while(1){
GPIO_WriteBit(GPIOC,GPIO_Pin_8,Bit_SET);
CoTickDelay (delay);
GPIO_WriteBit(GPIOC,GPIO_Pin_8,Bit_RESET);
CoTickDelay (delay);
}
}
/*
void taskSender(void *pdata){
int i=0;
while(1){
Env.txBuf[0]=0xB1;
Env.txBuf[1]=0x09;
Env.txBuf[2]=0x00;
Env.txBuf[3]=0x04;
for(i=0;i<10;i++){
USART_SendData(USART1, Env.txBuf[i]);
while (USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET);
}
CoTickDelay(500);//ждем 5 сек.
}
}
*/
void taskBlink(void *pdata){
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
while(1){
GPIOA->BRR=GPIO_Pin_2;
CoTickDelay(50);//ждем 0.5 сек.
GPIOA->BSRR=GPIO_Pin_2;
CoTickDelay(50);
}
}
void taskBlink0(void *param)
{
/* Set PIO2_0 as output. */
GPIO_SetDir(PORT2, LED0, 1);
for(;;) {
/* Turn On Led */
GPIO_ResetBits(PORT2, LED0);
CoTickDelay(20); /* Delay 200ms */
/* Turn Off Led */
GPIO_SetBits(PORT2, LED0);
CoTickDelay(20); /* Delay 200ms */
}
}
// ------------------------------------------------------------------------
// task1( ) - Blinks LED2 at a two second rate.
//
// Most CoOS tasks are coded as endless loops.
//
// This endless loop sets and clears GPIO port0, bit22.
//
// A one second CoTickDelay() function after each LED2
// state change gives the two second repetition rate.
//
// Note: The current default CoOS tick rate is 10 milliseconds.
// Thus 100 * 10msec = 1000msec or 1.0 seconds.
// ------------------------------------------------------------------------
void task1 (void* pdata){
while(1){
// turn OFF LED2
GPIO_ClearValue(PORT0, LED_PINS);
// delay (100 * 10msec/tick = 1000 msec)
CoTickDelay(100);
// turn ON LED2
GPIO_SetValue(PORT0, LED_PINS);
// delay (100 * 10msec/tick = 1000 msec)
CoTickDelay(100);
}
}
void taskBlink1(void *param)
{
/* Set PIO2_1 as output. */
GPIO_SetDir(PORT2, LED1, 1);
for(;;) {
/* Turn On Led */
GPIO_ResetBits(PORT2, LED1);
CoTickDelay(40); /* Delay 400ms */
/* Turn Off Led */
GPIO_SetBits(PORT2, LED1);
CoTickDelay(40); /* Delay 400ms */
}
}
/**
* -----------------------------------------------------------------------------
* @brief mutex2_execute
* @param[in] None
* @param[out] None
* @retval None
* @par Description
* @details The code of mutex test example 3.
* -----------------------------------------------------------------------------
*/
static void mutex2_execute (void)
{
unsigned char i;
Cnt = 0;
Exit_Flag = 0;
CoTickDelay (1);
for (i=0;i<MAX_SLAVE_TEST_TASKS;i++ ){
Flag [i] = 0;
}
for (i=0; i< MAX_SLAVE_TEST_TASKS; i++) {
Task_Id [i] = CoCreateTask (mutexTask02,(void*)i,MAINTEST_PRIMARY_PRIORITY-(i+1),&Task_Stack[i][SLAVE_TASK_STK_SIZE-1],SLAVE_TASK_STK_SIZE);
if (Task_Id[i] == E_CREATE_FAIL) {
printf ("\r Create the %d mutex task fail. \n",i+1);
}
}
while (Exit_Flag == 0) ;
Exit_Flag = 0;
for (i=0; i<MAX_SLAVE_TEST_TASKS; i++) {
testAssert((Flag[i] == Sequence2[i])," Mutex #2 ");
}
}
void gpsTask(void *unused)
{
gpsData.serial = serialOpen(USART1, 9600);
gpsData.mode = MODE_NMEA; // NMEA
while (1) {
CoTickDelay(25);
while (uartAvailable(gpsData.serial)) {
switch (gpsData.mode) {
case MODE_NMEA: // NMEA
gpsData.validFrames += gpsNewFrameNMEA(uartRead(gpsData.serial));
break;
case MODE_UBX: // UBX
gpsData.validFrames += gpsNewFrameUBLOX(uartRead(gpsData.serial));
break;
case MODE_MTK: // MTK
break;
case MODE_PASSTHROUGH: // GPS -> UART bridge
// TODO
// usbSerialWrite(uartRead(gpsData.serial));
break;
}
}
}
}
/**
* -----------------------------------------------------------------------------
* @brief mutex3_execute
* @param[in] None
* @param[out] None
* @retval None
* @par Description
* @details The code of mutex test example 2.
* -----------------------------------------------------------------------------
*/
static void mutex3_execute (void)
{
unsigned char i;
Cnt = 0;
Exit_Flag = 0;
CoTickDelay (1);
for (i=0;i<MAX_SLAVE_TEST_TASKS;i++ ){
Flag [i] = 0;
}
for (i=0; i< MAX_SLAVE_TEST_TASKS; i++) {
Task_Id [i] = CoCreateTask (mutexTask03,(void*)i,MAINTEST_PRIMARY_PRIORITY-(i+1),&Task_Stack[i][SLAVE_TASK_STK_SIZE-1],SLAVE_TASK_STK_SIZE);
testAssert((Task_Id[i] != E_CREATE_FAIL)," Mutex #3: Create mutex fail ");
}
while (Exit_Flag == 0) ;
Exit_Flag = 0;
for (i=0; i<MAX_SLAVE_TEST_TASKS; i++) {
testAssert((Flag[i] == Sequence3[i])," Mutex #3: Test fail. ");
}
}
/**
* -----------------------------------------------------------------------------
* @brief mutex1_execute
* @param[in] None
* @param[out] None
* @retval None
* @par Description
* @details The code of mutex test example 1.
* -----------------------------------------------------------------------------
*/
static void mutex1_execute (void)
{
unsigned char i;
Cnt = 0;
Del_Flag = 0;
Exit_Flag = 0;
CoTickDelay (1);
for (i=0; i<MUTEX_NUM; i++) {
Mutex_Id[i] = CoCreateMutex ();
if (Mutex_Id[i] == E_CREATE_FAIL) {
printf ("\r Create the %d mutex fail. \n",i+1);
}
}
for (i=0; i< MAX_SLAVE_TEST_TASKS; i++) {
Task_Id [i] = CoCreateTask (mutexTask01,(void*)i,MAINTEST_PRIMARY_PRIORITY-(i+1),&Task_Stack[i][SLAVE_TASK_STK_SIZE-1],SLAVE_TASK_STK_SIZE);
testAssert((Task_Id[i] != E_CREATE_FAIL)," Create task fail #3 ");
}
while (Exit_Flag == 0) ;
Del_Flag = 0;
Exit_Flag = 0;
for (i=0; i<MAX_SLAVE_TEST_TASKS; i++) {
testAssert((Flag[i] == Sequence[i])," Mutex #1 ");
}
}
long getCurrentTemperature()
{
CoEnterMutexSection(I2CMutex);
long i = 0;
while(i<100000)
{
i++;
}
writeDataBmp085(0xF4, 0x2E);
//wait 4.5ms
i = 0;
while(i<1000000)
{
i++;
}
long ut = getUtBMP085(0xf6);
CoTickDelay(300);
short oss = 0;
uint8_t transmitData = 0x34;
transmitData += oss<<6;
writeDataBmp085(0xF4, transmitData);
i = 0;
while(i<1000000)
{
i++;
}
long up = getUpBMP085(0xf6, oss);
long temp = getTemperature();
CoLeaveMutexSection(I2CMutex);
CoEnterMutexSection(currentTemperatureMutex);
currentTemperature = temp;
CoLeaveMutexSection(currentTemperatureMutex);
return temp;
}
void mixerTask(void * pdata)
{
s_pulses_paused = true;
while(1) {
if (!s_pulses_paused) {
uint16_t t0 = getTmr2MHz();
CoEnterMutexSection(mixerMutex);
doMixerCalculations();
CoLeaveMutexSection(mixerMutex);
#if defined(FRSKY) || defined(MAVLINK)
telemetryWakeup();
#endif
if (heartbeat == HEART_WDT_CHECK) {
wdt_reset();
heartbeat = 0;
}
t0 = getTmr2MHz() - t0;
if (t0 > maxMixerDuration) maxMixerDuration = t0 ;
}
CoTickDelay(1); // 2ms for now
}
}
void btPRESS (void* pdata){
while(1){
flag = GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_0);
if(flag==1){
if(btStatus==0)
{
GPIO_WriteBit(GPIOC,GPIO_Pin_9,Bit_SET);
const unsigned char menu[] = "/?!\r\n";
UART1Send(menu, sizeof(menu));
UART2Send(menu, sizeof(menu));
LCD_Clear(); //Clear the LCD.
LCD_GoTo(0,0); //Go to Line 0, position 0 of LCD.
LCD_SendText("@300 7E1");
LCD_GoTo(1,0); //Go to Line 1, position 2 (on the right) of LCD.
LCD_SendText (menu);
}
CoTickDelay (10);
btStatus = 1;
GPIO_WriteBit(GPIOC,GPIO_Pin_9,Bit_RESET);
}else{
GPIO_WriteBit(GPIOC,GPIO_Pin_9,Bit_RESET);
btStatus = 0;
}
}
}
uint32_t btPollDevice()
{
uint16_t x ;
uint32_t y ;
uint16_t rxchar ;
x = 'O' ;
btTxBuffer[0] = 'A' ;
btTxBuffer[1] = 'T' ;
btTx.size = 2 ;
btSendBuffer() ;
for( y = 0 ; y < BT_POLL_TIMEOUT ; y++) {
if ( ( rxchar = rxBtuart() ) != 0xFFFF ) {
if ( rxchar == x ) {
if ( x == 'O' ) {
x = 'K' ;
}
else {
break ; // Found "OK"
}
}
}
else {
CoTickDelay(1) ; // 2mS
}
}
if ( y < BT_POLL_TIMEOUT ) {
return 1 ;
}
else {
return 0 ;
}
}
/**
*******************************************************************************
* @brief "taskB" task code
* @param[in] None
* @param[out] None
* @retval None
* @par Description
* @details This task use to print message "taskB running". Indicate "taskB"
* had been executed.
*******************************************************************************
*/
void taskB (test_t* p){
unsigned int led_num;
led_num=p->m3;
for (;;) {
led_num++;
CoTickDelay (50);
}
}
/**
*******************************************************************************
* @brief "led" task code
* @param[in] None
* @param[out] None
* @retval None
* @par Description
* @details This function use to blink led,and set flag for "taskA" and "taskB".
*******************************************************************************
*/
void taskC (void* pdata){
unsigned int led_num;
for (;;) {
led_num++;
CoTickDelay (50);
}
}
void btSendBuffer()
{
btTx.buffer = btTxBuffer;
txPdcBt(&btTx);
while (btTx.ready == 1) {
CoTickDelay(1); // 2ms for now
}
btTx.size = 0;
}
/**
*******************************************************************************
* @brief "taskA" task code
* @param[in] None
* @param[out] None
* @retval None
* @par Description
* @details This task use to crate mutex and flags,print message "taskA running".
* Indicate "taskA" had been executed.
*******************************************************************************
*/
void taskA (unsigned int st)
{
unsigned int led_num;
led_num=st;
for (;;) {
led_num++;
CoTickDelay (50);
}
}
void runPID(void *Parameters)
{
float currError = 0, prevError = 0, errorSum = 0, errorDiff = 0,
outD = 0, outP = 0, outI = 0, output = 0;
int delayTicks;
struct PIDParameters * params = Parameters;
CoWaitForSingleFlag(params->InitCompleteFlagID, 0);
while (1)
{
if (RegisterMap[REGISTER_CONTROL_MODE].Bits & CONTROL_MODE_AUTO_MODE && RegisterMap[REGISTER_CONTROL_MODE].Bits & params->ControlBitMask)
{
CoPendSem(params->ThisPIDSemID, 0);
if(!params->IsTopLevelUnit)
CoPendSem(params->PrevPIDSemID, 0);
CoPendSem(params->DataSemID, 0);
currError = *(params->SetPoint) - *(params->ProcessVariable);
errorSum += currError;
errorDiff = currError - prevError;
prevError = currError;
outD = *(params->KD) * errorDiff;
outI = *(params->KI) * errorSum;
outP = *(params->KP) * currError;
output = outD + outP + outI;
*(params->OutputVariable) = output;
CoPostSem(params->ThisPIDSemID);
if(!params->IsTopLevelUnit)
CoPostSem(params->PrevPIDSemID);
CoPostSem(params->DataSemID);
}
else
{
// Reset if the PID controller is turned off
currError = 0;
errorSum = 0;
errorDiff = 0;
prevError = 0;
}
delayTicks = delaymsToTicks(*(params->UpdateTime_ms));
CoTickDelay(delayTicks);
}
}
static void task_led(void *pdata) {
(void)pdata;
PORTA_PCR10 = PORT_PCR_MUX(1); // set PORTA.11 as GPIO
GPIOA_PDDR = (1 << 10); // PORTA.11 output
for (;;) {
GPIOA_PTOR = (1 << 10); // toggle LED1
CoTickDelay (100);
}
}
/**
* -----------------------------------------------------------------------------
* @brief mutexTask03
* @param[in] pdata A pointer to parameter passed to task.
* @param[out] None
* @retval None
* @par Description
* @details Mutex test task code.
* -----------------------------------------------------------------------------
*/
static void mutexTask03 (void* pdata)
{
unsigned int task_para;
unsigned char err;
unsigned char mutex_id;
task_para = (unsigned int)pdata;
mutex_id = task_para % MUTEX_NUM;
for (;;)
{
err = CoEnterMutexSection (Mutex_Id[mutex_id]);
if (err != E_OK) {
printf ("\r Enter mutex error: \n");
printf ("\r Task id = %2d, mutex id = %2d \n",task_para,mutex_id);
}
if (task_para < MUTEX_NUM) {
CoTickDelay (30);
Flag [Cnt] = task_para+3;
Cnt++;
err = CoDelTask (Task_Id[MAX_SLAVE_TEST_TASKS-task_para-1]);
if (task_para == 1) {
if (err == E_OK) {
printf ("\r Error: delete task [%d] fail.\n",MAX_SLAVE_TEST_TASKS-task_para-1);
}
}
else {
if (err != E_OK) {
printf ("\r Error: delete task [%d] fail.\n",MAX_SLAVE_TEST_TASKS-task_para-1);
}
}
}
else {
Flag [Cnt] = task_para+3;
Cnt++;
}
err = CoLeaveMutexSection (Mutex_Id[mutex_id]);
if (err != E_OK) {
printf ("\r Leave Mutex error: \n");
printf ("\r Task id = %2d, mutex id = %2d \n",task_para,mutex_id);
}
if (task_para == 4)
Exit_Flag = 1;
CoExitTask ();
}
}
/**
*******************************************************************************
* @brief Task of blink led
* @param[in] pdata A pointer to parameter passed to task.
* @param[out] None
* @retval None
*
* @details This task use to blink led.
*******************************************************************************
*/
void led_blink(void *pdata)
{
char i;
pdata = pdata;
for (;;)
{
for (i=0;i<19;i++)
{
GPIOB->ODR = (GPIOB->ODR & 0xFFFF00FF) | (led[i] << 8);
CoTickDelay (20);
}
}
}
uint32_t btChangeBaudrate( uint32_t baudIndex )
{
uint16_t x ;
uint32_t y ;
uint16_t rxchar ;
x = 4 ; // 9600
if ( baudIndex == 0 )
{
x = 8 ; // 115200
}
else if ( baudIndex == 2 )
{
x = 5 ; // 19200
}
btTxBuffer[0] = 'A' ;
btTxBuffer[1] = 'T' ;
btTxBuffer[2] = '+' ;
btTxBuffer[3] = 'B' ;
btTxBuffer[4] = 'A' ;
btTxBuffer[5] = 'U' ;
btTxBuffer[6] = 'D' ;
btTxBuffer[7] = '0' + x ;
btTx.size = 8 ;
btSendBuffer() ;
x = 'O' ;
for( y = 0 ; y < BT_POLL_TIMEOUT ; y += 1 ) {
if ( ( rxchar = rxBtuart() ) != 0xFFFF ) {
if ( rxchar == x ) {
if ( x == 'O' ) {
x = 'K' ;
}
else {
break ; // Found "OK"
}
}
}
else {
CoTickDelay(1) ; // 2mS
}
}
if ( y < BT_POLL_TIMEOUT ) {
return 1 ;
}
else {
return 0 ;
}
}
/**
*******************************************************************************
* @brief Print task.
* @param[in] pdata A pointer to parameter passed to task.
* @param[out] None
* @retval None
*
* @details This task print adc value by UART.
*******************************************************************************
*/
void uart_print(void *pdata)
{
int AD_value;
pdata = pdata;
for (;;)
{
CoEnterMutexSection(mut_uart); /*!< Enter mutex area. */
AD_value = ADC_ConvertedValue;
uart_printf ("\r AD value = 0x");
uart_print_hex (AD_value,4);
CoLeaveMutexSection(mut_uart); /*!< Leave mutex area. */
CoTickDelay(100);
}
}
/**
* -----------------------------------------------------------------------------
* @brief mutexTask02
* @param[in] pdata A pointer to parameter passed to task.
* @param[out] None
* @retval None
* @par Description
* @details Mutex test task code.
* -----------------------------------------------------------------------------
*/
static void mutexTask02 (void* pdata)
{
unsigned int task_para;
unsigned char err;
unsigned char mutex_id;
task_para = (unsigned int)pdata;
mutex_id = task_para%MUTEX_NUM;
for (;;)
{
err = CoEnterMutexSection (Mutex_Id[mutex_id]);
if (err != E_OK) {
printf ("\r Enter mutex error: \n");
printf ("\r Task id = %2d, mutex id = %2d \n",task_para,mutex_id);
}
if (task_para < MUTEX_NUM) {
CoTickDelay (25);
}
Flag [Cnt] = task_para +3;
Cnt++ ;
if (Cnt == MAX_SLAVE_TEST_TASKS){
Exit_Flag = 1;
}
if (task_para < MUTEX_NUM) {
Del_Flag = 1;
CoExitTask ();
}
err = CoLeaveMutexSection (Mutex_Id[mutex_id]);
if (err != E_OK) {
printf ("\r Leave Mutex error: \n");
printf ("\r Task id = %2d, mutex id = %2d \n",task_para,mutex_id);
}
if (Del_Flag == 1) {
CoExitTask ();
}
}
}
StatusType CoTimeDelay(U8 hour,U8 minute,U8 sec,U16 millsec) {
U32 ticks;
#if CFG_PAR_CHECKOUT_EN >0 /* Check validity of parameter */
if(OSIntNesting > 0) {
return E_CALL;
}
if((minute > 59)||(sec > 59)||(millsec > 999)) {
return E_INVALID_PARAMETER;
}
#endif
if(OSSchedLock != 0) { /* Is OS lock? */
return E_OS_IN_LOCK; /* Yes,error return */
}
/* Get tick counter from time */
ticks = ((hour*3600) + (minute*60) + (sec)) * (CFG_SYSTICK_FREQ)\
+ (millsec*CFG_SYSTICK_FREQ + 500)/1000;
CoTickDelay(ticks); /* Call tick delay */
return E_OK; /* Return OK */
}
void cliTask(void * pdata)
{
char line[CLI_COMMAND_MAX_LEN+1];
int pos = 0;
cliPrompt();
for (;;) {
uint8_t c;
while (!cliRxFifo.pop(c)) {
CoTickDelay(10); // 20ms
}
if (c == 12) {
// clear screen
serialPrint("\033[2J\033[1;1H");
cliPrompt();
}
else if (c == 127) {
// backspace
if (pos) {
line[--pos] = '\0';
serialPutc(c);
}
}
else if (c == '\r' || c == '\n') {
// enter
serialCrlf();
line[pos] = '\0';
cliExecLine(line);
pos = 0;
cliPrompt();
}
else if (isascii(c) && pos < CLI_COMMAND_MAX_LEN) {
line[pos++] = c;
serialPutc(c);
}
}
}
/**
*******************************************************************************
* @brief Task of display adc value in LCD.
* @param[in] pdata A pointer to parameter passed to task.
* @param[out] None
* @retval None
*
* @details This task use to display adc value in lcd.
*******************************************************************************
*/
void lcd_display_adc(void *pdata)
{
int AD_value = 0;
int AD_scaled_ex = 0;
int AD_scaled = 0;
pdata = pdata;
for (;;)
{
AD_value = ADC_ConvertedValue; /*!< Read AD value. */
AD_scaled = AD_value / 52; /*!< AD value scaled to 0..78(lines on LCD). */
CoEnterMutexSection(mut_lcd);
if (AD_scaled != AD_scaled_ex) { /*!< If new AD value different than old.*/
AD_scaled_ex = AD_scaled;
lcd_bargraphXY(0, 1, AD_scaled);
}
CoLeaveMutexSection(mut_lcd);
CoTickDelay(80);
}
}
static void task2(void* pdata)
{
CoTickDelay(50);
CoPostSem(sem1ID);
CoExitTask();
}
void menusTask(void * pdata)
{
opentxInit();
#if defined(PCBTARANIS) && defined(REV9E)
while (1) {
uint32_t pwr_check = pwrCheck();
if (pwr_check == e_power_off) {
break;
}
else if (pwr_check == e_power_press) {
continue;
}
#else
while (pwrCheck() != e_power_off) {
#endif
U64 start = CoGetOSTime();
perMain();
// TODO remove completely massstorage from sky9x firmware
U32 runtime = (U32)(CoGetOSTime() - start);
// deduct the thread run-time from the wait, if run-time was more than
// desired period, then skip the wait all together
if (runtime < MENU_TASK_PERIOD_TICKS) {
CoTickDelay(MENU_TASK_PERIOD_TICKS - runtime);
}
}
#if defined(REV9E)
topLcdOff();
#endif
BACKLIGHT_OFF();
#if defined(PCBTARANIS)
displaySleepBitmap();
#else
lcd_clear();
displayPopup(STR_SHUTDOWN);
#endif
opentxClose();
boardOff(); // Only turn power off if necessary
}
extern void audioTask(void* pdata);
void tasksStart()
{
CoInitOS();
#if defined(CLI)
cliStart();
#endif
#if defined(BLUETOOTH)
btTaskId = CoCreateTask(btTask, NULL, 15, &btStack[BT_STACK_SIZE-1], BT_STACK_SIZE);
#endif
mixerTaskId = CoCreateTask(mixerTask, NULL, 5, &mixerStack[MIXER_STACK_SIZE-1], MIXER_STACK_SIZE);
menusTaskId = CoCreateTask(menusTask, NULL, 10, &menusStack[MENUS_STACK_SIZE-1], MENUS_STACK_SIZE);
audioTaskId = CoCreateTask(audioTask, NULL, 7, &audioStack[AUDIO_STACK_SIZE-1], AUDIO_STACK_SIZE);
#if !defined(SIMU)
audioMutex = CoCreateMutex();
mixerMutex = CoCreateMutex();
#endif
CoStartOS();
}
void btTask(void* pdata)
{
uint8_t byte;
btFlag = CoCreateFlag(true, false);
btTx.size = 0;
// Look for BT module baudrate, try 115200, and 9600
// Already initialised to g_eeGeneral.bt_baudrate
// 0 : 115200, 1 : 9600, 2 : 19200
uint32_t x = g_eeGeneral.btBaudrate;
btStatus = btPollDevice() ; // Do we get a response?
for (int y=0; y<2; y++) {
if (btStatus == 0) {
x += 1 ;
if (x > 2) {
x = 0 ;
}
btSetBaudrate(x) ;
CoTickDelay(1) ; // 2mS
btStatus = btPollDevice() ; // Do we get a response?
}
}
if (btStatus) {
btStatus = x + 1 ;
if ( x != g_eeGeneral.btBaudrate ) {
x = g_eeGeneral.btBaudrate ;
// Need to change Bt Baudrate
btChangeBaudrate( x ) ;
btStatus += (x+1) * 10 ;
btSetBaudrate( x ) ;
}
}
else {
btInit();
}
CoTickDelay(1) ;
btPollDevice(); // Do we get a response?
while (1) {
uint32_t x = CoWaitForSingleFlag(btFlag, 10); // Wait for data in Fifo
if (x == E_OK) {
// We have some data in the Fifo
while (btTxFifo.pop(byte)) {
btTxBuffer[btTx.size++] = byte;
if (btTx.size > 31) {
btSendBuffer();
}
}
}
else if (btTx.size) {
btSendBuffer();
}
}
}