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;
}
int32_t irobotUARTRead(
const irobotUARTPort_t port,
xqueue_t * const queue,
size_t nData
){
int32_t status = ERROR_SUCCESS;
// catch NULL pointers
if(!queue){
irobot_StatusMerge(&status, ERROR_INVALID_PARAMETER);
}
// read
while(!irobot_IsError(status) && nData--){
uint8_t rxByte = 0;
irobot_StatusMerge(&status, irobotUARTReadRaw(port, &rxByte, 1));
if(!irobot_IsError(status)){
xqueue_push8(queue, rxByte);
}
if(status == 1073676294){
status = 0; // ignore VISA "num bytes req = num bytes read"
}
}
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;
}
/* Set the iRobot baud rate; the serial port will be reconfigured to this new
baud rate and restarted, which will clear communication buffers and may introduce a delay.
Note: Baud 115200 appears to be unstable. */
extern int32_t irobotBaudChange(
const irobotUARTPort_t port, /* (in) UART port */
const irobotBaud_t baud /* (in) iRobot baud code */
){ /* (ret) Error / success code */
int32_t status = ERROR_SUCCESS;
uint8_t packet[OP_BAUD_SIZE];
packet[0] = OP_BAUD;
packet[1] = (uint8_t)baud;
if(!irobot_IsError(status)){
irobot_StatusMerge(&status, irobotUARTWriteRaw(port, packet, OP_BAUD_SIZE));
}
if(!irobot_IsError(status)){
irobot_StatusMerge(&status, irobotUARTClose(port));
}
if(!irobot_IsError(status)){
irobot_StatusMerge(&status, irobotUARTOpen(port, baud));
irobotDelayMs(50); /* delay 50ms */
}
return status;
}
int32_t irobotUARTOpen(
const irobotUARTPort_t port,
const irobotBaud_t baud
){
int32_t status = ERROR_SUCCESS;
uint32_t baudRate = 0;
irobot_StatusMerge(&status, irobotUARTBaudCodeToRate(baud, &baudRate));
if(!irobot_IsError(status)){
irobot_StatusMerge(
&status,
Uart_Open(
port,
baudRate,
8,
Uart_StopBits1_0,
Uart_ParityNone
)
);
}
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;
}
