void QUEUE_SendString(xQueueHandle queue, const unsigned char *str) {
while(*str!='\0') {
if (FRTOS1_xQueueSendToBack(queue, str, portMAX_DELAY)!=pdPASS) {
for(;;){} /* ups? */
}
str++;
}
}
void SQUEUE_SendString(const unsigned char *str) {
while(*str!='\0'){
if(FRTOS1_xQueueSendToBack(SQUEUE_Queue, str, 100/portTICK_RATE_MS) != pdPASS){
//loosing carcter!!
}
str++;
}
}
void QUEUE_SendData(xQueueHandle queue, const unsigned char *data, size_t dataSize) {
while(dataSize!=0) {
if (FRTOS1_xQueueSendToBack(queue, data, portMAX_DELAY)!=pdPASS) {
for(;;){} /* ups? */
}
data++;
dataSize--;
}
}
/*!
* \brief Adds binary data to the queue.
* \param queue Queue to be used
* \param data Pointer to the binary data
* \param dataSize Size of data in bytes
*/
static uint8_t AddToQueue(xQueueHandle queue, const unsigned char *data, size_t dataSize) {
while(dataSize!=0) {
if (FRTOS1_xQueueSendToBack(queue, data, RSTDIO_QUEUE_TIMEOUT_MS/portTICK_RATE_MS)!=pdPASS) {
return ERR_FAULT;
}
data++;
dataSize--;
}
return ERR_OK;
}
uint8_t DRV_SetMode(DRV_Mode mode) {
DRV_Command cmd;
cmd.cmd = DRV_SET_MODE;
cmd.u.mode = mode;
if (FRTOS1_xQueueSendToBack(DRV_Queue, &cmd, portMAX_DELAY)!=pdPASS) {
return ERR_FAILED;
}
FRTOS1_taskYIELD(); /* yield so drive task has a chance to read message */
return ERR_OK;
}
void SQUEUE_SendString(const unsigned char *str) {
/*! \todo Implement function */
#if PL_CONFIG_HAS_SQUEUE_SINGLE_CHAR
while(*str!='\0') {
if (FRTOS1_xQueueSendToBack(SQUEUE_Queue, str, 100/portTICK_RATE_MS)!=pdPASS) {
/*for(;;){}*/ /* ups? */ /* loosing character */
}
str++;
}
#else
unsigned char *ptr;
size_t bufSize;
bufSize = UTIL1_strlen(str)+1;
ptr = FRTOS1_pvPortMalloc(bufSize);
UTIL1_strcpy(ptr, bufSize, str);
if (FRTOS1_xQueueSendToBack(SQUEUE_Queue, &ptr, portMAX_DELAY)!=pdPASS) {
for(;;){} /* ups? */
}
#endif
}
uint8_t DRV_SetPos(int32_t left, int32_t right) {
DRV_Command cmd;
cmd.cmd = DRV_SET_POS;
cmd.u.pos.left = left;
cmd.u.pos.right = right;
if (FRTOS1_xQueueSendToBack(DRV_Queue, &cmd, portMAX_DELAY)!=pdPASS) {
return ERR_FAILED;
}
FRTOS1_taskYIELD(); /* yield so drive task has a chance to read message */
return ERR_OK;
}