Detect GPS Crossing a Boundary Line v2.0

Finish line

Under testing I’m getting good results using the following code to determine if crossing the startline. Please note the code is missing some bound tests which would prevent division by zero, etc.

#define FALSE			0
#define TRUE			1
#define _PI				3.14159
#define MIN(x, y)		(((x) < (y)) ? (x) : (y))
#define MAX(x, y)		(((x) > (y)) ? (x) : (y))
#define DEGTORAD(deg)	(((deg)*2.0*_PI)/360.0)
#define RADTODEG(rad)	(((rad)*360.0)/(2.0*_PI))
#define LINE_WIDTH		100.0
#define LINE_WIDTH_2	(LINE_WIDTH/2.0)
#define PROJECTION_DISTANCE 100.0

//startline endpoints
unsigned long sf1x, sf1y, sf2x, sf2y;

//define startline
void StartLine(float sx, float sy) {
	float tx, ty; //projected track coordinates
	float m, b;   //slope & y-intercept
	float temp;
	
	//project racetrack along current heading
	tx = PROJECTION_DISTANCE*cos(DEGTORAD((float)StartHdg));
	ty = PROJECTION_DISTANCE*sin(DEGTORAD((float)StartHdg));
	//projected racetrack slope & y-intercept 
	m = (sy - ty)/(sx - tx); //note: error here in previously posted code
	b = sy - (m*sx);
	//perpendicular (startline) slope & y-intercept
	m = (-1.0/m);
	b = sy - (m*sx);
	//define endpoints of perpendicular
	temp = sx + LINE_WIDTH_2;
	sf1y = (unsigned long)(m*temp + b);
	sf1x = (unsigned long)temp;
	temp -= LINE_WIDTH;
	sf2y = (unsigned long)(m*temp + b);
	sf2x = (unsigned long)temp;
}

//2d line intersection
unsigned char LineIntersection(unsigned long x1, unsigned long y1, unsigned long x2, unsigned long y2) {
  long z1, z2, z3, z4;
  int s1, s2, s3, s4;

  //quick rejection test
  if (!(MAX(sf1x, sf2x) >= MIN(x1, x2) && MAX(x1, x2) >= MIN(sf1x, sf2x) && 
	    MAX(sf1y, sf2y) >= MIN(y1, y2) && MAX(y1, y2) >= MIN(sf1y, sf2y))) {
    return FALSE;
  }

  //straddle tests
  if ((z1 = ((x1 - sf1x)*(sf2y - sf1y)) - ((y1 - sf1y)*(sf2x - sf1x))) < 0)
    s1 = -1; //counterclockwise
  else if (z1 > 0)
    s1 = 1;  //clockwise
  else
    s1 = 0;  //collinear

  if ((z2 = ((x2 - sf1x)*(sf2y - sf1y)) - ((y2 - sf1y)*(sf2x - sf1x))) < 0)
    s2 = -1;
  else if (z2 > 0)
    s2 = 1;
  else
    s2 = 0;

  if ((z3 = ((sf1x - x1)*(y2 - y1)) - ((sf1y - y1)*(x2 - x1))) < 0)
    s3 = -1;
  else if (z3 > 0)
    s3 = 1;
  else
    s3 = 0;

  if ((z4 = ((sf2x - x1)*(y2 - y1)) - ((sf2y - y1)*(x2 - x1))) < 0)
    s4 = -1;
  else if (z4 > 0)
    s4 = 1;
  else
    s4 = 0;

  if ((s1*s2 <= 0) && (s3*s4 <= 0))
    return TRUE;

  //line segments do not intersect
  return FALSE;
}
Advertisements

About Jim Eli

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

10 Responses to Detect GPS Crossing a Boundary Line v2.0

  1. B says:

    exellent work !!! do you have any code updates since march ?

  2. Max says:

    Interesting, a couple of years ago I worked to develop an AVR-based GPS lap timer for myself. It was for motorcycle racing and I was tired of destroying expensive (~$400) units whenever I crashed. I got as far as developing the timing code and building a functioning prototype, but I ran into some problems with packaging (I’ve found that effective packaging is the most difficult aspect of embedded designs, particularly when space is at a premium) and shelved the project.

    My preferred start/finish detection algorithm was actually angle based. My start/finish line was defined as a single point to the side of the race track (anywhere from right on the edge of the track to 15 meters or so away), which allowed me to set the start/finish line without having to enter a hot track as a side benefit (I could also define the start/finish line while lapping, and the software would move the defined point 10 meters to the side for me).

    With every GPS fix, I would use a compact and fast threshold algorithm to detect whether I was actually nearing the start/finish line. Once the threshold was satisfied, with every fix I would calculate the bearing and distance to the start/finish point (a trig function, but one that ran pretty quickly on a 16 Mhz Atmega328p). If there was a fix where the start/finish point was exactly 90 degrees from me, I was done (but this was rare with 5 hertz sampling). Otherwise I looked for a series of two fixes where the bearing went from in front of me (between 0 and 90 or -90) to behind me (larger than 90 or -90). For each of these two fixes, I would use trig to calculate my distance to the virtual start/finish line (imagine a triangle, my location is one point, the start/finish point is the second point, and the location of my path crossing the start/finish line is the third point), and using my instantaneous speed at each of the two fixes, I would interpolate the time of the actual crossing of the start/finish line. I used an average the two interpolated figures to arrive at my lap time.

    In testing against an infra-red beam lap timer, this algorithm was always within 0.1 second, and usually +/- 0.05 seconds off. Good enough for me. (There were a few times where it was off by a few tenths, cases where the GPS location error became significant right as I was passing the start/finish…)

    In terms of correcting for GPS fix errors, remember that the instantaneous speed and heading provided by most modern GPS chips are extremely accurate because they use Doppler shift calculations rather than distance/time (so the calculations do not integrate the fix location error, which may be +/- a few meters even in ideal conditions). A stationary GPS unit will typically display a speed of up to 1 or 2 knots in any arbitrary direction — given that the earth rotates at around 700 knots, that’s an error of only 0.1-0.3%, or less than 1 km/h at speeds up to 300 km/h. Use this precision to your advantage (i.e. use speed and heading to correct location errors).

    Have fun, and please keep the blog updated — I’m interested to see what you come up with!

  3. Ben says:

    Great…my mail is kpbwong@gmail.com Can you send me a msg so I can show you what we have done so far

  4. Hi, what would be the variable StartHdg?

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