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;
}
/* @funcstatic Calculate_Course_Lap_Data *******************************
**
** Calculate lap data totals from individual track points. Also
** generates time stamps for track points if they don't have
** time stamps yet (using an arbitrary speed of 10 km/h which is
** currently hardcoded. This is required so that couse points can
** refer to track points and identify them uniquely.
**
** @param [w] clp [GPS_PCourse_Lap] course lap to be calculated
** @param [r] ctk [GPS_PTrack *] track array to calculate lap from
** @param [r] ctk_start [int] start index of lap in track array
** @param [r] ctk_end [int] end index of lap in track array
**
** @return [void]
************************************************************************/
static void
Calculate_Course_Lap_Data(GPS_PCourse_Lap clp, GPS_PTrack *ctk,
int ctk_start, int ctk_end)
{
int i;
double heartrate_sum = 0, cadence_sum = 0;
int heartrate_sum_time = 0, cadence_sum_time = 0;
double time_synth_speed = 10.0 * 1000 / 3600; /* speed in m/s */
if (ctk_start && ctk_end && !ctk[ctk_start]->Time)
ctk[ctk_start]->Time = GPS_Time_Now();
else
time_synth_speed = 0;
clp->total_dist = 0;
clp->avg_heart_rate = 0;
clp->max_heart_rate = 0;
clp->intensity = 0;
clp->avg_cadence = 0xff;
for (i=ctk_start; i <= ctk_end; i++)
{
if (ctk[i]->heartrate && ctk[i]->heartrate > clp->max_heart_rate)
clp->max_heart_rate = ctk[i]->heartrate;
if (i < ctk_end)
{
double dist = 0;
int seg_time;
if (!ctk[i]->no_latlon && !ctk[i+1]->no_latlon)
dist = gcgeodist(ctk[i]->lat, ctk[i]->lon,
ctk[i+1]->lat, ctk[i+1]->lon);
clp->total_dist += dist;
if (time_synth_speed)
ctk[i+1]->Time = ctk[i]->Time + (dist / time_synth_speed + 0.5);
seg_time = ctk[i+1]->Time - ctk[i]->Time;
if (ctk[i]->heartrate)
{
heartrate_sum += ctk[i]->heartrate * seg_time;
heartrate_sum_time += seg_time;
}
if (ctk[i]->cadence)
{
cadence_sum += ctk[i]->cadence * seg_time;
cadence_sum_time += seg_time;
}
}
}
clp->total_time = 0;
clp->begin_lat = 0x7fffffff;
clp->begin_lon = 0x7fffffff;
clp->end_lat = 0x7fffffff;
clp->end_lon = 0x7fffffff;
if (ctk_start && ctk_end)
{
clp->total_time = (ctk[ctk_end]->Time - ctk[ctk_start]->Time) * 100;
if (!ctk[ctk_start]->no_latlon && !ctk[ctk_end]->no_latlon)
{
clp->begin_lat = ctk[ctk_start]->lat;
clp->begin_lon = ctk[ctk_start]->lon;
clp->end_lat = ctk[ctk_end]->lat;
clp->end_lon = ctk[ctk_end]->lon;
}
}
if (heartrate_sum_time)
clp->avg_heart_rate = heartrate_sum / heartrate_sum_time;
if (cadence_sum_time)
clp->avg_cadence = cadence_sum / cadence_sum_time;
}