diff --git a/firmware/v01/v01.ino b/firmware/v01/v01.ino
index 39f3c72..c4c3cf5 100644
--- a/firmware/v01/v01.ino
+++ b/firmware/v01/v01.ino
@@ -40,10 +40,18 @@ Adafruit_SSD1306 display(OLED_RESET);
 void one();				// Menu one
 void two();				// Menu two	
 void three();			// Menu three
+void four();			// Menu four
+void five();			// Menu five
 void read_serial();		// Menu that reads the serial
+void write_gps();		// Menu that writes gps out to serial
+void write_hdg();		// Menu that writes hdg out to serial
+void write_wnd();		// Menu that writes wnd out to serial
+void write_dep();		// Menu that writes dep out to serial
+void batt_icon();		// Draws the battery icon acording to value
+
 void write_serial();	// Menu that writes to serial
 void config_nmm();		// Configuration menu
-void batt_icon();		// Draws the battery icon acording to value
+
 
 
 // Variables creation
@@ -55,11 +63,6 @@ int DOWN_state = OFF;  // Holds state for DOWN, either ON or OFF
 int CANCEL_state = OFF;  // Holds state for CANCEL, either ON or OFF
 int batt_val = 0;	// Holds the value of battery voltage
 int incomingByte;	// Serial incoming byte buffer
-int lines = 0;
-int chars = 0;
-char sentence[68];
-String s;
-int i;
 int read_s = 0;
 
 
@@ -135,6 +138,12 @@ ISR(TIMER1_COMPA_vect) //
 		else if (menuState == 3) {
 			menuState = 2;
 		}
+		else if (menuState == 4) {
+			menuState = 3;
+		}
+		else if (menuState == 5) {
+			menuState = 4;
+		}
 		while (!rUP); // Check if button is still pressed do nothing
 	}
   
@@ -147,7 +156,13 @@ ISR(TIMER1_COMPA_vect) //
 			menuState = 3;
 		}
 		else if (menuState == 3) {
-			menuState = 3;
+			menuState = 4;
+		}
+		else if (menuState == 4) {
+			menuState = 5;
+		}
+		else if (menuState == 5) {
+			menuState = 5;
 		}
 		while (!rDOWN); // Check if button is still pressed do nothing
 	}
@@ -166,6 +181,14 @@ ISR(TIMER1_COMPA_vect) //
 			menuState = 12;
 			prevMenu = 3;
 		}
+		else if (menuState == 4) {
+			menuState = 13;
+			prevMenu = 4;
+		}
+		else if (menuState == 5) {
+			menuState = 14;
+			prevMenu = 5;
+		}
 		while (!rOK); // Check if button is still pressed do nothing
 	} 
 
@@ -180,6 +203,12 @@ ISR(TIMER1_COMPA_vect) //
 		else if (prevMenu == 3) {
 			menuState = 3;
 		}
+		else if (prevMenu == 4) {
+			menuState = 4;
+		}
+		else if (prevMenu == 5) {
+			menuState = 5;
+		}
 		while (!rCANCEL); // Check if button is still pressed do nothing
   }
 }
@@ -206,6 +235,16 @@ void loop() {
 	mySerial.end();
 	read_s = 0;
   }
+  else if (menuState == 4) {
+    four();
+	mySerial.end();
+	read_s = 0;
+  }
+  else if (menuState == 5) {
+    five();
+	mySerial.end();
+	read_s = 0;
+  }
   else if (menuState == 10) {
 	if (read_s == 0) {
 		mySerial.begin(4800);
@@ -233,13 +272,41 @@ void loop() {
 		batt_icon();
 		display.display();
 	}
-	write_serial();
+	write_gps();
   }
   else if (menuState == 12) {
     config_nmm();
 	mySerial.end();
 	read_s = 0;
   }
+  else if (menuState == 13) {
+    if (read_s == 0) {
+		mySerial.begin(4800);
+		read_s = 1;
+		display.clearDisplay();
+		display.setTextSize(1);
+		display.setTextColor(WHITE);
+		display.setCursor(1,17);
+		display.println("Sending Heading.....");
+		batt_icon();
+		display.display();
+	}
+	write_hdg();
+  }
+  else if (menuState == 14) {
+    if (read_s == 0) {
+		mySerial.begin(4800);
+		read_s = 1;
+		display.clearDisplay();
+		display.setTextSize(1);
+		display.setTextColor(WHITE);
+		display.setCursor(1,17);
+		display.println("Sending Depth.....");
+		batt_icon();
+		display.display();
+	}
+	write_dep();
+  }
   else {
     one();
 	mySerial.end();
@@ -315,6 +382,10 @@ void one (void) {
 	display.println("Send NMEA0183");
 	display.setCursor(1,32);
 	display.println("Config NMEA0183");
+	display.setCursor(1,42);
+	display.println("Send Heading");
+	display.setCursor(1,52);
+	display.println("Send Depth");
 	batt_icon();
 	display.display();
 }
@@ -330,6 +401,10 @@ void two (void) {
 	display.println("Send NMEA0183     <--");
 	display.setCursor(1,32);
 	display.println("Config NMEA0183");
+	display.setCursor(1,42);
+	display.println("Send Heading");
+	display.setCursor(1,52);
+	display.println("Send Depth");
 	batt_icon();
 	display.display();
 }
@@ -345,6 +420,48 @@ void three (void) {
 	display.println("Send NMEA0183");
 	display.setCursor(1,32);
 	display.println("Config NMEA0183   <--");
+	display.setCursor(1,42);
+	display.println("Send Heading");
+	display.setCursor(1,52);
+	display.println("Send Depth");
+	batt_icon();
+	display.display();
+}
+
+void four (void) {
+
+	display.clearDisplay();
+	display.setTextSize(1);
+	display.setTextColor(WHITE);
+	display.setCursor(1,12);
+	display.println("RS422 ==> USB");
+	display.setCursor(1,22);
+	display.println("Send NMEA0183");
+	display.setCursor(1,32);
+	display.println("Config NMEA0183");
+	display.setCursor(1,42);
+	display.println("Send Heading      <--");
+	display.setCursor(1,52);
+	display.println("Send Depth");
+	batt_icon();
+	display.display();
+}
+
+void five (void) {
+
+	display.clearDisplay();
+	display.setTextSize(1);
+	display.setTextColor(WHITE);
+	display.setCursor(1,12);
+	display.println("RS422 ==> USB");
+	display.setCursor(1,22);
+	display.println("Send NMEA0183");
+	display.setCursor(1,32);
+	display.println("Config NMEA0183");
+	display.setCursor(1,42);
+	display.println("Send Heading");
+	display.setCursor(1,52);
+	display.println("Send Depth        <--");
 	batt_icon();
 	display.display();
 }
@@ -372,3 +489,31 @@ void config_nmm (void) {
   batt_icon();
   display.display();
 }
+
+void write_gps (void) {
+
+//	mySerial.print("$GPGGA,090000.10,6350.37829338,N,02225.18272240,W,1,05,2.87,160.00,M,-21.3213,M,,*64");
+//	mySerial.println();
+	mySerial.println("Im a GPS  :)");
+}
+
+void write_hdg (void) {
+
+//	mySerial.print("$GPGGA,090000.10,6350.37829338,N,02225.18272240,W,1,05,2.87,160.00,M,-21.3213,M,,*64");
+//	mySerial.println();
+	mySerial.println("Im a Gyro  :)");
+}
+
+void write_wnd (void) {
+
+//	mySerial.print("$GPGGA,090000.10,6350.37829338,N,02225.18272240,W,1,05,2.87,160.00,M,-21.3213,M,,*64");
+//	mySerial.println();
+	mySerial.println("Im a Wind sensor  :)");
+}
+
+void write_dep (void) {
+
+//	mySerial.print("$GPGGA,090000.10,6350.37829338,N,02225.18272240,W,1,05,2.87,160.00,M,-21.3213,M,,*64");
+//	mySerial.println();
+	mySerial.println("Im an Echo sounder  :)");
+}