diff --git a/src/gui/view_mavlink.cpp b/src/gui/view_mavlink.cpp
index 826c7946b..80c07e834 100644
--- a/src/gui/view_mavlink.cpp
+++ b/src/gui/view_mavlink.cpp
@@ -90,6 +90,9 @@ void menuTelemetryMavlink(uint8_t event) {
 	case MENU_BATT:
 		menuTelemetryMavlinkBattery();
 		break;
+	case MENU_NAV:
+		menuTelemetryMavlinkNavigation();
+		break;
 	case MENU_GPS:
 		menuTelemetryMavlinkGPS();
 		break;
@@ -337,6 +340,42 @@ void menuTelemetryMavlinkBattery(void) {
     
 }
 
+/*!	\brief Navigation dislplay
+ *	\details Shows Navigation telemetry.
+ */
+void menuTelemetryMavlinkNavigation(void) {
+	
+	mav_title(PSTR("NAV"), MAVLINK_menu);
+	
+	uint8_t x, y, ynum;
+	x = 7 * FWNUM;
+//	x = xnum + 0 * FW;
+	ynum = 2 * FH;
+	y = FH;
+	
+    
+	x = 0;	
+    lcd_puts  (x, y, PSTR("Course"));
+	lcd_outdezAtt(x + 7 * FWNUM, ynum, telemetry_data.course, (DBLSIZE | UNSIGN));
+	lcd_puts(x + 7 * FWNUM, ynum, PSTR("o"));
+	x += 8 * (2 * FWNUM);
+    lcd_puts(x, y, PSTR("Heading"));
+	lcd_outdezAtt(x + 7 * FWNUM, ynum, telemetry_data.heading, (DBLSIZE | UNSIGN));
+	lcd_puts(x + 7 * FWNUM, ynum,  PSTR("o"));
+	y += 3 * FH;
+	ynum += 3 * FH;
+	
+	x = 0;	
+    lcd_puts  (x, y, PSTR("Bearing"));
+	lcd_outdezAtt(x + 7 * FWNUM, ynum, telemetry_data.bearing, (DBLSIZE | UNSIGN));
+	lcd_puts(x + 7 * FWNUM, ynum + FH, PSTR("o"));
+	x += 8 * (2 * FWNUM);
+/*    lcd_puts(x, y, PSTR("PC RSSI"));
+	lcd_outdezAtt(x + 7 * FWNUM, ynum, telemetry_data.pc_rssi, (DBLSIZE | UNSIGN));
+	lcd_puts(x + 7 * FWNUM, ynum + FH,  PSTR("%"));
+ */  
+}
+
 
 
 /*!	\brief GPS information menu
diff --git a/src/gui/view_mavlink.h b/src/gui/view_mavlink.h
index 1491ead91..1a88496a2 100644
--- a/src/gui/view_mavlink.h
+++ b/src/gui/view_mavlink.h
@@ -56,6 +56,7 @@ void mav_title(const pm_char * s, uint8_t index);
 void menuTelemetryMavlinkInfos(void);
 void menuTelemetryMavlinkFlightMode(void);
 void menuTelemetryMavlinkBattery(void);
+void menuTelemetryMavlinkNavigation(void);
 void menuTelemetryMavlinkGPS(void);
 #ifdef DUMP_RX_TX
 void lcd_outhex2(uint8_t x, uint8_t y, uint8_t val);
@@ -70,6 +71,7 @@ enum mavlink_menu_ {
 	MENU_INFO = 0, //
 	MENU_MODE,
 	MENU_BATT,
+	MENU_NAV,
 	MENU_GPS, //
 #ifdef DUMP_RX_TX
 	MENU_DUMP_RX, //
diff --git a/src/telemetry/mavlink.cpp b/src/telemetry/mavlink.cpp
index f38bd6895..b4b2ede36 100644
--- a/src/telemetry/mavlink.cpp
+++ b/src/telemetry/mavlink.cpp
@@ -192,6 +192,15 @@ static inline void REC_MAVLINK_MSG_ID_RADIO(const mavlink_message_t* msg) {
 //	telemetry_data.pc_rssi =  mavlink_msg_radio_get_rssi(msg);
 }
 
+//! \brief Navigaion output message
+static inline void REC_MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT(const mavlink_message_t* msg) {
+	telemetry_data.bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg);
+}
+
+//! \brief Hud navigation message
+static inline void REC_MAVLINK_MSG_ID_VFR_HUD(const mavlink_message_t* msg) {
+	telemetry_data.heading = mavlink_msg_vfr_hud_get_heading(msg);
+}
 
 /*!	\brief Hardbeat message
  */
@@ -389,6 +398,13 @@ static inline void handleMessage(mavlink_message_t* p_rxmsg) {
 		break;
 	case MAVLINK_MSG_ID_RADIO:
 		REC_MAVLINK_MSG_ID_RADIO(p_rxmsg);
+		break;
+	case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
+		REC_MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT(p_rxmsg);
+		break;
+	case MAVLINK_MSG_ID_VFR_HUD:
+		REC_MAVLINK_MSG_ID_VFR_HUD(p_rxmsg);
+		break;	
 	case MAVLINK_MSG_ID_HIL_CONTROLS:
 		REC_MAVLINK_MSG_ID_HIL_CONTROLS(p_rxmsg);
 		break;
diff --git a/src/telemetry/mavlink.h b/src/telemetry/mavlink.h
index 307f2921c..623ec9ebb 100644
--- a/src/telemetry/mavlink.h
+++ b/src/telemetry/mavlink.h
@@ -163,11 +163,11 @@ typedef struct Telemetry_Data_ {
 	uint8_t satellites_visible; ///< Number of satellites visible
 	Location_t loc_current;
 	float eph;
-	float course;
+	uint16_t course;
 	float v; // Ground speed
 	// Navigation
-	float heading;
-	float bearing;
+	uint16_t heading;
+	uint16_t bearing;
 
 #ifdef MAVLINK_PARAMS
 	// Params