//COM 中断接收
void USART1_IRQHandler()
{
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET && get_PV())//
{
GPS_Buff[GPS_Buff_ptr++] = USART_ReceiveData(USART1);
if(GPS_Buff[GPS_Buff_ptr-1]==0x12 )
{
if(GPS_Buff[GPS_Buff_ptr-2]==0x44 && GPS_Buff[GPS_Buff_ptr-3]==0xaa)
{
GPS_Buff_ptr=3;
GPS_Buff[0]=0xaa;
GPS_Buff[1]=0x44;
GPS_Buff[2]=0x12;
}
}
if (GPS_Buff_ptr==4)
GPSMsg.Len_Header=GPS_Buff[3];
else if (GPS_Buff_ptr==6)
GPSMsg.ID=*(uint16_t*)(&GPS_Buff[4]);
else if (GPS_Buff_ptr==10)
GPSMsg.Len_Msg=*(uint16_t*)(&GPS_Buff[8]);
if (GPS_Buff_ptr==(GPSMsg.Len_Header + GPSMsg.Len_Msg +4))
{
if(CalculateBlockCRC32(GPSMsg.Len_Header + GPSMsg.Len_Msg +4, GPS_Buff)==0);//60us
{
if (GPSMsg.ID==42)
{
GPSMsg.lat=*((double*)(&GPS_Buff[GPSMsg.Len_Header+8]));
GPSMsg.lon=*((double*)(&GPS_Buff[GPSMsg.Len_Header+16]));
GPS_pos_flag=1;
}
else if (GPSMsg.ID==99)
{
GPSMsg.hor_spd=*((double*)(&GPS_Buff[GPSMsg.Len_Header+16]));
GPSMsg.trk_gnd=*((double*)(&GPS_Buff[GPSMsg.Len_Header+24]));
GPS_vel_flag=1;
printf("%f %f\r\n",GPSMsg.hor_spd, GPSMsg.trk_gnd);
}
}
}
}
else
USART_ReceiveData(USART1);
}
bool
AP_GPS_NOVA::parse(uint8_t temp)
{
switch (nova_msg.nova_state)
{
default:
case nova_msg_parser::PREAMBLE1:
if (temp == NOVA_PREAMBLE1)
nova_msg.nova_state = nova_msg_parser::PREAMBLE2;
nova_msg.read = 0;
break;
case nova_msg_parser::PREAMBLE2:
if (temp == NOVA_PREAMBLE2)
{
nova_msg.nova_state = nova_msg_parser::PREAMBLE3;
}
else
{
nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
}
break;
case nova_msg_parser::PREAMBLE3:
if (temp == NOVA_PREAMBLE3)
{
nova_msg.nova_state = nova_msg_parser::HEADERLENGTH;
}
else
{
nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
}
break;
case nova_msg_parser::HEADERLENGTH:
Debug("NOVA HEADERLENGTH\n");
nova_msg.header.data[0] = NOVA_PREAMBLE1;
nova_msg.header.data[1] = NOVA_PREAMBLE2;
nova_msg.header.data[2] = NOVA_PREAMBLE3;
nova_msg.header.data[3] = temp;
nova_msg.header.nova_headeru.headerlength = temp;
nova_msg.nova_state = nova_msg_parser::HEADERDATA;
nova_msg.read = 4;
break;
case nova_msg_parser::HEADERDATA:
if (nova_msg.read >= sizeof(nova_msg.header.data)) {
Debug("parse header overflow length=%u\n", (unsigned)nova_msg.read);
nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
break;
}
nova_msg.header.data[nova_msg.read] = temp;
nova_msg.read++;
if (nova_msg.read >= nova_msg.header.nova_headeru.headerlength)
{
nova_msg.nova_state = nova_msg_parser::DATA;
}
break;
case nova_msg_parser::DATA:
if (nova_msg.read >= sizeof(nova_msg.data)) {
Debug("parse data overflow length=%u msglength=%u\n", (unsigned)nova_msg.read,nova_msg.header.nova_headeru.messagelength);
nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
break;
}
nova_msg.data.bytes[nova_msg.read - nova_msg.header.nova_headeru.headerlength] = temp;
nova_msg.read++;
if (nova_msg.read >= (nova_msg.header.nova_headeru.messagelength + nova_msg.header.nova_headeru.headerlength))
{
Debug("NOVA DATA exit\n");
nova_msg.nova_state = nova_msg_parser::CRC1;
}
break;
case nova_msg_parser::CRC1:
nova_msg.crc = (uint32_t) (temp << 0);
nova_msg.nova_state = nova_msg_parser::CRC2;
break;
case nova_msg_parser::CRC2:
nova_msg.crc += (uint32_t) (temp << 8);
nova_msg.nova_state = nova_msg_parser::CRC3;
break;
case nova_msg_parser::CRC3:
nova_msg.crc += (uint32_t) (temp << 16);
nova_msg.nova_state = nova_msg_parser::CRC4;
break;
case nova_msg_parser::CRC4:
nova_msg.crc += (uint32_t) (temp << 24);
nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
uint32_t crc = CalculateBlockCRC32((uint32_t)nova_msg.header.nova_headeru.headerlength, (uint8_t *)&nova_msg.header.data, (uint32_t)0);
crc = CalculateBlockCRC32((uint32_t)nova_msg.header.nova_headeru.messagelength, (uint8_t *)&nova_msg.data, crc);
if (nova_msg.crc == crc)
{
return process_message();
}
else
{
Debug("crc failed");
crc_error_counter++;
}
break;
}
return false;
}
int main(int argc, char *argv[])
{
char *azcPort = "/dev/ttyS0";
char *azcBaud = "230400";
int iBaudRate = 230400;
int iPort;
int iBytesRead = 0;
unsigned char azucBuffer[MAX_LOG_LENGTH];
unsigned char ucHeaderLength = 0;
int iTotalBytes = 0;
unsigned short usMessageLength = 0;
unsigned short usMessageID = 0;
unsigned long ulCRC_Calc, ulCRC_Tx;
/** controls if shared memory is open for use: 0 - No, 1 - Yes */
int iUseSharedMemory = 0;
struct termios tPortSettings;
/** structure to store positions */
BESTPOS BestPosData;
RANGE RangeData;
GPSEPHEM GPSEphemeris[MAX_PRNS];
int iAbandonPacket = 0;
long lMeasurementNumber = 0;
GPSTime GPSTimeStamp;
unsigned long ulGPSMilliSeconds;
unsigned long ulPRN;
/* shared memory objects */
int shmGPSId;
key_t shmGPSKey;
/* GPS Data reference */
GPSData *ptGPSData;
/* setup signal handlers */
signal(SIGHUP, HandleSignal);
signal(SIGKILL, HandleSignal);
signal(SIGTERM, HandleSignal);
signal(SIGALRM, HandleSignal);
signal(SIGINT, HandleSignal);
/* read input parameters */
switch(argc)
{
case 1:
/* defaults */
break;
case 2:
/* set port */
azcPort = argv[1];
break;
case 3:
/* set port and baud */
azcPort = argv[1];
azcBaud = argv[2];
switch(atoi(azcBaud))
{
case 9600: iBaudRate = B9600; break;
case 19200: iBaudRate = B19200; break;
case 38400: iBaudRate = B38400; break;
case 57600: iBaudRate = B57600; break;
case 115200: iBaudRate = B115200; break;
case 230400: iBaudRate = B230400; break;
default: fprintf(stderr,"Unknown Baud - setting default B230400!\n"); iBaudRate = B230400; break;
}
break;
default:
fprintf(stderr,"%s Unknown Argument List... Exiting\n",MODULE_NAME);
exit(-1);
break;
}
fprintf(stdout,"%s Using Port: %s at %s\n",MODULE_NAME, azcPort, azcBaud);
/* open port */
iPort = open(azcPort, O_RDWR);
if(iPort < 0)
{
fprintf(stderr,"Cannot open port, %s:",azcPort);
perror("open()");
exit(-1);
}
/* initialise port settings */
bzero(&tPortSettings,sizeof(tPortSettings));
/* configure port settings */
tPortSettings.c_iflag &= ~(IGNPAR|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON);
/*tPortSettings.c_iflag |= (IXON|IXOFF); // enable software flow control */
tPortSettings.c_cflag &= ~(CSIZE|PARENB);
/*tPortSettings.c_cflag |= (B115200|CS8|CLOCAL|CREAD); */
tPortSettings.c_cflag |= (CS8|CLOCAL|CREAD);
/* Set the Baud Rate (same for input and output) */
cfsetispeed(&tPortSettings, iBaudRate);
cfsetospeed(&tPortSettings, iBaudRate);
tPortSettings.c_oflag &= ~(OPOST);
tPortSettings.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN);
tPortSettings.c_cc[VTIME] = 10; /* wait for 1 second before returning */
tPortSettings.c_cc[VMIN] = 0; /* can return with no data */
tcflush(iPort,TCIFLUSH);
tcsetattr(iPort,TCSANOW,&tPortSettings);
/* connect to shared memory for GPS*/
shmGPSKey = GPSDATA_KEY;
if((shmGPSId = shmget(shmGPSKey, sizeof(GPSData), 0666)) < 0)
{
perror("shmget(): cannot find GPS data storage - no shared mem used");
iUseSharedMemory = 0;
} else
{
/* shared memory is intiialised */
iUseSharedMemory = 1;
}
/* attach */
if(iUseSharedMemory == 1)
{
if((ptGPSData = shmat(shmGPSId, NULL, 0)) == (GPSData *) -1)
{
perror("shmat(): attaching GPS Data Storage");
exit(-1);
}
}
log("Initialised GPS Store - Hit CTRL-C to exit");
while(iShutdown == 0)
{
/* clear buffer */
memset(azucBuffer,0,MAX_LOG_LENGTH);
iAbandonPacket = 0;
/* synchronoise packet - look for binary header - 0xAA 0x44 0x12 */
do
{
azucBuffer[0] = azucBuffer[1];
azucBuffer[1] = azucBuffer[2];
iBytesRead = read(iPort,&azucBuffer[2],1);
if(iShutdown == 1)
{
iAbandonPacket=1;
break;
}
} while(azucBuffer[0] != 0xAA && azucBuffer[1] != 0x44 && azucBuffer[2] != 0x12);
if(iAbandonPacket == 1)
{
continue;
}
/* read header length */
iBytesRead = read(iPort,&azucBuffer[3],1);
if(iBytesRead == 1)
{
ucHeaderLength = azucBuffer[3];
} else
{
log("Could not read header length");
break;
}
/* sanity check on header length */
if(ucHeaderLength > 28)
{
/* too large - try again */
err("Header unexpectedly large");
iAbandonPacket = 1;
continue;
}
/* we have already received the first 4 bytes */
iTotalBytes = 4;
/* read rest of header */
while(iTotalBytes<ucHeaderLength)
{
iBytesRead = read(iPort,&azucBuffer[iTotalBytes],MAX_LOG_LENGTH-2);
iTotalBytes += iBytesRead;
if(iTotalBytes > MAX_LOG_LENGTH)
{
err("Too many bytes received");
iAbandonPacket = 1;
break;
}
}
if(iAbandonPacket == 1)
{
continue;
}
/* read message length */
memcpy(&usMessageLength,&azucBuffer[8],2);
/* receive rest of message */
while(iTotalBytes<(ucHeaderLength + usMessageLength + 4)) /* 4 is the length of the CRC */
{
iBytesRead = read(iPort,&azucBuffer[iTotalBytes],MAX_LOG_LENGTH-2);
iTotalBytes += iBytesRead;
if(iTotalBytes > MAX_LOG_LENGTH)
{
err("Too many bytes received");
iAbandonPacket = 1;
break;
}
}
if(iAbandonPacket == 1)
{
continue;
}
/* get the message ID */
memcpy(&usMessageID,&azucBuffer[4],2);
/* debug message */
fprintf(stdout,"Message type: %d received, %d bytes total\n",usMessageID, iTotalBytes);
/* perform checksum */
ulCRC_Calc = CalculateBlockCRC32(ucHeaderLength+usMessageLength,azucBuffer);
/* retrieve checksum from message */
memcpy(&ulCRC_Tx,&azucBuffer[ucHeaderLength+usMessageLength],4);
if(ulCRC_Tx != ulCRC_Calc)
{
/* we have an error */
log("CRC Check Error");
} else
{
/* get the time stamp */
memcpy(&GPSTimeStamp.usGPSWeek,&azucBuffer[14],2);
memcpy(&ulGPSMilliSeconds,&azucBuffer[16],4);
GPSTimeStamp.fGPSSecondOfWeek = ((float)ulGPSMilliSeconds) / 1000.0;
/* extract the message data */
switch(usMessageID)
{
case NOVATEL_BESTPOS:
/* copy timestamp */
memcpy(&BestPosData.gtTimeStamp,&GPSTimeStamp,sizeof(GPSTime));
/*extract data */
memcpy(&BestPosData.dLatitude,&azucBuffer[ucHeaderLength+8],8);
memcpy(&BestPosData.dLongitude,&azucBuffer[ucHeaderLength+16],8);
memcpy(&BestPosData.dHeight,&azucBuffer[ucHeaderLength+24],8);
memcpy(&BestPosData.fUndulation,&azucBuffer[ucHeaderLength+32],4);
memcpy(&BestPosData.ulDatumID,&azucBuffer[ucHeaderLength+36],4);
memcpy(&BestPosData.fLatitudeSigma,&azucBuffer[ucHeaderLength+40],4);
memcpy(&BestPosData.fLongitudeSigma,&azucBuffer[ucHeaderLength+44],4);
memcpy(&BestPosData.fHeightSigma,&azucBuffer[ucHeaderLength+48],4);
memcpy(&BestPosData.azucBaseStationID[0],&azucBuffer[ucHeaderLength+52],4);
memcpy(&BestPosData.fDifferentialAge,&azucBuffer[ucHeaderLength+56],4);
memcpy(&BestPosData.fSolutionAge,&azucBuffer[ucHeaderLength+60],4);
memcpy(&BestPosData.ucNumberObservationsTracked,&azucBuffer[ucHeaderLength+64],1);
memcpy(&BestPosData.ucNumberL1ObservationsUsed,&azucBuffer[ucHeaderLength+65],1);
memcpy(&BestPosData.ucNumberL1ObservationsAboveRTKMaskAngle,&azucBuffer[ucHeaderLength+66],1);
memcpy(&BestPosData.ucNumberL2ObservationsAboveRTKMaskAngle,&azucBuffer[ucHeaderLength+67],1);
fprintf(stdout,"BESTPOS: %lf %lf %lf\n",BestPosData.dLatitude,
BestPosData.dLongitude,
BestPosData.dHeight);
break;
case NOVATEL_RANGE: /* pseudorange measurements */
/* copy timestamp */
memcpy(&RangeData.gtTimeStamp,&GPSTimeStamp,sizeof(GPSTime));
/* get number of measurements */
memcpy(&RangeData.lNumberObservations,&azucBuffer[ucHeaderLength],4);
fprintf(stdout,"RANGE: %ld Measurements\n",RangeData.lNumberObservations);
/* limit max measurements */
if(RangeData.lNumberObservations > NOVATEL_MAXCHANNELS)
{
RangeData.lNumberObservations = NOVATEL_MAXCHANNELS;
}
for(lMeasurementNumber=0;lMeasurementNumber<RangeData.lNumberObservations;lMeasurementNumber++)
{
memcpy(&RangeData.usPRN[lMeasurementNumber],
&azucBuffer[ucHeaderLength + 4 + lMeasurementNumber*44],2);
memcpy(&RangeData.usGlonassFrequency[lMeasurementNumber],
&azucBuffer[ucHeaderLength + 6 + lMeasurementNumber*44],2);
memcpy(&RangeData.dPseudorange[lMeasurementNumber],
&azucBuffer[ucHeaderLength + 8 + lMeasurementNumber*44],8);
memcpy(&RangeData.fPseudorangeSigma[lMeasurementNumber],
&azucBuffer[ucHeaderLength + 16 + lMeasurementNumber*44],4);
memcpy(&RangeData.dCarrierPhase[lMeasurementNumber],
&azucBuffer[ucHeaderLength + 20 + lMeasurementNumber*44],8);
memcpy(&RangeData.fCarrierPhaseSigma[lMeasurementNumber],
&azucBuffer[ucHeaderLength + 28 + lMeasurementNumber*44],4);
memcpy(&RangeData.fDoppler[lMeasurementNumber],
&azucBuffer[ucHeaderLength + 32 + lMeasurementNumber*44],4);
memcpy(&RangeData.fCNo[lMeasurementNumber],
&azucBuffer[ucHeaderLength + 36 + lMeasurementNumber*44],4);
memcpy(&RangeData.fLockTime[lMeasurementNumber],
&azucBuffer[ucHeaderLength + 40 + lMeasurementNumber*44],4);
memcpy(&RangeData.ulTrackingStatus[lMeasurementNumber],
&azucBuffer[ucHeaderLength + 44 + lMeasurementNumber*44],4);
/* fprintf(stdout,"%d %f: RANGE: PRN:%d %lf\n",
RangeData.gtTimeStamp.usGPSWeek,
RangeData.gtTimeStamp.fGPSSecondOfWeek,
RangeData.usPRN[lMeasurementNumber],
RangeData.dPseudorange[lMeasurementNumber]);
*/
}
break;
case NOVATEL_GPSEPHEM: /* satellite ephemeris */
/* get the PRN */
memcpy(&ulPRN,&azucBuffer[ucHeaderLength],4);
/* copy timestamp */
memcpy(&GPSEphemeris[ulPRN].gtTimeStamp,&GPSTimeStamp,sizeof(GPSTime));
/* copy ephemeris parameters */
memcpy(&GPSEphemeris[ulPRN].ulPRN,&azucBuffer[ucHeaderLength],4);
memcpy(&GPSEphemeris[ulPRN].dTOW,&azucBuffer[ucHeaderLength+4],8);
memcpy(&GPSEphemeris[ulPRN].ulHealth,&azucBuffer[ucHeaderLength+12],4);
memcpy(&GPSEphemeris[ulPRN].ulIODE1,&azucBuffer[ucHeaderLength+16],4);
memcpy(&GPSEphemeris[ulPRN].ulIODE2,&azucBuffer[ucHeaderLength+20],4);
memcpy(&GPSEphemeris[ulPRN].ulGPSWeek,&azucBuffer[ucHeaderLength+24],4);
memcpy(&GPSEphemeris[ulPRN].ulZWeek,&azucBuffer[ucHeaderLength+28],4);
memcpy(&GPSEphemeris[ulPRN].dTOE,&azucBuffer[ucHeaderLength+32],8);
memcpy(&GPSEphemeris[ulPRN].dA,&azucBuffer[ucHeaderLength+40],8);
memcpy(&GPSEphemeris[ulPRN].dDeltaN,&azucBuffer[ucHeaderLength+48],8);
memcpy(&GPSEphemeris[ulPRN].dM0,&azucBuffer[ucHeaderLength+56],8);
memcpy(&GPSEphemeris[ulPRN].dEccentricity,&azucBuffer[ucHeaderLength+64],8);
memcpy(&GPSEphemeris[ulPRN].dOmega,&azucBuffer[ucHeaderLength+72],8);
memcpy(&GPSEphemeris[ulPRN].dcuc,&azucBuffer[ucHeaderLength+80],8);
memcpy(&GPSEphemeris[ulPRN].dcus,&azucBuffer[ucHeaderLength+88],8);
memcpy(&GPSEphemeris[ulPRN].dcrc,&azucBuffer[ucHeaderLength+96],8);
memcpy(&GPSEphemeris[ulPRN].dcrs,&azucBuffer[ucHeaderLength+104],8);
memcpy(&GPSEphemeris[ulPRN].dcic,&azucBuffer[ucHeaderLength+112],8);
memcpy(&GPSEphemeris[ulPRN].dcis,&azucBuffer[ucHeaderLength+120],8);
memcpy(&GPSEphemeris[ulPRN].dInclination0,&azucBuffer[ucHeaderLength+128],8);
memcpy(&GPSEphemeris[ulPRN].dInclination_dot,&azucBuffer[ucHeaderLength+136],8);
memcpy(&GPSEphemeris[ulPRN].dOmega0,&azucBuffer[ucHeaderLength+144],8);
memcpy(&GPSEphemeris[ulPRN].dOmega_dot,&azucBuffer[ucHeaderLength+152],8);
memcpy(&GPSEphemeris[ulPRN].ulIODC,&azucBuffer[ucHeaderLength+160],4);
memcpy(&GPSEphemeris[ulPRN].dTOC,&azucBuffer[ucHeaderLength+164],8);
memcpy(&GPSEphemeris[ulPRN].dTGD,&azucBuffer[ucHeaderLength+172],8);
memcpy(&GPSEphemeris[ulPRN].dA_f0,&azucBuffer[ucHeaderLength+180],8);
memcpy(&GPSEphemeris[ulPRN].dA_f1,&azucBuffer[ucHeaderLength+188],8);
memcpy(&GPSEphemeris[ulPRN].dA_f2,&azucBuffer[ucHeaderLength+196],8);
memcpy(&GPSEphemeris[ulPRN].ulAntiSpoofing,&azucBuffer[ucHeaderLength+204],4);
memcpy(&GPSEphemeris[ulPRN].dN,&azucBuffer[ucHeaderLength+208],8);
memcpy(&GPSEphemeris[ulPRN].dURA,&azucBuffer[ucHeaderLength+216],8);
fprintf(stdout,"PRN%ld Ephemeris Stored\n",GPSEphemeris[ulPRN].ulPRN);
break;
default:
log("Unknown Message");
break;
}
}
}
/* shutdown */
close(iPort);
log("Shutting Down");
/* exit */
return 0;
} /* main */
// 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;
}
