DW finite state machine GPS logger

FSM_dw_gps_1.ino

/*GPS logger for Ublox GPS module outputting NMEA data through Uart
by DWhacks 

For Teensy

Help from Feyr and tgmobile on ##foulab IRC


using finite state machine http://playground.arduino.cc/code/FiniteStateMachine
*/



//includes
#include <SD.h>
#include "FiniteStateMachine.h"
 //http://playground.arduino.cc/code/FiniteStateMachine


HardwareSerial Uart = HardwareSerial(); //use hardware serial on the teensy

//Defines
#define fixLed 16 //LED to indicate we have a fix
#define writeLed 15 //LED to indicate write to SD card

const byte NUMBER_OF_SELECATBLE_STATES = 3;  //how many states

//initialize states
//State SEARCH = State(searchGps); 
State buff = State(bufferGps);
State getFix = State(gpsFix);
State sdLog = State(logGps);

FSM stateMachine = FSM(buff); //initiallize state machine, start in state FIX

const int chipSelect = 0; //For SDcard

#define BUFFSIZE 90 //90
char buffer[BUFFSIZE];
uint8_t bufferidx = 0;
bool fix = false; // current fix data
bool gotGPRMC;    //true if current data is a GPRMC strinng
uint8_t i;
File logfile;
char c;
uint8_t sum; 
int fsm_state;

// read a Hex value and return the decimal equivalent
uint8_t parseHex(char c) {
  if (c < '0')
    return 0;
  if (c <= '9')
    return c - '0';
  if (c < 'A')
    return 0;
  if (c <= 'F')
    return (c - 'A')+10;
}

// blink out an error code
void error(uint8_t errno) {
  while(1) {
    for (i=0; i<errno; i++) {
      digitalWrite(writeLed, HIGH);
      delay(100);
      digitalWrite(writeLed, LOW);
      delay(100);
    }
    for (; i<10; i++) {
      delay(200);
    }
  }
}



 void setup() 
{
  pinMode(fixLed, OUTPUT);
  pinMode(writeLed, OUTPUT); 
  pinMode(0, OUTPUT); 

  Serial.begin(115200);//start serial
    Uart.begin(115200);//start GPS

    Serial.println("\r\nStarting...");

    // see if the card is present and can be initialized:
  if (!SD.begin(chipSelect)) {
    Serial.println("Card init. failed!");
    error(1);
  }

  strcpy(buffer, "GPSLOG00.TXT");
  for (i = 0; i < 100; i++) {
    buffer[6] = '0' + i/10;
    buffer[7] = '0' + i%10;
    // create if does not exist, do not open existing, write, sync after write
    if (! SD.exists(buffer)) {
      break;
    }
  }

  logfile = SD.open(buffer, FILE_WRITE);
  if( ! logfile ) {
    Serial.print("Couldnt create"); 
    Serial.println(buffer);
    error(3);
  }
  Serial.print("Starting to log on ..."); 
  Serial.println(buffer);
}


  void loop() 
 {
  switch (fsm_state){
      case 0: stateMachine.transitionTo(buff); break; //default state
      case 1: stateMachine.transitionTo(getFix); break; //
      case 2: stateMachine.transitionTo(sdLog); break; //
  }
 
  stateMachine.update();
 }

//Functions

void bufferGps(){
//buffer the gps serial data
// read one 'line'
  if (Uart.available()) {
    c = Uart.read();

    if (bufferidx == 0) {
      while (c != '$')
        c = Uart.read(); // wait till we get a $
    }
    buffer[bufferidx] = c;

    if (c == '\n') {

      buffer[bufferidx+1] = 0; // terminate it

      if (buffer[bufferidx-4] != '*') {
        // no checksum?
        Serial.print('*');
        bufferidx = 0;
        return;
      }
      // get checksum
      sum = parseHex(buffer[bufferidx-3]) * 16;
      sum += parseHex(buffer[bufferidx-2]);

      // check checksum
      for (i=1; i < (bufferidx-4); i++) {
        sum ^= buffer[i];
      }
      if (sum != 0) {
        //putstring_nl("Cxsum mismatch");
        Serial.print('~');
        bufferidx = 0;
        return;
      }
    } 
  }
  stateMachine.transitionTo(getFix);
}

void gpsFix(){
//check for fix
gotGPRMC = strstr(buffer, "GPRMC"); //GPRMC
      if (gotGPRMC)
      {
        // find out if we got a fix
        char *p = buffer;
        p = strchr(p, ',')+1;
        p = strchr(p, ',')+1;       // skip to 3rd item
        
        if (p[0] == 'V') {
          digitalWrite(fixLed, LOW);
          fix = false;
        } else {
          digitalWrite(fixLed, HIGH);  //Turn on LED1 when we have a fix
          fix = true;
          stateMachine.transitionTo(sdLog);
        }
      }      
      if (!fix) {
          Serial.print('_');
          bufferidx = 0;
          stateMachine.immediateTransitionTo(buff);
        }
}        



void logGps(){
//write to sd card
  gotGPRMC = strstr(buffer, "GPRMC"); //GPRMC
  if (gotGPRMC)      //If we have a GPRMC string
        {
          
          // Bill Greiman - need to write bufferidx + 1 bytes to getCR/LF 
          bufferidx++;
          delay(250); //Origonal 10000
          digitalWrite(writeLed, HIGH);      // Turn on LED 2 (indicates write to SD)

          logfile.write((uint8_t *) buffer, bufferidx);    //write the string to the SD file
          logfile.flush();
                  

          digitalWrite(writeLed, LOW);    //turn off LED2 (write to SD is finished)
          bufferidx = 0;    //reset buffer pointer
          stateMachine.transitionTo(buff);

        }
  else {
    stateMachine.transitionTo(buff);
  }
}