diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index a2bc31834a..ec72214498 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -95,7 +95,7 @@ void update_events(void);
 // All GPS access should be through this pointer.
 GPS         *g_gps;
 
-#if HIL_MODE == HIL_MODE_NONE
+#if HIL_MODE == HIL_MODE_DISABLED
 
 	// real sensors
 	AP_ADC_ADS7844          adc;
@@ -145,15 +145,19 @@ GPS         *g_gps;
 	#error Unrecognised HIL_MODE setting.
 #endif // HIL MODE
 
-// HIL
 #if HIL_MODE != HIL_MODE_DISABLED
 	#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
-		GCS_MAVLINK hil;
+		GCS_MAVLINK	hil(Parameters::k_param_streamrates_port0);
 	#elif HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
 		HIL_XPLANE hil;
 	#endif // HIL PROTOCOL
 #endif // HIL_MODE
 
+//  We may have a hil object instantiated just for mission planning
+#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0
+	GCS_MAVLINK	hil(Parameters::k_param_streamrates_port0);
+#endif
+
 #if HIL_MODE != HIL_MODE_ATTITUDE
 	#if HIL_MODE != HIL_MODE_SENSORS
 		// Normal
@@ -171,7 +175,7 @@ GPS         *g_gps;
 ////////////////////////////////////////////////////////////////////////////////
 //
 #if   GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
-	GCS_MAVLINK         gcs;
+	GCS_MAVLINK	gcs(Parameters::k_param_streamrates_port3);
 #else
 	// If we are not using a GCS, we need a stub that does nothing.
 	GCS_Class           gcs;
@@ -1280,4 +1284,4 @@ void tuning(){
 		g.pitch_max.set(g.rc_6.control_in * 2);  // 0 to 2000
 
 	#endif
-}
\ No newline at end of file
+}
diff --git a/ArduCopterMega/GCS.h b/ArduCopterMega/GCS.h
index 1798f28234..fc7a141886 100644
--- a/ArduCopterMega/GCS.h
+++ b/ArduCopterMega/GCS.h
@@ -136,7 +136,7 @@ protected:
 class GCS_MAVLINK : public GCS_Class
 {
 public:
-	GCS_MAVLINK();
+	GCS_MAVLINK(AP_Var::Key key);
 	void    update(void);
 	void	init(BetterStream *port);
 	void	send_message(uint8_t id, uint32_t param = 0);
@@ -171,12 +171,6 @@ private:
 
 	mavlink_channel_t chan;
     uint16_t packet_drops;
-    uint16_t rawSensorStreamRate;
-    uint16_t attitudeStreamRate;
-    uint16_t positionStreamRate;
-    uint16_t rawControllerStreamRate;
-    uint16_t rcStreamRate;
-    uint16_t extraStreamRate[3];
 
 	// waypoints
     uint16_t requested_interface; // request port to use
@@ -193,14 +187,15 @@ private:
 	float junk; //used to return a junk value for interface
 
 	// data stream rates
-	uint16_t streamRateRawSensors;
-	uint16_t streamRateExtendedStatus;
-	uint16_t streamRateRCChannels;
-	uint16_t streamRateRawController;
-	uint16_t streamRatePosition;
-	uint16_t streamRateExtra1;
-	uint16_t streamRateExtra2;
-	uint16_t streamRateExtra3;
+	AP_Var_group	    _group;
+	AP_Int16 streamRateRawSensors;
+	AP_Int16 streamRateExtendedStatus;
+	AP_Int16 streamRateRCChannels;
+	AP_Int16 streamRateRawController;
+	AP_Int16 streamRatePosition;
+	AP_Int16 streamRateExtra1;
+	AP_Int16 streamRateExtra2;
+	AP_Int16 streamRateExtra3;
 };
 #endif // GCS_PROTOCOL_MAVLINK
 
diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde
index 4a54302b11..cec84de0ba 100644
--- a/ArduCopterMega/GCS_Mavlink.pde
+++ b/ArduCopterMega/GCS_Mavlink.pde
@@ -4,22 +4,22 @@
 
 #include "Mavlink_Common.h"
 
-GCS_MAVLINK::GCS_MAVLINK() :
+GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
 packet_drops(0),
 // parameters
 // note, all values not explicitly initialised here are zeroed
 waypoint_send_timeout(1000), // 1 second
 waypoint_receive_timeout(1000), // 1 second
 // stream rates
-streamRateRawSensors(0),
-streamRateExtendedStatus(0),
-streamRateRCChannels(0),
-streamRateRawController(0),
-//streamRateRawSensorFusion(0),
-streamRatePosition(0),
-streamRateExtra1(0),
-streamRateExtra2(0),
-streamRateExtra3(0)
+_group	(key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
+streamRateRawSensors		(&_group, 0, 0, PSTR("RAW_SENS")),
+streamRateExtendedStatus	(&_group, 1, 0, PSTR("EXT_STAT")),
+streamRateRCChannels		(&_group, 2, 0, PSTR("RC_CHAN")),
+streamRateRawController		(&_group, 3, 0, PSTR("RAW_CTRL")),
+streamRatePosition			(&_group, 4, 0, PSTR("POSITION")),
+streamRateExtra1			(&_group, 5, 0, PSTR("EXTRA1")),
+streamRateExtra2			(&_group, 6, 0, PSTR("EXTRA2")),
+streamRateExtra3			(&_group, 7, 0, PSTR("EXTRA3"))
 {
 }
 
@@ -173,34 +173,35 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     streamRatePosition           = freq;
                     streamRateExtra1             = freq;
                     streamRateExtra2             = freq;
-                    streamRateExtra3             = freq;
+                    streamRateExtra3.set_and_save(freq);	// We just do set and save on the last as it takes care of the whole group.
 					break;
 				case MAV_DATA_STREAM_RAW_SENSORS:
-                    streamRateRawSensors = freq;
+                    streamRateRawSensors = freq;		// We do not set and save this one so that if HIL is shut down incorrectly
+														// we will not continue to broadcast raw sensor data at 50Hz.
 					break;
 				case MAV_DATA_STREAM_EXTENDED_STATUS:
-                    streamRateExtendedStatus = freq;
+                    streamRateExtendedStatus.set_and_save(freq);
 					break;
 				case MAV_DATA_STREAM_RC_CHANNELS:
-                    streamRateRCChannels = freq;
+                    streamRateRCChannels.set_and_save(freq);
 					break;
 				case MAV_DATA_STREAM_RAW_CONTROLLER:
-                    streamRateRawController = freq;
+                    streamRateRawController.set_and_save(freq);
 					break;
 					//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
-                    //    streamRateRawSensorFusion = freq;
+                    //    streamRateRawSensorFusion.set_and_save(freq);
 					//    break;
 				case MAV_DATA_STREAM_POSITION:
-                    streamRatePosition = freq;
+                    streamRatePosition.set_and_save(freq);
 					break;
 				case MAV_DATA_STREAM_EXTRA1:
-                    streamRateExtra1 = freq;
+                    streamRateExtra1.set_and_save(freq);
 					break;
 				case MAV_DATA_STREAM_EXTRA2:
-                    streamRateExtra2 = freq;
+                    streamRateExtra2.set_and_save(freq);
 					break;
 				case MAV_DATA_STREAM_EXTRA3:
-                    streamRateExtra3 = freq;
+                    streamRateExtra3.set_and_save(freq);
 					break;
 				default:
 					break;
diff --git a/ArduCopterMega/HIL.h b/ArduCopterMega/HIL.h
index 8a10a82fb5..85b65fa1be 100644
--- a/ArduCopterMega/HIL.h
+++ b/ArduCopterMega/HIL.h
@@ -71,7 +71,7 @@ public:
 	/// @param	severity	A value describing the importance of the message.
 	/// @param	str			The text to be sent.
 	///
-	void		send_text_P(uint8_t severity, const prog_char_t *str) {}
+	void		send_text(uint8_t severity, const prog_char_t *str) {}
 
 	/// Send acknowledgement for a message.
 	///
@@ -114,7 +114,7 @@ public:
 	void	init(BetterStream *port);
 	void	send_message(uint8_t id, uint32_t param = 0);
 	void	send_text(uint8_t severity, const char *str);
-	void	send_text_P(uint8_t severity, const prog_char_t *str);
+	void	send_text(uint8_t severity, const prog_char_t *str);
 	void	acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
 private:
 	void output_HIL();
diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h
index e734e849e3..bebd8e4de8 100644
--- a/ArduCopterMega/Parameters.h
+++ b/ArduCopterMega/Parameters.h
@@ -17,7 +17,7 @@ public:
     // The increment will prevent old parameters from being used incorrectly
     // by newer code.
     //
-    static const uint16_t k_format_version = 5;
+    static const uint16_t k_format_version = 6;
 
     //
     // Parameter identities.
@@ -49,6 +49,11 @@ public:
         //
         k_param_log_bitmask,
         k_param_frame_type,
+		
+		// 110: Telemetry control
+		//
+		k_param_streamrates_port0 = 110,
+		k_param_streamrates_port3,
 
         //
         // 140: Sensor parameters
diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h
index b7d933fafc..9f7ce82be3 100644
--- a/ArduCopterMega/config.h
+++ b/ArduCopterMega/config.h
@@ -73,18 +73,20 @@
 // HIL_MODE                                 OPTIONAL
 // HIL_PORT                                 OPTIONAL
 
-#if HIL_MODE != HIL_MODE_DISABLED	// we are in HIL mode
-
- # undef GPS_PROTOCOL
- # define GPS_PROTOCOL GPS_PROTOCOL_NONE
+#ifndef HIL_MODE
+#define HIL_MODE	HIL_MODE_DISABLED
+#endif
 
  #ifndef HIL_PROTOCOL
- # error Must define HIL_PROTOCOL if HIL_MODE is not disabled
+#define HIL_PROTOCOL	HIL_PROTOCOL_MAVLINK
  #endif
 
  #ifndef HIL_PORT
- # error Must define HIL_PORT if HIL_PROTOCOL is set
+#define HIL_PORT 0
  #endif
+#if HIL_MODE != HIL_MODE_DISABLED	// we are in HIL mode
+ # undef GPS_PROTOCOL
+ # define GPS_PROTOCOL GPS_PROTOCOL_NONE
 
 #endif
 
@@ -125,9 +127,9 @@
 #endif
 
 //Chris: Commenting out assignment of GCS to port 3 because it kills MAVLink on Port 0
-// #ifndef GCS_PORT
-// # define GCS_PORT			3
-// #endif
+#ifndef GCS_PORT
+# define GCS_PORT			3
+#endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Serial port speeds.
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index dc4dd88086..66b7e2748f 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -155,6 +155,23 @@ void init_ardupilot()
 	#else
 		gcs.init(&Serial);
 	#endif
+	
+	// init the HIL
+#if HIL_MODE != HIL_MODE_DISABLED
+
+  #if HIL_PORT == 3
+	hil.init(&Serial3);
+  #elif HIL_PORT == 1
+	hil.init(&Serial1);
+  #else
+	hil.init(&Serial);
+  #endif
+#endif
+
+//  We may have a hil object instantiated just for mission planning
+#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0
+	hil.init(&Serial);
+#endif
 
 	if(g.compass_enabled)
 		init_compass();
@@ -508,4 +525,4 @@ init_throttle_cruise()
 			//Serial.printf("throttle_cruise: %d\n", g.throttle_cruise.get());
 		}
 	}
-}
\ No newline at end of file
+}