int32_t irobotSensorPollSensorGroup6(
const irobotUARTPort_t port,
irobotSensorGroup6_t * const sensorGroup6
){
int32_t status = ERROR_SUCCESS;
// initialize communications buffer
uint8_t txBuffer[OP_SENSORS_SIZE];
uint8_t rxQueueBuffer[SENSOR_SIZE_UPPER_BOUND] = {0x00};
xqueue_t rxQueue;
xqueue_init(&rxQueue, rxQueueBuffer, SENSOR_SIZE_UPPER_BOUND);
// check for NULL pointer
if(!sensorGroup6){
return ERROR_INVALID_PARAMETER;
}
// request packet
txBuffer[0] = OP_SENSORS;
txBuffer[1] = SENSOR_GROUP6;
irobot_StatusMerge(&status, irobotUARTWriteRaw(port, txBuffer, OP_SENSORS_SIZE));
// receive response
if(!irobot_IsError(status)){
irobot_StatusMerge(&status, irobotUARTRead(port, &rxQueue, SENSOR_GROUP6_SIZE));
}
// read response
if(!irobot_IsError(status)){
irobot_StatusMerge(&status, irobotReadSensorGroup6(&rxQueue, sensorGroup6));
}
return status;
}
/* 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;
}
