// How to handle static variables with multiple sensors? objects? add to gpspacket?
int read_gps(struct gps *gpsData_ptr)
{
cyg_io_handle_t port_handle;
cyg_serial_buf_info_t buff_info;
unsigned int len = sizeof (buff_info);
// get serial port handle
cyg_io_lookup( gpsData_ptr->portName, &port_handle );
cyg_io_get_config (port_handle, CYG_IO_GET_CONFIG_SERIAL_BUFFER_INFO,\
&buff_info, &len);
unsigned int bytesInBuffer = buff_info.rx_count;
unsigned int bytesReadThisCall =0;;
unsigned short msgPayloadSize = 0, bytesToRead = 0, bytesRead = 0;
int status =0;
// Initialization of persistent local buffer
if (gpsData_ptr->localBuffer == NULL)
{
gpsData_ptr->localBuffer = (unsigned char*) malloc (1024 * sizeof (unsigned char));
}
// First check if there are any bytes in the serial buffer, return if none
if( bytesInBuffer == 0 )
return -1;
// Get localBuffer stored in gps packet. This is to keep the following code readable
localBuffer = gpsData_ptr->localBuffer;
bytesInLocalBuffer= gpsData_ptr->bytesInLocalBuffer;
readState = gpsData_ptr->readState;
//fprintf(stderr, "read state is %d (before while)\n", readState);
// Keep reading until we've processed all of the bytes in the buffer
while (bytesReadThisCall < bytesInBuffer){
//fprintf(stderr, "read state is %d (after while)\n", readState);
switch (readState){
case 0: //Look for packet header bytes
// Read in up to 4 bytes to the first open location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],4-bytesInLocalBuffer);
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (localBuffer[0] == '$'){ // Check for first header byte
bytesInLocalBuffer = 1;
//fprintf(stderr, "case 0, $ header type \n");
if (localBuffer[1] == 'B'){ // Check for second header byte
bytesInLocalBuffer = 2;
if (localBuffer[2] == 'I'){ // Check for third header byte
bytesInLocalBuffer = 3;
if (localBuffer[3] == 'N'){ // Check for fourth header byte
bytesInLocalBuffer = 4;
readState++; // header complete, move to next stage
}
}
}
}
else {
gpsData_ptr->err_type = noPacketHeader;
}
break; // end case 0
case 1: // Look for block ID and data length
// Find how many bytes need to be read for block ID (2) + data length (2) - bytes already read (includes 4 byte header)
bytesToRead = 8 - bytesInLocalBuffer;
// Read in bytes to the last location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
readState++;
//printf ("\n<GPS>: Got msgID: %d and Data Length: %d", localBuffer[5]*256 + localBuffer[4], localBuffer[7]*256 + localBuffer[6]);
}
else{
gpsData_ptr->err_type = incompletePacket;
}
break; // end case 1
case 2: //Read payload, checksum, and stop bytes
// Find message payload size
msgPayloadSize = localBuffer[7]*256 + localBuffer[6]; // data is in little endian format
// Error checking on payload size. If size is bigger than expected, dump packet
if(msgPayloadSize > GPS_MAX_MSG_SIZE){
gpsData_ptr->err_type = incompletePacket;
reset_localBuffer();
}
// Find how many bytes need to be read for the total message (Header (4) + ID (2) + Size (2) + Payload + checksum (2) + stop (2) - bytes already read )
bytesToRead = msgPayloadSize + 12 - bytesInLocalBuffer;
// Read in the remainder of the message to the local buffer, starting at the first empty location
bytesRead = read (gpsData_ptr->port, &localBuffer[bytesInLocalBuffer], bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
//printf ("\n<GPS>: Got complete message! Tried for %d, got %d",bytesToRead,bytesRead);
//printf ("\n<GPS>: My checksum: %d Recv: %d",do_chksum(localBuffer, 8, msgPayloadSize+8),(localBuffer[8+msgPayloadSize+1]*256 + localBuffer[8+msgPayloadSize]));
// Checksum verification
if ( do_chksum(localBuffer, 8, msgPayloadSize+8) == (localBuffer[8+msgPayloadSize+1]*256 + localBuffer[8+msgPayloadSize]) ){
// If it's OK, extract data
parse_gps( gpsData_ptr );
gpsData_ptr->err_type = data_valid;
//printf ("\n<GPS>: Success!");
status = 1;
}
else{
//printf ("\n<GPS>: Checksum mismatch!");
gpsData_ptr->err_type = checksum_err;
}
reset_localBuffer();
}
else{
//printf ("\n<GPS>: Didn't get complete message. Tried for %d, got %d",bytesToRead,bytesRead);
gpsData_ptr->err_type= incompletePacket;
status = 0;
}
break; // end case 2
default:
reset_localBuffer();
printf ("\n<GPS>: Why are you here?");
status = 0;
break; // end default
} // end switch (readState)
} // end while (bytesReadThisCall < bytesInBuffer)
// Store local buffer in gps packet
gpsData_ptr->localBuffer = localBuffer;
gpsData_ptr->bytesInLocalBuffer = bytesInLocalBuffer;
gpsData_ptr->readState = readState;
return status;
}
int read_gps(struct gps *gpsData_ptr)
{
// serial port buffer handle
cyg_io_handle_t port_handle;
cyg_serial_buf_info_t buff_info;
unsigned int len = sizeof (buff_info);
// get serial port handle
cyg_io_lookup( gpsData_ptr->portName, &port_handle );
cyg_io_get_config (port_handle, CYG_IO_GET_CONFIG_SERIAL_BUFFER_INFO,\
&buff_info, &len);
unsigned int bytesInBuffer = buff_info.rx_count;
unsigned int bytesReadThisCall =0;;
unsigned short msgPayloadSize = 0, bytesToRead = 0, bytesRead = 0;
int status = 0;
// Initialization of persistent local buffer
if (gpsData_ptr->localBuffer == NULL)
{
gpsData_ptr->localBuffer = (unsigned char*) malloc (1024 * sizeof (unsigned char));
}
// First check if there are any bytes in the serial buffer, return if none
if( bytesInBuffer == 0 )
return -1;
// Get localBuffer stored in gps packet. This is to keep the following code readable
localBuffer = gpsData_ptr->localBuffer;
bytesInLocalBuffer= gpsData_ptr->bytesInLocalBuffer;
readState = gpsData_ptr->readState;
// Keep reading until we've processed all of the bytes in the buffer
while (bytesReadThisCall < bytesInBuffer){
switch (readState){
case 0: //Look for packet header bytes
// Read in up to 2 bytes to the first open location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],2-bytesInLocalBuffer);
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (localBuffer[0] == 0xA0){ // Check for first header byte
bytesInLocalBuffer = 1;
if (localBuffer[1] == 0xA2){ // Check for second header byte
bytesInLocalBuffer = 2;
readState++; // header complete, move to next stage
}
}
else {
gpsData_ptr->err_type = noPacketHeader;
}
break; // end case 0
case 1: //Look for message size bytes
// Find how many bytes need to be read for data length (2) - bytes already read (includes 2 byte header)
bytesToRead = 4 - bytesInLocalBuffer;
// Read in up to 2 bytes to the first open location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],bytesToRead);
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
if (bytesRead == bytesToRead){
readState++; // size bytes complete, move to next stage
}
else{
//printf ("\n<GPS>: Failed to get size bytes");
gpsData_ptr->err_type = incompletePacket;
}
break; // end case 1
case 2: //Read payload, checksum, and stop bytes
// Find message payload size
msgPayloadSize = getbeuw(localBuffer,2)&0x7FFF; // packet size is 15 bits. bitwise AND ensures only 15 bits are used.
//printf("<GPS>: msgPayloadSize: %d\n",msgPayloadSize);
// Error checking on payload size. If size is bigger than expected, dump packet
if(msgPayloadSize > GPS_MAX_MSG_SIZE){
gpsData_ptr->err_type = incompletePacket;
reset_localBuffer();
}
// Find how many bytes need to be read for the total message (Header (2) + Size (2) + Payload + checksum (2) + stop (2) - bytes already read )
bytesToRead = msgPayloadSize + 8 - bytesInLocalBuffer;
// Read in the remainder of the message to the local buffer, starting at the first empty location
bytesRead = read (gpsData_ptr->port, &localBuffer[bytesInLocalBuffer], bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
//printf ("\n<GPS>: Got complete message! Tried for %d, got %d",bytesToRead,bytesRead);
//printf("<GPS>: Packet ID %d \n", localBuffer[4]);
//printf("<GPS>: msgPayloadSize: %d\n",msgPayloadSize);
//printf("<GPS>: My checksum: %d Recv: %d\n",do_chksum(localBuffer, 4, msgPayloadSize+4),getbeuw(localBuffer,4+msgPayloadSize));
// Checksum verification (15-bit, big endian)
if ( (do_chksum(localBuffer, 4, msgPayloadSize+4)&0x7FFF) == (getbeuw(localBuffer,4+msgPayloadSize)&0x7FFF) ){
// If it's OK, extract data
parse_gps( gpsData_ptr );
gpsData_ptr->err_type = data_valid;
//printf ("\n<GPS>: Success!");
status = 1;
}
else{
//printf ("\n<GPS>: Checksum mismatch!");
gpsData_ptr->err_type = checksum_err;
}
reset_localBuffer(); // Clear the local buffer regardless
}
else{
//printf ("<GPS>: Didn't get complete message. Tried for %d, got %d. %d\n",bytesToRead,bytesRead,msgPayloadSize);
gpsData_ptr->err_type= incompletePacket;
status = 0;
}
break; // end case 2
default:
reset_localBuffer();
printf ("\n<GPS>: Why are you here?");
status = 0;
break; // end default
} // end switch (readState)
} // end while (bytesReadThisCall < bytesInBuffer)
// Store local buffer in gps packet
gpsData_ptr->localBuffer = localBuffer;
gpsData_ptr->bytesInLocalBuffer = bytesInLocalBuffer;
gpsData_ptr->readState = readState;
return status;
}
// How to handle static variables with multiple sensors? objects? add to gpspacket?
int read_gps(struct gps *gpsData_ptr)
{
cyg_io_handle_t port_handle;
cyg_serial_buf_info_t buff_info;
unsigned int len = sizeof (buff_info);
// get serial port handle
cyg_io_lookup( gpsData_ptr->portName, &port_handle );
cyg_io_get_config (port_handle, CYG_IO_GET_CONFIG_SERIAL_BUFFER_INFO,\
&buff_info, &len);
unsigned int bytesInBuffer = buff_info.rx_count;
unsigned int bytesReadThisCall = 0;
unsigned short msgPayloadSize = 0, bytesToRead = 0, bytesRead = 0;
unsigned long CRC_computed, CRC_read;
int j, status =0;
// Initialization of persistent local buffer
if (gpsData_ptr->localBuffer == NULL)
{
gpsData_ptr->localBuffer = (unsigned char*) malloc (1024 * sizeof (unsigned char));
}
// First check if there are any bytes in the serial buffer, return if none
if( bytesInBuffer == 0 )
return -1;
// Get localBuffer stored in gps packet. This is to keep the following code readable
localBuffer = gpsData_ptr->localBuffer;
bytesInLocalBuffer= gpsData_ptr->bytesInLocalBuffer;
readState = gpsData_ptr->readState;
while (bytesReadThisCall < bytesInBuffer){
switch (readState){
case 0: //Look for packet header bytes
// Read in up to 3 bytes to the first open location in the local buffer
//fprintf(stderr,"bytesInLocalBuffer is %d\n",bytesInLocalBuffer);
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],3-bytesInLocalBuffer);
//fprintf(stderr,"The first three bytes are %0X %0X %0X\n",localBuffer[0],localBuffer[1],localBuffer[2]);
//fprintf(stderr,"bytesRead is %d\n",bytesRead);
//fprintf(stderr,"Read %d bytes, The first three bytes are %0X %0X %0X\n", bytesRead,localBuffer[0],localBuffer[1],localBuffer[2]);
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (localBuffer[0] == 0xAA){ // Check for first header byte
bytesInLocalBuffer = 1;
//fprintf(stderr, "case 0, 0xAA header type \n");
if (localBuffer[1] == 0x44){ // Check for second header byte
bytesInLocalBuffer = 2;
if (localBuffer[2] == 0x12){ // Check for third header byte
bytesInLocalBuffer = 3;
readState++;
}
}
}
else {
gpsData_ptr->err_type = noPacketHeader;
}
break; // end case 0
case 1: // Look for block ID and data length
// Read 28 Header Bytes
bytesToRead = 28 - bytesInLocalBuffer;
// Read in bytes to the last location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
readState++;
//fprintf (stderr,"<GPS>: Got msgID: %d and Data Length: %d\n", localBuffer[5]*256 + localBuffer[4], localBuffer[9]*256 + localBuffer[8]);
//printf ("<GPS>: localBuffer[0] = %02X localBuffer[1] = %02X localBuffer[2] = %02X localBuffer[3] = %02X localBuffer[4] = %02X localBuffer[5] = %02X \n", localBuffer[0], localBuffer[1], localBuffer[2], localBuffer[3], localBuffer[4], localBuffer[5]);
}
else{
gpsData_ptr->err_type = incompletePacket;
}
break; // end case 1
case 2: //Read payload
// Find message payload size
msgPayloadSize = localBuffer[9]*256 + localBuffer[8]; // data is in little endian format
// Error checking on payload size. If size is bigger than expected, dump packet
if(msgPayloadSize > GPS_MAX_MSG_SIZE){
gpsData_ptr->err_type = incompletePacket;
reset_localBuffer();
}
// Find how many bytes need to be read for the total message (Sync (3) + Remaining Header (25) + Payload - bytes already read )
bytesToRead = msgPayloadSize + 28 - bytesInLocalBuffer;
//printf("bytesInLocalBuffer is %d bytesToRead is %d \n",bytesInLocalBuffer,bytesToRead);
// Read in the remainder of the message to the local buffer, starting at the first empty location
bytesRead = read (gpsData_ptr->port, &localBuffer[bytesInLocalBuffer], bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
//printf ("<GPS>: Got complete message! Tried for %d, got %d\n",bytesToRead,bytesRead);
readState++;
}
else {
gpsData_ptr->err_type = incompletePacket;
}
break; // end case 2
case 3: // read CRC bytes (4 bytes)
bytesToRead = 4;
bytesRead = read (gpsData_ptr->port, &localBuffer[bytesInLocalBuffer], bytesToRead);
bytesInLocalBuffer += bytesRead;
bytesReadThisCall += bytesRead;
if(bytesRead == bytesToRead) {
// CRC verification
CRC_computed = CalculateBlockCRC32(bytesInLocalBuffer-4,localBuffer);
endian_swap(localBuffer,140,4);
CRC_read = *(uint32_t *) (localBuffer+140);
if (CRC_computed == CRC_read) {
gpsData_ptr->err_type = data_valid;
parse_gps(gpsData_ptr);
//fprintf (stderr,"<GPS t = %9.3lf>: Success!\n",gpsData_ptr->GPS_TOW);
}
else{
send_status("GPS CRC ERR");
//fprintf (stderr,"<GPS>: Checksum mismatch!\n");
/* ============= DEBUG CHECKSUM ERROR ================
fprintf (stderr,"<GPS %d>: Checksum mismatch! Buffer: %02X%02X%02X%02X Read: %08lX Computed: %08lX\n",localBuffer[5]*256 + localBuffer[4],localBuffer[140],localBuffer[141],localBuffer[142],localBuffer[143],CRC_read,CRC_computed);
fprintf (stderr,"Hex: \n");
for (j = 0; j < bytesInLocalBuffer; j++) {
fprintf(stderr,"%02X ",localBuffer[j]);
if(j%8==7)
fprintf(stderr,"\n");
}
*/
gpsData_ptr->err_type = checksum_err;
}
reset_localBuffer();
}
else{
//printf ("\n<GPS>: Didn't get complete message. Tried for %d, got %d",bytesToRead,bytesRead);
gpsData_ptr->err_type= incompletePacket;
status = 0;
}
break; // end case 3
default:
reset_localBuffer();
printf ("\n<GPS>: Why are you here?");
status = 0;
break; // end default
} // end switch (readState)
} // end while (bytesReadThisCall < bytesInBuffer)
// Store local buffer in gps packet
gpsData_ptr->localBuffer = localBuffer;
gpsData_ptr->bytesInLocalBuffer = bytesInLocalBuffer;
gpsData_ptr->readState = readState;
return status;
}