sketch_mar05a.cpp

#include <SoftwareSerial.h>

const int rxPin = 9;
const int txPin = 12; // unused

const int actLed = 10;
const int fixLed = 8;

SoftwareSerial gpsSerial(rxPin, txPin);

/* sum of ASCII codepoints */
#define GGA 207
#define GLL 223
#define GSA 219
#define GSV 240
#define RMC 226
#define VTG 241

/* GPS reception status */
char cur_nmea_msg[82];
bool currently_receiving_msg = false;
int cur_recv_idx = 0;

int utc_hours = 0;
int utc_minutes = 0;
float utc_seconds = 0;

float latitude = 0;
bool latitude_north = false;

float longitude = 0;
bool longitude_east = false;

float altitude = 0;

int n_satellites = 0;
bool gps_fix_valid = false;

unsigned long last_msg_received = -1;

char* nmea_value(char *cur) {
  // skip past initial commas and spaces to the next data element
  while (*cur == ',' || *cur == ' ') {
    cur++;
  }

  return cur;
}

char* next_field(char *cur) { 
  // Find the next comma or CR
  while (*cur != ',' && *cur != 0x0D) {
    cur++;
  }

  // Find the start of the next data value
  while (*cur != ' ' && *cur != ',') {
    cur++;
  }

  return cur+1;
}

float read_decimal_number(char* cur) {
  int whole = 0;
  int frac = 0;
  int n_frac_digits = 0;
  float out = 0;

  while (*cur != '.' && isDigit(*cur)) {
    whole = (whole * 10) + (*cur - '0');
    cur++;
  }

  if (*cur == 0x0D || *cur == 0x0A) {
    return (float)whole;
  }

  cur++;
  
  while (isDigit(*cur)) {
    frac = (frac * 10) + (*cur - '0');
    cur++;
  }

  out = (float)whole;

  if (frac > 0) {
    out += (float)frac / (float)n_frac_digits;
  }
  
  return out;
}

int read_digits(char *cur, int n) {
  int out = 0;

  while(n-- > 0 && isDigit(*cur)) {
    out = (out * 10) + (*cur++ - '0');
  }

  return out;
}

void print_gps_data() {
  Serial.print("UTC time: ");
  Serial.print(utc_hours);
  Serial.print(":");
  Serial.print(utc_minutes);
  Serial.print(":");
  Serial.print(utc_seconds);
  Serial.print("\n");

  Serial.print("Latitude: ");
  Serial.print(latitude);
  Serial.println(latitude_north ? " N" : " S");

  Serial.print("Longitude: ");
  Serial.print(longitude);
  Serial.println(longitude_east ? " E" : " W");

  Serial.print("Altitude: ");
  Serial.print(altitude);
  Serial.println("m MSL");
  
  Serial.print("Satellites: ");
  Serial.println(n_satellites);

  Serial.print("Data Valid: ");
  Serial.println(gps_fix_valid ? "Yes" : "No");
}

void handle_gga_msg() {
  /*
   * message data in order:
   * 1. UTC time (hhmmss.sss)
   * 2. Latitude (ddmm.mmmm)
   * 3. North / South indicator (N/S)
   * 4. Longitude (dddmm.mmmm)
   * 5. East / West indicator (E/W)
   * 6. Position fix indicator
   * 7. # of satellites used
   * 8. Horizontal Dilution of Precision
   * 9. MSL altitude (m)
   */

  // read in UTC time:
  char* cur_value = next_field(cur_nmea_msg);
  utc_hours = read_digits(cur_value, 2);
  utc_minutes = read_digits(cur_value+2, 2);
  utc_seconds = read_decimal_number(cur_value+4);

  // Read latitude
  cur_value = next_field(cur_value);  
  latitude = read_decimal_number(cur_value) / 100.0f;

  // Read N/S indicator
  cur_value = next_field(cur_value);
  latitude_north = (*cur_value == 'N');

  // Read longitude
  cur_value = next_field(cur_value);
  longitude = read_decimal_number(cur_value) / 1000.0f;

  // Read E/W indicator
  cur_value = next_field(cur_value);
  longitude_east = (*cur_value == 'E');

  // read position fix validity indicator
  cur_value = next_field(cur_value);
  int fix_status = read_digits(cur_value, 1);
  gps_fix_valid = (fix_status > 0);

  // read number of satellites used
  cur_value = next_field(cur_value);
  n_satellites = (int)read_decimal_number(cur_value);

  // skip HDOP
  cur_value = next_field(cur_value);

  // read MSL altitude
  cur_value = next_field(cur_value);
  altitude = read_decimal_number(cur_value);

  Serial.println("\nReceived GGA message.");
  print_gps_data();
}

void handle_gll_msg() {
  /*
   * message data in order:
   * 1. UTC time (hhmmss.sss)
   * 2. Latitude (ddmm.mmmm)
   * 3. North / South indicator (N/S)
   * 4. Longitude (dddmm.mmmm)
   * 5. East / West indicator (E/W)
   * 6. Position fix indicator
   * 7. # of satellites used
   * 8. Horizontal Dilution of Precision
   * 9. MSL altitude (m)
   */

  // Read latitude
  char* cur_value = next_field(cur_nmea_msg);
  latitude = read_decimal_number(cur_value) / 100.0f;

  // Read N/S indicator
  cur_value = next_field(cur_value);
  latitude_north = (*cur_value == 'N');

  // Read longitude
  cur_value = next_field(cur_value);
  longitude = read_decimal_number(cur_value) / 1000.0f;

  // Read E/W indicator
  cur_value = next_field(cur_value);
  longitude_east = (*cur_value == 'E');

  // read UTC time
  cur_value = next_field(cur_value);
  utc_hours = read_digits(cur_value, 2);
  utc_minutes = read_digits(cur_value+2, 2);
  utc_seconds = read_decimal_number(cur_value+4);

  // read fix status
  cur_value = next_field(cur_value);
  gps_fix_valid = (*cur_value == 'A');
  
  //Serial.println("\nReceived GLL message.");
  //print_gps_data();
}

void handle_nmea_msg() {
  int ident = cur_nmea_msg[3] + cur_nmea_msg[4] + cur_nmea_msg[5];

  if (ident == GGA) {
    last_msg_received = millis();
    return handle_gga_msg();
  } else if (ident == GLL) {
    last_msg_received = millis();
    return handle_gll_msg();
  }
}

void setup() {
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);

  pinMode(actLed, OUTPUT);
  pinMode(fixLed, OUTPUT);
  
  gpsSerial.begin(9600);
  Serial.begin(9600);
}

void loop() {  
  if (gpsSerial.available() > 0) {
    int inByte = gpsSerial.read();
    if (inByte == '$') {
      currently_receiving_msg = true;
      cur_recv_idx = 0;
      for (size_t i=0;i<82;i++) {
        cur_nmea_msg[i] = 0;
      }
    }

    if (currently_receiving_msg) {
      if(inByte == 0x0D || inByte == 0x0A) {
        currently_receiving_msg = false;
        handle_nmea_msg();
      } else {
        cur_nmea_msg[cur_recv_idx++] = inByte;
      }
    }
  }

  if ((millis() - last_msg_received) < 25) {
    digitalWrite(actLed, HIGH);
  } else {
    digitalWrite(actLed, LOW);
  }

  if (gps_fix_valid) {
    digitalWrite(fixLed, HIGH);
  } else {
    digitalWrite(fixLed, LOW);
  }
}