static int __add_queue(queue_t *queue, queue_body_t *toadd) { int ret = 0; lock_queue(queue); if(is_full_queue(queue)) { debug(DEBUG_UTILS, "Add to full queue"); ret = -1; goto unl; } toadd->next = (queue_body_t *)QUEUE_TAIL_NULL; if( is_tail_null(queue) ) { queue->tail = toadd; queue->head = toadd; queue->curLength = 1; ret = 0; goto unl; } queue->tail->next = toadd; queue->tail = toadd; queue->curLength++; unl: unlock_queue(queue); return ret; }
int add_to_queue(SqQueue * queue, PktData pktData) { if(is_full_queue(queue)) { // Overwrite elements in the front queue->front = (queue->front + 1) % MAX_SIZE; } queue->accXData[queue->rear] = pktData.accX; queue->gyroXData[queue->rear] = pktData.gyroX; queue->magXData[queue->rear] = pktData.magX; queue->accYData[queue->rear] = pktData.accY; queue->gyroYData[queue->rear] = pktData.gyroY; queue->magYData[queue->rear] = pktData.magY; queue->accZData[queue->rear] = pktData.accZ; queue->gyroZData[queue->rear] = pktData.gyroZ; queue->magZData[queue->rear] = pktData.magZ; queue->timeStamp[queue->rear] = pktData.timeStamp; queue->rear = (queue->rear + 1) % MAX_SIZE; return (queue->rear - 1 + MAX_SIZE ) % MAX_SIZE; }