            //Advance to next way point
            mCurrentWayPoint++;
            lcd2.setCursor(0,3);
-           lcd2.print("**Waypoint Reached**");
+           lcd2.print(F("**Waypoint Reached**"));
          } else {
            //Navigate to the way point
            if(abs(compassHeading - heading) < 5){
              lcd2.setCursor(0,3);
-             lcd2.print("  Command: FORWARD");
+             lcd2.print(F("  Command: FORWARD"));
              forward(); 
            } else if (compassHeading > heading) {
              lcd2.setCursor(0,3);
-             lcd2.print("  Command: LEFT"); 
+             lcd2.print(F("  Command: LEFT")); 
              left();
            } else {
              lcd2.setCursor(0,3);
-             lcd2.print("  Command: RIGHT");
+             lcd2.print(F("  Command: RIGHT"));
              right(); 
            }
          }
          lcd2.setCursor(11,1);
-         lcd2.print("G:");
+         lcd2.print(F("G:"));
          lcd2.print(heading);
          lcd2.setCursor(0,2);
-         lcd2.print("Distance: ");
+         lcd2.print(F("Distance: "));
          lcd2.print(formatDistance(distance));
        break;
        case CMD_ADD_WAY_POINT:
          lcd.clear();
          lcd.setCursor(0,0);
-         lcd.print("     Adding");
+         lcd.print(F("     Adding"));
          lcd.setCursor(0,1);
-         lcd.print("    Waypoint");
+         lcd.print(F("    Waypoint"));
          lcd2.clear();
          lcd2.setCursor(0,0);
-         lcd2.print("------ Saving ------");
+         lcd2.print(F("------ Saving ------"));
          lcd2.setCursor(0,1);
-         lcd2.print(" LAT: ");
+         lcd2.print(F(" LAT: "));
          lcd2.print(flat, 8);
          lcd2.setCursor(0,2);
-         lcd2.print(" LON: ");
+         lcd2.print(F(" LON: "));
          lcd2.print(flon, 8);
          lcd2.setCursor(0,3);
-         lcd2.print("-- To Waypoint :");
+         lcd2.print(F("-- To Waypoint :"));
          lcd2.print(mCurrentWayPoint);
          lcd2.setCursor(18,3);
-         lcd2.print("--");
+         lcd2.print(F("--"));
 
          mLat[mCurrentWayPoint] = flat;
          mLon[mCurrentWayPoint] = flon;

// 2x16 LCD stuff
LiquidCrystal lcd(8, 9, 4, 5, 6, 7); 
// define some values used by the panel and buttons
int lcd_key     = 0;
int adc_key_in  = 0;
#define btnRIGHT  0
#define btnUP     1
#define btnDOWN   2

#define MODE_MENU 1
#define MODE_NAVIGATE 2
#define NUMBER_OF_MODES 3
byte mMode = MODE_MENU;
/* Time in millis that the naivagtion should update new data or not */
#define REFRESH_RATE 1000
unsigned long lastUpdate = 0;

//Store Waypoints
#define MAX_WAYPOINTS 7
float mLat[MAX_WAYPOINTS]; //38.628967, -90.270577 Science Center
float mLon[MAX_WAYPOINTS];
byte mCurrentWayPoint = 0;

//Kalman filter for smoothing compass value
Kalman compassFilter(0.125,32,1023,0); //suggested initial values for high noise filtering
