uint8_t xqueue_push(xqueue_t * const queue, const uint8_t value){
	if(xqueue_full(queue))
		xqueue_drop(queue);

	*(queue->data + queue->head) = value;
	queue->head = (queue->head + 1) & queue->capacityMask;
	queue->wCount++;
	return value;
}
void xqueue_push_string(xqueue_t * const queue, const unsigned char * str){
	size_t ii;
	for(ii = 0; *str; ii++){
		if(xqueue_full(queue))
			xqueue_drop(queue);
		*(queue->data + queue->head) = *str++;
		queue->head = (queue->head + 1) & queue->capacityMask;
		queue->wCount++;
	}
}
示例#3
0
/* process a sensors stream that has been configured
   to transmit SensorGroup6 (all sensors).  */
extern int32_t irobotSensorStreamProcessAll(
    xqueue_t * const queue,							/* (in/out)	raw byte stream */
    irobotSensorGroup6_t * const sensors,			/* (out)	sensors			*/
    bool * const packetFound						/* (out)	packet found	*/
) {
    int32_t status = ERROR_SUCCESS;

    /* check for NULL pointers */
    if(!queue || !sensors || !packetFound) {
        return ERROR_INVALID_PARAMETER;
    }

    *packetFound = false;

    /*	Align buffer with iRobot sensor stream, according to format:
    	[Header:19] [Len:27] [Packet ID:0] [SenGroup0 (26 bytes)] [CxSum] */
    while(!xqueue_empty(queue) && xqueue_front(queue) != SENSOR_STREAM_HEADER) {
        xqueue_drop(queue);
    }

    /*	Check for properly formatted header;
    	NOTE: iRobot OI spec incorrectly omits the header from the checksum */
    if( xqueue_count(queue) >= SENSOR_GROUP6_SIZE + 4	/* size of entire packet		*/
            && xqueue_pop(queue) == SENSOR_STREAM_HEADER		/* sensor stream packet			*/
            && xqueue_pop(queue) == SENSOR_GROUP6_SIZE + 1		/* size of payload + checksum	*/
            && xqueue_pop(queue) == SENSOR_GROUP6) {			/* payload is sensor group 6	*/
        /* Checksum: cxsum = [Header:19] + [n-bytes:Sen6Size+1=53] + [packet ID:6] + [data (Sen6Size)] */
        uint8_t cxsum = 0;
        cxsum += SENSOR_STREAM_HEADER;
        cxsum += SENSOR_GROUP6_SIZE + 1;
        cxsum += SENSOR_GROUP6;
        cxsum += xqueue_checksum(queue, SENSOR_GROUP6_SIZE + 1, 0);	/* payload and encoded checksum */

        /* checksum passed */
        if(cxsum == 0) {
            irobot_StatusMerge(&status, irobotReadSensorGroup6(queue, sensors));
            xqueue_pop(queue);		/* clear checksum */
            if(!irobot_IsError(status)) {
                *packetFound = true;
            }
        }
    }

    return status;
}
int32_t irobotUARTWrite(
	const irobotUARTPort_t	port,
	xqueue_t * const		queue
){
	int32_t status = ERROR_SUCCESS;

	// catch NULL pointers
	if(!queue){
		irobot_StatusMerge(&status, ERROR_INVALID_PARAMETER);
	}
	
	// write
	while(!irobot_IsError(status) && !xqueue_empty(queue)){
		uint8_t txByte = xqueue_front(queue);
		irobot_StatusMerge(&status, irobotUARTWriteRaw(port, &txByte, 1));
		if(!irobot_IsError(status)){
			xqueue_drop(queue);
		}
	}

	return status;
}