GPS Predictive Lap Timer Update

Arduino Mega

The picture above shows the Arduino Mega Predictive Lap Timer being fed GPS RMC-Sentences from an Ardweeny pretending to be a GPS receiver. The Ardweeny is running my Scripted GPS Simulator code and is connected to a 16Mbit AT45DB161D DataFlash memory chip. This is very early beta, but here’s the Arduino Mega GPS predictive lap timer software to fiddle with:

/* 
  GPS Predictive Lap Timer
  Copyright 2012, 2013, all rights reserved.
  James M. Eli
  4/3/2013
  bill of materials:
    (1) arduino mega1280@16MHz/5v
    (1) ls20031 5Hz gps
    (1) pololu voltage regulator
    (1) sparkfun logic level converter
    (2) push-button switches
    (1) power source
  mtk gps config:
    $PMTK251,19200*22\r\n                             //baud rate
    $PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n //rmc sentence only
    $PMTK220,200*2C\r\n                               //5Hz update
    $PMTK313,1*2E\r\n                                 //enable waas
    $PMTK301,2*2E\r\n                                 //enable waas as dgps source
    $PMTK001,314,33*36<CR><LF>                        //output msg with flag: 0=invalid packet/1=unsupported packet/2=action failed/3=action succeded
*/
#include <avr/io.h> 
#include <math.h>
 
//
//definitions
//
 
//enable debug output
#ifdef DEBUG
  #undef DEBUG
#endif
#define DEBUG 1
 
#define TRUE                          1
#define FALSE                         0
#define MIN(x, y)                     (((x) < (y)) ? (x) : (y))
#define MAX(x, y)                     (((x) > (y)) ? (x) : (y))
#define DEGTORAD(deg)                 (deg*57.29577995)
 
//modes of operation
#define GPS_INIT_MODE                 (uint8_t)0x01
#define PRESTART_MODE                 (uint8_t)0x02
#define RUNNING_MODE                  (uint8_t)0x04
#define STOPPED_MODE                  (uint8_t)0x08
 
//below this speed gps assumed stationary (scaled *10)
#define MINIMUM_GPS_SPEED             20
 
//5Hz*60seconds*5minutes
#define MAX_TIME                      1500 //(5*60*5) 5Hz*60seconds*5minutes
 
//uart 
#define BAUDRATE                      19200
#define BAUD_PRESCALE                 (((F_CPU/(USART_BAUDRATE*16UL))) - 1)
 
//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)
 
//gps string buufer size
#define MAXIMUM_GPS_SENTENCE_SIZE     128
 
//GeoCopy function uses these to convert lat/long to x/y coords 
#define LATITUDE                      0x01
#define LONGITUDE                     0x02
 
//LED pins PORTB0-5 and POTRTD7
//PORTB = RRRGGGxx/321123xx and neutral LED is on PORTD
#define LED_RED3                      8  //PB0
#define LED_RED2                      9  //PB1
#define LED_RED1                      10 //PB2
#define LED_NEUTRAL                   7  //PD7
#define LED_GREEN1                    11 //PB3
#define LED_GREEN2                    12 //PB4
#define LED_GREEN3                    13 //PB5
#define LED_MASK                      0b00000011
 
//debug pin for testing execuition speed
#define DEBUG_PIN                     5
#define DEBUG_PORT                    PORTC
 
//definitions for the startline
static const float LINE_WIDTH = 50.0f;
static const float LINE_WIDTH_2 = 25.0f;
static const float PROJECTION_DISTANCE = 100.0f;
 
//
//declare globals 
//
 
//mode of operation
uint8_t mode;
 
//gps nmea rmc string
char gps_string[MAXIMUM_GPS_SENTENCE_SIZE];
//12 items per line
char *rmc_tokens[RMC_FIELDS];
 
//lap & gps sentence counters
uint16_t lap, counter;
 
//heading crossing start/finish
int16_t start_heading;
//simple moving average used with heading
int16_t sma;
 
//We use the following 2 arrays for storing the current and best lap gps instantaneous velocities.
uint16_t speed1[MAX_TIME];
uint16_t speed2[MAX_TIME];
 
//target adjusts sensitivity of LEDs via bit shifting 
int8_t min_led_threshold;  //range = 0x01 to 0x32 (6 steps)
 
//structure holding lap information
struct speed_ {
  uint32_t total;
  uint16_t *pSpeed;
} current_lap, best_lap;
 
//structure representing integer cartesian 2d point
struct ipoint_ {
  int32_t x;
  int32_t y;
} sf1, sf2,   //start/finish line endpoints
previous_pt,  //coordinates of previous GPS location
current_pt,   //coordinates of current GPS location
start_pt;     //coordinates of start/finish location
 
//structure representing integer cartesian 2d point
struct fpoint_t {
  float x;
  float y;
};
 
//
//math functions
//
 
//integer (long) square root function
uint32_t SquareRoot(long n) {
  long i, j, k;
  if (n == 0)
    return 0;
  k = 1;
  j = 0;
  do {
    i = j;
    j = k;
    k = (j + (n/j))>>1;
  } while (k != i);
  return(j);
}
 
//integer version of pythagorean equation
uint32_t Distance(int32_t x1, int32_t y1, int32_t x2, int32_t y2) {
  //special 0-0 case
  if (x1 == x2 && y1 == y2)
    return 0;
  return SquareRoot((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
}
 
//pythagorean equation
float fDistance(float x1, float y1, float x2, float y2) {
  return sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
}
 
//simple weighted average
int16_t SimpleMovingAverage(int16_t i) {
  sma *= 5;
  sma += i;
  sma /= 6;
  return sma;
}
 
//absolute values
int32_t Abs32(int32_t n) {
  return (n<0 ? -n : n);
}
 
int16_t Abs16(int16_t n) {
  return (n<0 ? -n : n);
}
 
//construct a startline
void StartLine(float sx, float sy, float hdg) {
  struct fpoint_t t; //projected track coordinates
  float m, b;        //slope & y-intercept
  //project racetrack along current heading
  t.x = sx + PROJECTION_DISTANCE*cos(DEGTORAD(hdg));
  t.y = sy + PROJECTION_DISTANCE*sin(DEGTORAD(hdg));
  
  //projected racetrack slope & y-intercept 
  m = (sy - t.y)/(sx - t.x);
  b = sy - (m*sx);
  
  //perpendicular (startline) slope & y-intercept
  m = (-1.0/m);
  b = sy - (m*sx);
  
  //define endpoints of perpendicular
  t.x = sx + LINE_WIDTH_2; //note tx used as a temporary value here
  sf1.y = (long)(m*t.x + b);
  sf1.x = (long)t.x;
  t.x -= LINE_WIDTH;
  sf2.y = (long)(m*t.x + b);
  sf2.x = (long)t.x;
}
 
//2d line-segment intersection test
unsigned char LineIntersection(long x1, long y1, long x2, long y2) {
  long z;
  int s1, s2, s3, s4;
 
  //quick rejection test (bounding box)
  if (!(MAX(sf1.x, sf2.x) >= MIN(x1, x2) && MAX(x1, x2) >= MIN(sf1.x, sf2.x) && 
 MAX(sf1.y, sf2.y) >= MIN(y1, y2) && MAX(y1, y2) >= MIN(sf1.y, sf2.y))) {
    return FALSE;
  }
 
  //straddle tests
  if ((z = ((x1 - sf1.x)*(sf2.y - sf1.y)) - ((y1 - sf1.y)*(sf2.x - sf1.x))) < 0)
    s1 = -1; //counterclockwise
  else if (z > 0)
    s1 = 1;  //clockwise
  else
    s1 = 0;  //collinear
  
  if ((z = ((x2 - sf1.x)*(sf2.y - sf1.y)) - ((y2 - sf1.y)*(sf2.x - sf1.x))) < 0)
    s2 = -1;
  else if (z > 0)
    s2 = 1;
  else
    s2 = 0;
 
  if ((z = ((sf1.x - x1)*(y2 - y1)) - ((sf1.y - y1)*(x2 - x1))) < 0)
    s3 = -1;
  else if (z > 0)
    s3 = 1;
  else
    s3 = 0;
  
  if ((z = ((sf2.x - x1)*(y2 - y1)) - ((sf2.y - y1)*(x2 - x1))) < 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;
}
 
//2d lines point of intersection
void IntersectionPoint(long x1, long y1, long x2, long y2, long *xi, long *yi) {
  long denom, numera; //numerb
  float mua; //mub;
  
  //no checks because it is assumed the lines intersect
  denom = (sf2.y - sf1.y)*(x2 - x1) - (sf2.x - sf1.x)*(y2 - y1);
  numera = (sf2.x - sf1.x)*(y1 - sf1.y) - (sf2.y - sf1.y)*(x1 - sf1.x);
  //numerb = (x2 - x1)*(y1 - sf1.y) - (y2 - y1)*(x1 - sf1.x);
  
  mua = numera/denom;
  //mub = numerb/denom;
  *xi = (long)x1 + mua*(x2 - x1);
  *yi = (long)y1 + mua*(y2 - y1);
}
 
//
//utility functions
//
 
//led_pin_bits are: 0bxxx00000 for SLOW LEDs and 0b000xxx00 for FAST LEDs
void ProcessLEDs(int16_t delta) {
  uint8_t i, init_led, led_pin_bits;
 
  led_pin_bits = 0;
  if (delta > 0)
    init_led = 0b00000100;
  else
    init_led = 0b00100000;
  delta = Abs16(delta);
  
  //slow side inital_led = 0b00100000 and fast side: 0b00000100
  for (i=0; i<3; i++) {
    if (delta >= min_led_threshold) {
      led_pin_bits <<= 1;        //shift LED up one
      led_pin_bits += init_led;  //turn on lower LED
    } else
      break;
    delta -= min_led_threshold;
  }
   //turn on LEDs
   PORTB |= led_pin_bits;
}
 
//turn all leds off
void inline LEDsOff(void) {
  PORTB &= LED_MASK;
  bitClear(PORTD, LED_NEUTRAL);
}
 
//string compare
uint8_t StrComp(char *s, char *t) {
  for (; *s == *t; s++, t++)
    if (*s == '\0')
      return 0;
  return (*s - *t);
}
 
//convert char to integer
int16_t Atoi(char s[]) {
  uint8_t i;
  int16_t n, sign;
 
  //skip white space
  for (i=0; s[i]== ' ' || s[i] == '\n' || s[i] == '\t'; i++)
    ;
  //sign
  sign = 1;
  if (s[i] == '+' || s[i] == '-')
    sign = (s[i++] == '+') ? 1 : -1;
  //convert
  for (n=0; s[i] >= '0' && s[i] <= '9'; i++)
    n = 10*n + s[i] - '0';
  
  return(sign*n);
}
 
//convert char to long
int32_t Atol(char s[]) {
  uint8_t i;
  int32_t n;
 
  //skip white space
  for (i=0; s[i]== ' ' || s[i] == '\n' || s[i] == '\t'; i++)
    ;
  //convert
  for (n=0; s[i] >= '0' && s[i] <= '9'; i++)
    n = 10*n + s[i] - '0';
  return(n);
}
 
//copy excluding decimal point
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
}
 
//special lat/long copy 
void GeoCopy(char *s, char *d, uint8_t 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';
}
 
//parse gps rmc string
uint8_t ParseRMC(char *line, char *strings[]) {
  char *p;
  uint8_t n;
 
  p = line;
  n = 0;
  for ( ; ; ) {
    while (*p == ',')                   //ditch leading commas
      p++;
    if (*p == '\0')                 //nothing of use
      return n;
    strings[n++] = p;                   //save the string
    while (*p != ',' && *p != '\0')     //find the next field
      p++;
    if (*p == '\0' || n >= RMC_FIELDS) //nothing else of use or too many fields
      return n;
    *p++ = '\0';                        //split the field
  }
}
 
//
//main program
//
 
//check gps fix validity
void GPSInit(void) {
  if (StrComp(rmc_tokens[RMC_STATUS], "A")) {
    mode = PRESTART_MODE;
    LEDsOff();
    bitSet(PORTB, LED_NEUTRAL);
  }
}
 
//check for startline definition
void PreStart(void) {
  char s[12];
    
  start_heading = SimpleMovingAverage(Atoi(rmc_tokens[RMC_TRACK]));
  //check for button press
  //if ( !(PINA & (1<<PORTA1)) ) { 
    //extract new geo position
    GeoCopy(rmc_tokens[RMC_LATITUDE], s, LATITUDE);
    start_pt.x = Atol(s);
    GeoCopy(rmc_tokens[RMC_LONGITUDE], s, LONGITUDE);
    start_pt.y = Atol(s);
    //
    //
    //establish start/finish line & heading for testing purposes
    start_pt.x = 4559510;   //debug start data
    start_pt.y = 2269500;   //debug start data
    //heading while crossing start/finish
    start_heading = 290; //debug start data
    //
    //
    //    
    //construct perpendicular
    StartLine((float)start_pt.x, (float)start_pt.y, (float)start_heading);
    previous_pt.x = start_pt.x;
    previous_pt.y= start_pt.y;
    mode = RUNNING_MODE;
    LEDsOff();
//}
}
 
//running laps
void Run(void) {
  //instantaneous GPS speed
  uint16_t current_speed;
  uint16_t previous_speed;
  //average lap speeds
  float average, best_average;
  int16_t heading;
  char s[12];
 
  //check gps position validity
  if (!StrComp(rmc_tokens[RMC_STATUS], "A")) {
    //fail gracefully here...
    ;
    //count invalid sentences, if exceeding x? then stop
    //mode= STOPPED_MODE;  
    //return;
  }
  
  //gps instantaneous velocity
  CopyNoDecimal(rmc_tokens[RMC_SPEED], s);
  //convert to integer and scale*10
  previous_speed = current_speed;
  current_speed = Atoi(s);
  if (current_speed < MINIMUM_GPS_SPEED) {
    mode = STOPPED_MODE;
    return;
  }
 
  //save velocity of current gps update
  *(current_lap.pSpeed + counter) = current_speed;
  //calculate running total of best lap speed
  best_lap.total += *(best_lap.pSpeed + counter);
  //calculate running total of current lap speed
  current_lap.total += current_speed;
  //compute average lap speeds
  average = (float)current_lap.total/(counter + 1);
  best_average = (float)best_lap.total/(counter + 1);
  
  /*
  ASSumption Alert: We are doing a small trick here by converting the gps position from
  latitude/longitude into cartesian x/y coordinates. This should work because our concern is 
  about relative not actual position/distance. This allows us to simplify our calculations.
  It should work...
    
  If you wanted to square up the lat/long to cartesian conversion, then you should correct the
  latitude like so:  x = latitude*Cosine(MeanLatitude)
  */  
  //extract new geo position
  GeoCopy(rmc_tokens[RMC_LATITUDE], s, LATITUDE);
  current_pt.x = Atol(s);
  GeoCopy(rmc_tokens[RMC_LONGITUDE], s, LONGITUDE);
  current_pt.y = Atol(s);
 
  //correct heading for crossing start/finish line?
  heading = Abs16(start_heading - SimpleMovingAverage(Atoi(rmc_tokens[RMC_TRACK])));
  if (counter > 3 && heading < 30) { 
    
    //crossed start/finish line?
    if (LineIntersection(previous_pt.x, previous_pt.y, current_pt.x, current_pt.y)) {
      float d1, d2, dt; //time_frac;
      struct ipoint_ int_pt; //coords of start/finish line & track intersection point
 
      //reset to await next gps udate
      current_lap.total = 0;
      best_lap.total = 0;
      counter = 0;
      
      //calculate portion of current segment allocated to current lap
      IntersectionPoint(previous_pt.x, previous_pt.y, current_pt.x, current_pt.y, &int_pt.x, &int_pt.y);
      //overall length of segment
      d1 = fDistance((float)previous_pt.x, (float)previous_pt.y, (float)current_pt.x, (float)current_pt.y);
      //length from start line intersection point to current point
      d2 = fDistance((float)int_pt.x, (float)int_pt.y, (float)current_pt.x, (float)current_pt.y);
      dt = d2/d1;
      //calc time allocated to current lap
      //time_frac = dt*0.2;
 
      //next lap fraction
 
      //next_time_frac = 0.2 – time_frac;


 
      //do we have a lap to compare against?
 
      if (lap > 0) {
        //have we completed a faster lap?
        if (average > best_average) { 
          best_average = average;
          //swap speed arrays
          if (current_lap.pSpeed == speed1) {
            current_lap.pSpeed = speed2;
            best_lap.pSpeed = speed1;
          } else {
            current_lap.pSpeed = speed1;
            best_lap.pSpeed = speed2;
          }
        }
      }
 
      //calc speed and save velocity of current gps update
      *current_lap.pSpeed = (uint16_t)(previous_speed + (int16_t)(dt*(current_speed - previous_speed)));
      //calculate running total of current lap speed
      current_lap.total = current_speed;
      best_lap.total = *(best_lap.pSpeed);
      //new lap complete
      lap++;
    }
  }
 
  //handle LEDs here
  if (lap > 1) {
    int16_t delta;
    
    delta = (int16_t)(average - best_average);
    LEDsOff();
    if (Abs16(delta) < min_led_threshold)
      bitSet(PORTD, LED_NEUTRAL);  //display neutral led
    else 
      ProcessLEDs(delta);          //display red or green leds
  }
  
  //prepare for next iteration
  previous_pt.x = current_pt.x;
  previous_pt.y = current_pt.y;
  //increment gps sentence counter
  counter++;                              

  //check exceeding maximum array size?
  if (counter >= MAX_TIME) {
    mode = STOPPED_MODE;
    counter = 0;
    current_lap.total = 0;
    best_lap.total = 0;
  }
}
 
void setup(void) { 
  //configure individual pins
  //pinMode(LED_NEUTRAL, OUTPUT);
  DDRB = DDRB | 0b1111100;  //set data direction for led pins on port B
  DDRD |= (1<<PORTD7);           //set neutral led pin
  DDRA |= (1<<PORTA1) | (1<<PORTA2);    //set BUTTON pins as input
  PORTA |= (1<<PORTA2) | (1<<PORTA2);   //turn pullup resistor on BUTTON pin
  DDRD |= (1<<PORTD2);           //set RXD1 pin as input
  PORTD |= (1<<PORTD2);          //turn pullup resistor on
  LEDsOff();
  //initialize usart
  Serial.begin(BAUDRATE);
  //clean out usart buffer
  Serial.flush();
  //set program mode to undefined start/finish
  mode = GPS_INIT_MODE;
  //init various stuff
  current_lap.pSpeed = speed1;
  best_lap.pSpeed = speed2;
  best_lap.total = 0;
  current_lap.total = 0;
  counter = 0;
  lap = 0;
  sma = 0;
  min_led_threshold = 1;
  //config gps baud rate
  Serial.print("$PMTK251,19200*22\r\n");
  //rmc sentence only
  Serial.print("$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n");
  //5Hz update
  Serial.print("$PMTK220,200*2C\r\n");
  //end config gps
  
  //assume gps status no lock
  bitSet(PORTB, LED_RED3);
}
 
void loop(void) {
  static uint8_t gps_string_index;
 
  gps_string_index = 0;
  while(1) {
    char c;
 
#ifdef DEBUG
    //set pin for timing test
    bitSet(DEBUG_PORT, DEBUG_PIN);
#endif
 
    if (Serial.available()) {
      c = Serial.read();
 
      if (c != '\n' && gps_string_index < MAXIMUM_GPS_SENTENCE_SIZE) {
        gps_string[gps_string_index++] = c;
 
      } else {
        gps_string[gps_string_index] = '\0';
        gps_string_index = 0;
        //parse gps string
        ParseRMC(gps_string, rmc_tokens);  
        //process gps string
        switch (mode) {
          case PRESTART_MODE:
            PreStart();
            break;
          case RUNNING_MODE:
            Run();
            break;
          case STOPPED_MODE:
            LEDsOff();
          case GPS_INIT_MODE:
          default:
            GPSInit();
            break;
        }
 
        /*
        if ( !(PINA & (1<<PORTA2)) ) {
          //adjust min_led_threshold
          min_led_threshold <<= 1;
          if (min_led_threshold > 32)
            min_led_threshold = 1;
        }
        */ 

      }
    } else {
      //no serial data available ???
      ;
    }
 
#ifdef DEBUG
    //clear pin for timing test
    bitSet(DEBUG_PORT, DEBUG_PIN);
    //debug output gps string
    /*
    for (int i=0; i<strlen(gps_string); i++)
      Serial.print(gps_string[i]);
    */
#endif
  }
}
Advertisements

About Jim Eli

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

One Response to GPS Predictive Lap Timer Update

  1. gibata says:

    Hello I try the Laptimer on th Hockenheim circuit and compare the time with a transponder. I have to high difference between both sytems. I suppose that the StratLine function is the Problem. Please could you check what the Problem is ?

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