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++;
}
}
/* 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;
}