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; }