Predicitive GPS Lap Timer (PC Prototype Version)

PIR

Here is a data file representing several laps of the Portland International Raceway. The data consists of 7,376 5Hz GPS RMC (filtered) sentences extracted from a Race Technology DL-1 Data Logger. The file has a .doc extension but its actually formatted as a text (.txt) file.

Race Technology DL-1

The following test code reads the text file and processes it. This code will form the basis of my µC-based GPS Predictive Lap Timer. The code uses a few unconventional tricks in order to minimize the use of time-consuming and space-wasting floating point variables and math functions. Therefore, DO NOT USE THIS CODE unless you fully understand these limitations.

My next step is to translate this code for the AVR, and start Hardware-in-the-loop (HIL) simulation using my Arduino GPS Simulator.

Here is the Microsoft Visual C++ 2010 Express Test Code:

/*
  Predictive GPS Lap Timer Test
  All Rights Reserved.
  James M. Eli
  3/30/2012
*/
//
//pelles c version
//
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#define FALSE                         0
#define TRUE                          1

#define MIN(x, y)                     (((x) < (y)) ? (x) : (y))
#define MAX(x, y)                     (((x) > (y)) ? (x) : (y))
#define DEGTORAD(deg)                 (deg*57.29577995)

#define MAX_TIME                      1500 //(5*60*5) 5Hz*60seconds*5minutes
#define GPS_STRING_LENGTH             96   //maximum possible characters in a GPS string (+ fudge)

#define PREFIX_MODE                   0x01
#define PRESTART_MODE                 0x02
#define RUNNING_MODE                  0x04
#define STOPPED_MODE                  0x08
#define LATITUDE                      0x01
#define LONGITUDE                     0x02

//gps rmc sentence fields
#define RMC_PREFIX                    0
#define RMC_TIME                      1
#define RMC_STATUS                    2
#define RMC_LATITUDE                  3
#define RMC_LATITUDE_PREFIX           4
#define RMC_LONGITUDE                 5
#define RMC_LONGITUDE_PREFIX          6
#define RMC_SPEED                     7
#define RMC_TRACK                     8
#define RMC_DATE                      9
#define RMC_MAGNETIC_VARIATION        10
#define RMC_MAGNETIC_VARIATION_PREFIX 11
#define RMC_FAA_MODE_INDICATOR        12
#define RMC_CHECKSUM                  13
//number of fields in gps rmc string
#define RMC_FIELDS                    (RMC_CHECKSUM + 1)

static const float LINE_WIDTH = 50.0f;
static const float LINE_WIDTH_2 = 25.0f;
static const float PROJECTION_DISTANCE = 100.0f;

typedef struct speed_ {
  unsigned long total;
  unsigned short *pSpeed;
} speed;
speed current, best;

typedef struct point_ {
  long x, y;
} point;

typedef struct line_ {
  point p1, p2;
} line;

//arrays of current/best lap instanteous velocities 
unsigned short speed_array_1[MAX_TIME];
unsigned short speed_array_2[MAX_TIME];
//best lap total speed
float best_average;
//coordinates of start/finish location
point start;
//startline endpoints
line sf;
//heading crossing start/finish
short start_heading;
//mode of operation: PREFIX, PRESTART, RUN, STOP
unsigned char mode;

//File name (Path)
const char *filePath = "pir.txt";
FILE *file = NULL;

//construct a startline
void StartLine(float sx, float sy, float shdg) {
  float tx, ty; //projected track coordinates
  float m, b;   //slope & y intercept
	
  //project racetrack along current heading
  tx = sx + PROJECTION_DISTANCE*cos(DEGTORAD(shdg));
  ty = sy + PROJECTION_DISTANCE*sin(DEGTORAD(shdg));
  //projected racetrack slope & y-intercept 
  m = (sy - ty)/(sx - tx);
  b = sy - (m*sx);

  //construct perpendicular (startline) slope & y-intercept
  m = -1.0/m;
  b = sy - (m*sx);

  //define endpoints of the perpendicular
  tx = sx + LINE_WIDTH_2; //note" tx re-used as a temporary value here
  sf.p1.y = (long)(m*tx + b);
  sf.p1.x = (long)tx;
  tx -= LINE_WIDTH;
  sf.p2.y = (long)(m*tx + b);
  sf.p2.x = (long)tx;
}

//2d line intersection
unsigned char LineIntersection(line track) {
  long z;
  int s1, s2, s3, s4;

  //quick rejection test
  if (!(MAX(sf.p1.x, sf.p2.x) >= MIN(track.p1.x, track.p2.x) && MAX(track.p1.x, track.p2.x) >= MIN(sf.p1.x, sf.p2.x) && 
	    MAX(sf.p1.y, sf.p2.y) >= MIN(track.p1.y, track.p2.y) && MAX(track.p1.y, track.p2.y) >= MIN(sf.p1.y, sf.p2.y))) {
    return FALSE;
  }
  //straddle tests
  if ((z = ((track.p1.x - sf.p1.x)*(sf.p2.y - sf.p1.y)) - ((track.p1.y - sf.p1.y)*(sf.p2.x - sf.p1.x))) < 0)
    s1 = -1; //counterclockwise
  else if (z > 0)
    s1 = 1;  //clockwise
  else
    s1 = 0;  //collinear
  
  if ((z = ((track.p2.x - sf.p1.x)*(sf.p2.y - sf.p1.y)) - ((track.p2.y - sf.p1.y)*(sf.p2.x - sf.p1.x))) < 0)
    s2 = -1;
  else if (z > 0)
    s2 = 1;
  else
    s2 = 0;
  
  if ((z = ((sf.p1.x - track.p1.x)*(track.p2.y - track.p1.y)) - ((sf.p1.y - track.p1.y)*(track.p2.x - track.p1.x))) < 0)
    s3 = -1;
  else if (z > 0)
    s3 = 1;
  else
    s3 = 0;
  
  if ((z = ((sf.p2.x - track.p1.x)*(track.p2.y - track.p1.y)) - ((sf.p2.y - track.p1.y)*(track.p2.x - track.p1.x))) < 0)
    s4 = -1;
  else if (z > 0)
    s4 = 1;
  else
    s4 = 0;
  
  if ((s1*s2 <= 0) && (s3*s4 <= 0))
    return TRUE;
  //line segments do not intersect
  return FALSE;
}

//parse gps csv string
int ParseRMC(char *line, char *strings[]) {
  char *p;
  unsigned char n, size = RMC_FIELDS;

  p = line;
  n = 0;
  for ( ; ; ) {
	//ditch leading commas
    while (*p == ',')
      p++;
	//nothing of use
    if (*p == '\0')
      return n;
	//save the string
    strings[n++] = p;
	//find the next field
    while (*p != ',' && *p != '\0')
      p++;
	//nothing else of use or too many fields
    if (*p == '\0' || n >= size)
      return n;
	//split the field
    *p++ = '\0';
  }
}

void CopyNoDecimal(char *s, char *d) {
	while (*s != '.')
		*d++ = *s++;	//copy up to decimal point
	*d++ = *++s;		//copy 1 digit past decimal
	*d = '\0';			//terminate string
}

void GeoCopy(char *s, char *d, unsigned char value) {
	int i;

	i = 0;
	if (value == LONGITUDE)  {
		//skip first numeral
		i++;
		s++;
	}
	//copy all numerals, skipping negative sign/decimal point
	do {
		if ((*s >= '0') && (*s <= '9')) {
			*d++ = *s;
			i++;
		}
	} while (*s++ != '\0');
	//pad end with zeros
	if (i < 7)
		while (i++ < 7)
			*d++ = '0';
	//truncate last numeral and null terminate
	*--d = '\0';
}

void Run(void) {
	//string space for each gps rmc sentence/fields
	char GPSString[GPS_STRING_LENGTH];							
	char *GPSTokens[RMC_FIELDS];
	//instantaneous GPS speed
	unsigned short gps_speed;
	//lap counters
	unsigned short lap, counter;
	//coordinates of current & previous GPS location
	line track;
	//current lap average speed
	float avgerage_speed;

	//init various stuff
	current.pSpeed = speed_array_1;
	best.pSpeed = speed_array_2;
	avgerage_speed = 0.0;
	best_average = 0.0;
	counter = 0;
	lap = 0;

	//
	//simulate setting startline
	//
	mode = RUNNING_MODE;
	//note maximum uint32_t value = 4294967295
	start.x = 4559400;
	start.y = 2269100;  //truncated
	//heading while crossing start/finish
	start_heading = 290;
	//
	//end simulate setting startline
	//

	//define startline
	StartLine((float)start.x, (float)start.y, (float)start_heading);
	track.p1.x = start.x;
	track.p1.y = start.y;

	//main gps string processing loop
	while (fgets(GPSString, GPS_STRING_LENGTH, file) != NULL) { //grab a gps string
		char s[12];

		if (mode != RUNNING_MODE)
			return;

		//parse gps string
		ParseRMC(GPSString, GPSTokens);

		//gps instantaneous velocity
		CopyNoDecimal(GPSTokens[RMC_SPEED], s);
		//converted to integer*10
		gps_speed = atoi(s);
		//stationary?
		if (gps_speed < 20) {
			printf("mode: STOP speed <20\n");
			mode = STOPPED_MODE;
			break;
		}

		//save instaneous velocity of current lap
		*(current.pSpeed + counter) = gps_speed;
		//running total of lap speed
		current.total += gps_speed;
		//running total of fastest lap speed
		best.total += *(best.pSpeed + counter);

		//current lap average speed
		avgerage_speed = (float)current.total/(counter + 1);

		//x position
		GeoCopy(GPSTokens[RMC_LATITUDE], s, LATITUDE);
		track.p2.x = atol(s);
		//y position
		GeoCopy(GPSTokens[RMC_LONGITUDE], s, LONGITUDE);
		track.p2.y = atol(s);

		long heading;
		heading = atol(GPSTokens[RMC_TRACK]);

		//heading sanity check
		if ( (counter > 3) && (abs(start_heading - heading) < 30) ) {
			
			//crossed start/finish line?
			if (LineIntersection(track)) {

				//we crossed the start/finish line, first lap?
				if (lap > 0) {
					printf("%2d: (%d) %3.1f %3.1f", lap, counter, current_avg/10, best_average/10);
	
					if (avgerage_speed > best_average) {
						//a new fast lap
						printf(" <New Fast Lap\n");
						best_average = avgerage_speed;
						//swap speed arrays
						if (current.pSpeed == speed_array_1) {
							current.pSpeed = speed_array_2;
							best.pSpeed = speed_array_1;
						} else {
							current.pSpeed = speed_array_1;
							best.pSpeed = speed_array_2;
						}
					} else
						putchar('\n');
				}

				//reset and await next gps udate
				current.total = 0;
				best.total = 0;
				lap++;
				counter = 0;
			}
		}

		//
		//display lap time prediction here…
		//

		//prepare for next iteration
		track.p1.x = track.p2.x;
		track.p1.y = track.p2.y;
		//increment gps sentence counter
		counter++;
		//exceeding array size?
		if (counter >= MAX_TIME) {
			printf("mode: STOP Exceeded MAX_TIME\n");
			mode = STOPPED_MODE;
			counter = 0;
			current.total = 0;
			best.total = 0;
		}
	}
}


int main(void) {
  if (fopen_s(&file, filePath, "r")) {		//attempt to open gps data file
    printf("Error opening file.\n");
    return 0;
  }

  printf("Waiting for keypress.\n");
  while (1)
    if (getchar())
      break;
  Run();

  fclose(file);
  getchar();		//wait before closing...
  return 0;
}
Advertisements

About Jim Eli

µC experimenter
This entry was posted in Uncategorized and tagged , , , , , , , , . Bookmark the permalink.

10 Responses to Predicitive GPS Lap Timer (PC Prototype Version)

  1. Ian Cope says:

    This looks fantastic, especially as it’s done without the complex floating point maths.
    However I do have one question. The StartLine method uses the global start_heading variable, which is not set anywhere. How do you calculate this value?

    Thanks
    Ian

  2. Jim Eli says:

    Ian,
    start_heading is the GPS track (ok, not really the heading…) pulled from the RMC sentence. I accidentally posted an incomplete version of the code. I’ve updated it.

  3. Ian Cope says:

    Jim,
    Thanks for the update. I’m still going through this, but think that I’ve found an issue that I’d like to check with you.
    The the StartLine Method I think that the calculation of m is wrong. Shouldn’t it be m = (sy – ty)/(sx – tx); ? Just wanted to check that I understand this correctly.

    Thanks
    Ian

    • Jim Eli says:

      Ian,
      Thanks for pointing out the error. A few bugs migrated into the code because at the time I was writing this I was switching between MS Visual C++ Express and Pelles C. I started the project in MS and ended in Pelles. However, this bug escaped me, and it seems like you have a good grasp on it.
      Jim

      P.S. Code has been updated.

  4. Ian Cope says:

    Hi,
    I’m struggling to understand what your doing with the LineIntersection method. My best guess is checking for intersections from various positions, but I can’t work it out totally. Any chance you could post a description of the theory please?

    Also in your testing how accurate/reliable have you find the heading field within the RMC sentence? I’ve found it to be quite un-reliable so I’m having to use the history of the RMC data at the start finish point to give me 2 points on the line, what I can then calculate the m and b values for,

    Any help would be appreciated

    Thanks
    Ian

    • Jim Eli says:

      LineIntersection returns TRUE if our “track” (a line between previous position & current position) intersects the starting line. In the current implementation, heading is only used to construct the perpendicular starting line (perpendicular to the track). However, the heading should be confirmed (within +/- 30 degrees) during the LineInersection check for additional validity (to eliminate the false indication of startline crossing when travelling in the opposite direction).

      GPS heading is a calculated value derived from GPS motion over time. Therefore it is subject to erratic behavior. I intend to implement a simple moving average (SMA) to calm it down somewhat.

  5. Toby says:

    This looks great! Around line 330 you have a comment about displaying lap time prediction, can you give any hints, clues as to how this is done given the current state of data ? For example is it possible to calculate the instantaneous time gained or lost on the current lap compared to the best lap ?

  6. Toby says:

    Hi Jim, just had a look at the new code and seen how you use the delta between instantaneous current average speed and instantaneous best average speed (based on GPS ticks) to light up the red / green LED’s.

    This got me thinking what would happen if I was to take different lines around a hairpin.The first time (best time) I hug the inside line at slow speed which takes say 20 gps “ticks” from entry to exit, second time around, I take hug outside at high speed which takes say 30 gps “ticks”. Note that both 20 and 30 ticks get me to the same point on the circuit.

    Would I be right in thinking that the lights would show that I had gone quicker on the 2nd run due to having the higher instant average speed, even though the first “inner” line was in fact quicker by 10 ticks ? Or am I getting confused in thinking that distance has to be considered somehow ?

    • Jim Eli says:

      You are correct, the lights would indicate a temporary improvement in average speed, however as the lap progressed the improvement would either remain or disappear. The situation you propose is the classic tradeoff a driver must evaluate in pursuit of a faster lap time.

      But also consider if driving “close” to the proper line and near the “limits” of the vehicle a large delta in speed vs. distance is unlikely. Regardless, at the finish line, a higher average speed will always equal a lower lap time, and incorporating this calculation into the algorithm is planned. Further enhancements to eliminate similar “features” in the program might add significantly to the complexity and prohibit using a simple uC.

      Thanks for the comments.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s