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