int32 GPS_Serial_Flush(gpsdevh *fd)
{
unsigned char u;
int n;
GPS_Diag("Flush:");
n=0;
while( (GPS_Serial_Chars_Ready(fd)) && (n < 1000))
{
GPS_Serial_Read(fd,&u,1);
GPS_Diag("%02x ", u);
n++;
}
GPS_Diag("\r\n");
return 1;
}
int32 GPS_Serial_Packet_Read(gpsdevh *fd, GPS_PPacket *packet)
{
time_t start;
int32 n;
int32 len;
UC u;
int32 isDLE;
UC *p;
uint32 i;
UC chk=0, chk_read;
const char *m1;
const char *m2;
len = 0;
isDLE = gpsFalse;
p = (*packet)->data;
start = GPS_Time_Now();
GPS_Diag("Rx Data:");
while(GPS_Time_Now() < start+GPS_TIME_OUT)
{
if((n=GPS_Serial_Chars_Ready(fd)))
{
if(GPS_Serial_Read(fd,&u,1)==-1)
{
perror("read");
GPS_Error("GPS_Packet_Read: Read error");
gps_errno = FRAMING_ERROR;
return 0;
}
GPS_Diag("%02x ", u);
if(!len)
{
if(u != DLE)
{
(void) fprintf(stderr,"GPS_Packet_Read: No DLE. Data received, but probably not a garmin packet.\n");
(void) fflush(stderr);
return 0;
}
++len;
continue;
}
if(len==1)
{
(*packet)->type = u;
++len;
continue;
}
if(u == DLE)
{
if(isDLE)
{
isDLE = gpsFalse;
continue;
}
isDLE = gpsTrue;
}
if(len == 2)
{
(*packet)->n = u;
len = -1;
continue;
}
if(u == ETX)
if(isDLE)
{
if(p-(*packet)->data-2 != (*packet)->n)
{
GPS_Error("GPS_Packet_Read: Bad count");
gps_errno = FRAMING_ERROR;
return 0;
}
chk_read = *(p-2);
for(i=0,p=(*packet)->data;i<(*packet)->n;++i)
chk -= *p++;
chk -= (*packet)->type;
chk -= (UC)((*packet)->n);
if(chk != chk_read)
{
GPS_Error("CHECKSUM: Read error\n");
gps_errno = FRAMING_ERROR;
return 0;
}
m1 = Get_Pkt_Type((*packet)->type, (*packet)->data[0], &m2);
if (gps_show_bytes) {
GPS_Diag(" ");
for (i = 0; i < (*packet)->n; i++) {
char c = (*packet)->data[i];
GPS_Diag("%c", c); //isalnum(c) ? c : '.');
}
GPS_Diag(" ");
}
GPS_Diag("(%-8s%s)\n", m1, m2 ? m2 : "");
return (*packet)->n;
}
if (p - (*packet)->data >= MAX_GPS_PACKET_SIZE)
{
GPS_Error("GPS_Serial_Packet_Read: Bad payload size/no ETX found");
gps_errno = FRAMING_ERROR;
return 0;
}
*p++ = u;
}
}
GPS_Error("GPS_Packet_Read: Timeout. No data received.");
gps_errno = SERIAL_ERROR;
return 0;
}
