GPS Joystick

#include <Adafruit_GPS.h>
#include <SoftwareSerial.h>

SoftwareSerial gpsSerial(12, 11);

Adafruit_GPS GPS(&gpsSerial);

#define JOY_X A4
#define JOY_Y A5
#define MOVE_INCR 0.001 // how much to move per tick

#define VLOGIC 5.0

float latoffset = 0;
float lonoffset = 0;

// Set GPSECHO to 'false' to turn off echoing the GPS data to the Serial console
// Set to 'true' if you want to debug and listen to the raw GPS sentences. 
#define GPSECHO  false

// this keeps track of whether we're using the interrupt
// off by default!
boolean usingInterrupt = false;
void useInterrupt(boolean); // Func prototype keeps Arduino 0023 happy

void setup()  
{
  Serial.begin(9600);
  Serial.println("Adafruit GPS library basic test!");

  // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
  GPS.begin(9600);
  
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_ALLDATA);
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);   // 1 Hz update rate
  GPS.sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
  GPS.sendCommand(PGCMD_ANTENNA);

  // the nice thing about this code is you can have a timer0 interrupt go off
  // every 1 millisecond, and read data from the GPS for you. that makes the
  // loop code a heck of a lot easier!
  useInterrupt(true);

  delay(1000);
}

volatile uint8_t joystick_tick = 0;
volatile boolean timeToReadJoystick = false;

// Interrupt is called once a millisecond, looks for any new GPS data, and stores it
SIGNAL(TIMER0_COMPA_vect) {
  char c = GPS.read();
  // if you want to debug, this is a good time to do it!
#ifdef UDR0
  if (GPSECHO)
    if (c) UDR0 = c;  
    // writing direct to UDR0 is much much faster than Serial.print 
    // but only one character can be written at a time. 
#endif

  joystick_tick++;
  if (joystick_tick == 0) { //once evey 256ms
    timeToReadJoystick = true;
  }
}

void useInterrupt(boolean v) {
  if (v) {
    // Timer0 is already used for millis() - we'll just interrupt somewhere
    // in the middle and call the "Compare A" function above
    OCR0A = 0xAF;
    TIMSK0 |= _BV(OCIE0A);
    usingInterrupt = true;
  } else {
    // do not call the interrupt function COMPA anymore
    TIMSK0 &= ~_BV(OCIE0A);
    usingInterrupt = false;
  }
}

uint32_t timer = millis();
void loop()                     // run over and over again
{
  // in case you are not using the interrupt above, you'll
  // need to 'hand query' the GPS, not suggested :(
  if (! usingInterrupt) {
    // read data from the GPS in the 'main loop'
    char c = GPS.read();
    // if you want to debug, this is a good time to do it!
    if (GPSECHO)
      if (c) Serial.print(c);
  }
  
  // if a sentence is received, we can check the checksum, parse it...
  if (GPS.newNMEAreceived()) {
    // a tricky thing here is if we print the NMEA sentence, or data
    // we end up not listening and catching other sentences! 
    // so be very wary if using OUTPUT_ALLDATA and trytng to print out data
    //Serial.println(GPS.lastNMEA());   // this also sets the newNMEAreceived() flag to false
  
    char *nmea = GPS.lastNMEA();
    char *p = nmea;
    if (strstr(p, "$GPRMC")) {
      //Serial.print("\n### "); Serial.print(nmea);
      // RMC has lat/lon
      p = strchr(p, ',')+1;  // skip to time
      if (!p) return;
      p = strchr(p, ',')+1;  // skip to fix
      if (!p) return;  
      p = strchr(p, ',')+1;  // skip to lat
      if (!p) return;
      
      replaceAndPrintNMEA(nmea, p);

    } else if (strstr(p, "$GPGGA")) {
      p = strchr(p, ',')+1;  // skip to time
      if (!p) return;
      p = strchr(p, ',')+1;  // skip to lat
      if (!p) return;
      
      replaceAndPrintNMEA(nmea, p);
    } else if (strstr(p, "$GPGLL")) {
      p = strchr(p, ',')+1;  // skip to lat
      if (!p) return;
      
      replaceAndPrintNMEA(nmea, p);    
    } else {
      Serial.print(nmea);
    }
  }

  // every N ticks, we'll check the joystick
  if (timeToReadJoystick) {
    float jx = analogRead(JOY_X);
    float jy = analogRead(JOY_Y);
    jx *= VLOGIC;  jx /= 1024;
    jy *= VLOGIC;  jy /= 1024;
    if (jx > VLOGIC * 0.7) {
      latoffset += MOVE_INCR;
    }
    if (jx < VLOGIC * 0.3) {
      latoffset -= MOVE_INCR ;
    }

    if (jy > VLOGIC * 0.7) {
      lonoffset += MOVE_INCR;
    }
    if (jy < VLOGIC * 0.3) {
      lonoffset -= MOVE_INCR;
    }

    //Serial.print("\njoyX "); Serial.print(jx); Serial.print("\t"); Serial.println(latoffset,4);
    //Serial.print("joyY "); Serial.print(jy); Serial.print("\t"); Serial.println(lonoffset,4);

    // reset the flag
    timeToReadJoystick = false;
  }
}

void replaceAndPrintNMEA(char *nmea, char *p) {
       float lat = atof(p);
      // zero out this part
      p[0] = 0;
      Serial.print(nmea);
      
      // print new lat
      Serial.print(lat + latoffset, 4);
      p = strchr(p+1, ',')+1;  // skip direction
      if (!p) return;
      nmea = p-1; // start next section of nmea sentence
      p = strchr(p, ',')+1;  // skip to long
      if (!p) return;
      float lon = atof(p);

      // zero out this part
      p[0] = 0;
      Serial.print(nmea);

      // print new longitude
      Serial.print(lon + lonoffset, 4);
      p = strchr(p+1, ',');  // skip to rest
      Serial.print(p);
}