uint8_t xqueue_checksum(const xqueue_t * const queue, size_t nvalues, const size_t nAdvance){
uint8_t cxsum = 0;
size_t ii;
nvalues = MIN(nvalues, xqueue_count(queue) - nAdvance);
for(ii = 0; ii < nvalues; ii++){
cxsum += xqueue_at(queue, ii + nAdvance);
}
return cxsum;
}
/* 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;
}
size_t xqueue_space(const xqueue_t * const queue){
return queue->capacity - xqueue_count(queue);
}
bool xqueue_full(const xqueue_t * const queue){
return xqueue_count(queue) >= queue->capacity;
}
void xqueue_drop_many(xqueue_t * const queue, size_t n){
n = MIN(n, xqueue_count(queue));
queue->tail = (queue->tail + n) & queue->capacityMask;
queue->rCount += n;
}
