Esempio n. 1
0
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;
}
Esempio n. 2
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;
}