diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index f3651d3da0..729d2e9ddf 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -483,7 +483,7 @@ static void fast_loop()
 
 	#if HIL_MODE == HIL_MODE_SENSORS
 		// update hil before dcm update
-		hil.update();
+		gcs_update();
 	#endif
 
 	dcm.update_DCM();
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 77da01321e..e87af0945c 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -1401,8 +1401,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // TODO: check scaling for temp/absPress
             float temp = 70;
             float absPress = 1;
-            //      Serial.printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc);
-            //      Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
+                  //Serial.printf_P(PSTR("accel: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc);
+                  //Serial.printf_P(PSTR("gyro: %d %d %d\n"), packet.xgyro, packet.ygyro, packet.zgyro);
 
             // rad/sec
             Vector3f gyros;
@@ -1432,7 +1432,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // set pressure hil sensor
             // TODO: check scaling
             float temp = 70;
-            barometer.setHIL(temp,packet.press_diff1);
+            barometer.setHIL(temp,packet.press_diff1 + 101325);
             break;
         }
 #endif // HIL_MODE
diff --git a/ArduPlane/config.h b/ArduPlane/config.h
index be76ce2426..be0e3729f9 100644
--- a/ArduPlane/config.h
+++ b/ArduPlane/config.h
@@ -83,24 +83,6 @@
  # define GPS_PROTOCOL GPS_PROTOCOL_NONE
 #endif
 
-
-// If we are in XPlane, diasble the mag
-#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
-
- // check xplane settings
- #if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
-
-  // MAGNETOMETER not supported by XPLANE
-  # undef MAGNETOMETER
-  # define MAGNETOMETER			DISABLED
-
-  # if HIL_MODE != HIL_MODE_ATTITUDE
-  #  error HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE
-  # endif
-
- #endif
-#endif
-
 //////////////////////////////////////////////////////////////////////////////
 // GPS_PROTOCOL
 //
diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde
index aeaeb6aacc..a4da47bba4 100644
--- a/ArduPlane/sensors.pde
+++ b/ArduPlane/sensors.pde
@@ -22,7 +22,7 @@ static void init_barometer(void)
 	for(int i = 0; i < 30; i++){		// We take some readings...
 
 		#if HIL_MODE == HIL_MODE_SENSORS
-			hil.update(); 				// look for inbound hil packets
+			gcs_update(); 				// look for inbound hil packets
 		#endif
 
 		barometer.Read(); 				// Get initial data from absolute pressure sensor
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log
index 132b40a960..3da39a53f9 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log
@@ -7,36 +7,38 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77:
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()':
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h:390: warning: overflow in implicit constant conversion
 autogenerated: At global scope:
-autogenerated:107: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
-autogenerated:117: warning: 'void decrement_WP_index()' declared 'static' but never defined
+autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined
+autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined
+autogenerated:119: warning: 'void decrement_WP_index()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used
-autogenerated:179: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:180: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:181: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
-autogenerated:208: warning: 'void debug_motors()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:183: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:210: warning: 'void debug_motors()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:171: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:229: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:230: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:231: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:231: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:232: warning: 'long int cross_track_test()' declared 'static' but never defined
+autogenerated:233: warning: 'void reset_crosstrack()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:235: warning: 'long int get_altitude_above_home()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:256: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:187: warning: 'void trim_yaw()' defined but not used
-autogenerated:243: warning: 'void readCommands()' declared 'static' but never defined
-autogenerated:244: warning: 'void parseCommand(char*)' declared 'static' but never defined
+autogenerated:245: warning: 'void readCommands()' declared 'static' but never defined
+autogenerated:246: warning: 'void parseCommand(char*)' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:247: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:249: warning: 'long int read_baro_filtered()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used
-autogenerated:261: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:269: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:270: warning: 'int read_num_from_serial()' declared 'static' but never defined
+autogenerated:263: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:264: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:271: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:272: warning: 'int read_num_from_serial()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/system.pde:439: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:284: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never defined
+autogenerated:286: warning: 'void init_optflow()' declared 'static' but never defined
+autogenerated:293: warning: 'void fake_out_gps()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:428: warning: 'undo_event' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:434: warning: 'condition_rate' defined but not used
@@ -45,6 +47,7 @@ autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never de
 /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/test.pde:32: warning: 'int8_t test_xbee(uint8_t, const Menu::arg*)' declared 'static' but never defined
 %% libraries/APM_BMP085/APM_BMP085.o
+%% libraries/APM_BMP085/APM_BMP085_hil.o
 %% libraries/APM_PI/APM_PI.o
 %% libraries/APM_RC/APM_RC.o
 %% libraries/AP_ADC/AP_ADC_ADS7844.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt
index ce54929ef6..f992e2abad 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt
@@ -572,9 +572,9 @@
 0000010a t send_raw_imu2(mavlink_channel_t)
 0000010a t test_gps(unsigned char, Menu::arg const*)
 0000010c t test_current(unsigned char, Menu::arg const*)
-0000010e t send_extended_status1(mavlink_channel_t, unsigned int)
 0000010e W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
 00000112 t set_next_WP(Location*)
+00000112 t send_extended_status1(mavlink_channel_t, unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000118 t set_command_with_index(Location, int)
@@ -600,6 +600,7 @@
 000001be t arm_motors()
 000001cc t start_new_log()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
+000001e6 t verify_nav_wp()
 000001ea t init_home()
 00000210 t setup_motors(unsigned char, Menu::arg const*)
 00000216 t set_mode(unsigned char)
@@ -622,4 +623,4 @@
 00000870 t process_next_command()
 000008f4 W Parameters::Parameters()
 000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-00001bc4 T loop
+000019f6 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log
index 132b40a960..3da39a53f9 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log
@@ -7,36 +7,38 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77:
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()':
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h:390: warning: overflow in implicit constant conversion
 autogenerated: At global scope:
-autogenerated:107: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
-autogenerated:117: warning: 'void decrement_WP_index()' declared 'static' but never defined
+autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined
+autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined
+autogenerated:119: warning: 'void decrement_WP_index()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used
-autogenerated:179: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:180: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:181: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
-autogenerated:208: warning: 'void debug_motors()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:183: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:210: warning: 'void debug_motors()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:171: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:229: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:230: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:231: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:231: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:232: warning: 'long int cross_track_test()' declared 'static' but never defined
+autogenerated:233: warning: 'void reset_crosstrack()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:235: warning: 'long int get_altitude_above_home()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:256: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:187: warning: 'void trim_yaw()' defined but not used
-autogenerated:243: warning: 'void readCommands()' declared 'static' but never defined
-autogenerated:244: warning: 'void parseCommand(char*)' declared 'static' but never defined
+autogenerated:245: warning: 'void readCommands()' declared 'static' but never defined
+autogenerated:246: warning: 'void parseCommand(char*)' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:247: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:249: warning: 'long int read_baro_filtered()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used
-autogenerated:261: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:269: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:270: warning: 'int read_num_from_serial()' declared 'static' but never defined
+autogenerated:263: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:264: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:271: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:272: warning: 'int read_num_from_serial()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/system.pde:439: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:284: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never defined
+autogenerated:286: warning: 'void init_optflow()' declared 'static' but never defined
+autogenerated:293: warning: 'void fake_out_gps()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:428: warning: 'undo_event' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:434: warning: 'condition_rate' defined but not used
@@ -45,6 +47,7 @@ autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never de
 /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/test.pde:32: warning: 'int8_t test_xbee(uint8_t, const Menu::arg*)' declared 'static' but never defined
 %% libraries/APM_BMP085/APM_BMP085.o
+%% libraries/APM_BMP085/APM_BMP085_hil.o
 %% libraries/APM_PI/APM_PI.o
 %% libraries/APM_RC/APM_RC.o
 %% libraries/AP_ADC/AP_ADC_ADS7844.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt
index 14e8679415..641f9b80f2 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt
@@ -572,9 +572,9 @@
 0000010a t send_raw_imu2(mavlink_channel_t)
 0000010a t test_gps(unsigned char, Menu::arg const*)
 0000010c t test_current(unsigned char, Menu::arg const*)
-0000010e t send_extended_status1(mavlink_channel_t, unsigned int)
 0000010e W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
 00000112 t set_next_WP(Location*)
+00000112 t send_extended_status1(mavlink_channel_t, unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000118 t set_command_with_index(Location, int)
@@ -599,6 +599,7 @@
 000001a8 t print_radio_values()
 000001be t arm_motors()
 000001cc t start_new_log()
+000001e4 t verify_nav_wp()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
 000001ea t init_home()
 00000210 t setup_motors(unsigned char, Menu::arg const*)
@@ -622,4 +623,4 @@
 00000870 t process_next_command()
 000008f4 W Parameters::Parameters()
 000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-00001bc2 T loop
+000019f6 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log
index 7438ee818c..94326941b9 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log
@@ -14,41 +14,43 @@ autogenerated: At global scope:
 autogenerated:51: warning: 'void send_raw_imu1(mavlink_channel_t)' declared 'static' but never defined
 autogenerated:52: warning: 'void send_raw_imu2(mavlink_channel_t)' declared 'static' but never defined
 autogenerated:53: warning: 'void send_raw_imu3(mavlink_channel_t)' declared 'static' but never defined
-autogenerated:103: warning: 'void Log_Write_Raw()' declared 'static' but never defined
-autogenerated:107: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
-autogenerated:109: warning: 'void Log_Write_Control_Tuning()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/Log.pde:738: warning: 'void Log_Write_Attitude()' defined but not used
-autogenerated:117: warning: 'void decrement_WP_index()' declared 'static' but never defined
+autogenerated:105: warning: 'void Log_Write_Raw()' declared 'static' but never defined
+autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+autogenerated:111: warning: 'void Log_Write_Control_Tuning()' declared 'static' but never defined
+autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined
+autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/Log.pde:776: warning: 'void Log_Write_Attitude()' defined but not used
+autogenerated:119: warning: 'void decrement_WP_index()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used
-autogenerated:179: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:180: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:181: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
-autogenerated:208: warning: 'void debug_motors()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:183: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:210: warning: 'void debug_motors()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:171: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:229: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:230: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:231: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:231: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:232: warning: 'long int cross_track_test()' declared 'static' but never defined
+autogenerated:233: warning: 'void reset_crosstrack()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:235: warning: 'long int get_altitude_above_home()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:256: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:187: warning: 'void trim_yaw()' defined but not used
-autogenerated:243: warning: 'void readCommands()' declared 'static' but never defined
-autogenerated:244: warning: 'void parseCommand(char*)' declared 'static' but never defined
-autogenerated:245: warning: 'void ReadSCP1000()' declared 'static' but never defined
-autogenerated:246: warning: 'void init_barometer()' declared 'static' but never defined
-autogenerated:247: warning: 'long int read_baro_filtered()' declared 'static' but never defined
-autogenerated:248: warning: 'long int read_barometer()' declared 'static' but never defined
-autogenerated:249: warning: 'void read_airspeed()' declared 'static' but never defined
-autogenerated:250: warning: 'void zero_airspeed()' declared 'static' but never defined
-autogenerated:261: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:269: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:270: warning: 'int read_num_from_serial()' declared 'static' but never defined
+autogenerated:245: warning: 'void readCommands()' declared 'static' but never defined
+autogenerated:246: warning: 'void parseCommand(char*)' declared 'static' but never defined
+autogenerated:247: warning: 'void ReadSCP1000()' declared 'static' but never defined
+autogenerated:248: warning: 'void init_barometer()' declared 'static' but never defined
+autogenerated:249: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:250: warning: 'long int read_barometer()' declared 'static' but never defined
+autogenerated:251: warning: 'void read_airspeed()' declared 'static' but never defined
+autogenerated:252: warning: 'void zero_airspeed()' declared 'static' but never defined
+autogenerated:263: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:264: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:271: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:272: warning: 'int read_num_from_serial()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/system.pde:439: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:284: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never defined
+autogenerated:286: warning: 'void init_optflow()' declared 'static' but never defined
+autogenerated:293: warning: 'void fake_out_gps()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:336: warning: 'old_altitude' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:354: warning: 'abs_pressure' defined but not used
@@ -64,6 +66,7 @@ autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never de
 /root/apm/ardupilot-mega/ArduCopter/test.pde:26: warning: 'int8_t test_baro(uint8_t, const Menu::arg*)' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:32: warning: 'int8_t test_xbee(uint8_t, const Menu::arg*)' declared 'static' but never defined
 %% libraries/APM_BMP085/APM_BMP085.o
+%% libraries/APM_BMP085/APM_BMP085_hil.o
 %% libraries/APM_PI/APM_PI.o
 %% libraries/APM_RC/APM_RC.o
 %% libraries/AP_ADC/AP_ADC_ADS7844.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt
index 26471e85a3..e76064e898 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt
@@ -209,7 +209,6 @@
 00000007 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000008 t setup_erase(unsigned char, Menu::arg const*)
 00000008 r __menu_name__planner_menu
-00000008 W AP_IMU_Shim::update()
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -428,7 +427,6 @@
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
-0000002c B imu
 0000002e t print_divider()
 0000002e t gcs_send_text_P(gcs_severity, prog_char_t const*)
 0000002e t gcs_data_stream_send(unsigned int, unsigned int)
@@ -437,6 +435,7 @@
 0000002f r init_ardupilot()::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 B imu
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
 00000032 r Log_Read_GPS()::__c
@@ -474,6 +473,7 @@
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
 00000052 t report_version()
+00000052 W AP_IMU_Shim::update()
 00000054 t print_enabled(unsigned char)
 00000055 r main_menu_help(unsigned char, Menu::arg const*)::__c
 00000056 t readSwitch()
@@ -588,6 +588,7 @@
 000001c8 t setup_motors(unsigned char, Menu::arg const*)
 000001cc t start_new_log()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
+000001e6 t verify_nav_wp()
 000001ea t init_home()
 00000216 t set_mode(unsigned char)
 00000220 t test_wp(unsigned char, Menu::arg const*)
@@ -606,5 +607,5 @@
 00000824 b g
 00000870 t process_next_command()
 000008f4 W Parameters::Parameters()
-00001530 T loop
+0000136a T loop
 00001604 T GCS_MAVLINK::handleMessage(__mavlink_message*)
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log
index 7438ee818c..94326941b9 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log
@@ -14,41 +14,43 @@ autogenerated: At global scope:
 autogenerated:51: warning: 'void send_raw_imu1(mavlink_channel_t)' declared 'static' but never defined
 autogenerated:52: warning: 'void send_raw_imu2(mavlink_channel_t)' declared 'static' but never defined
 autogenerated:53: warning: 'void send_raw_imu3(mavlink_channel_t)' declared 'static' but never defined
-autogenerated:103: warning: 'void Log_Write_Raw()' declared 'static' but never defined
-autogenerated:107: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
-autogenerated:109: warning: 'void Log_Write_Control_Tuning()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/Log.pde:738: warning: 'void Log_Write_Attitude()' defined but not used
-autogenerated:117: warning: 'void decrement_WP_index()' declared 'static' but never defined
+autogenerated:105: warning: 'void Log_Write_Raw()' declared 'static' but never defined
+autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+autogenerated:111: warning: 'void Log_Write_Control_Tuning()' declared 'static' but never defined
+autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined
+autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/Log.pde:776: warning: 'void Log_Write_Attitude()' defined but not used
+autogenerated:119: warning: 'void decrement_WP_index()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used
-autogenerated:179: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:180: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:181: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
-autogenerated:208: warning: 'void debug_motors()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:183: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:210: warning: 'void debug_motors()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:171: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:229: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:230: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:231: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:231: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:232: warning: 'long int cross_track_test()' declared 'static' but never defined
+autogenerated:233: warning: 'void reset_crosstrack()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:235: warning: 'long int get_altitude_above_home()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:256: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:187: warning: 'void trim_yaw()' defined but not used
-autogenerated:243: warning: 'void readCommands()' declared 'static' but never defined
-autogenerated:244: warning: 'void parseCommand(char*)' declared 'static' but never defined
-autogenerated:245: warning: 'void ReadSCP1000()' declared 'static' but never defined
-autogenerated:246: warning: 'void init_barometer()' declared 'static' but never defined
-autogenerated:247: warning: 'long int read_baro_filtered()' declared 'static' but never defined
-autogenerated:248: warning: 'long int read_barometer()' declared 'static' but never defined
-autogenerated:249: warning: 'void read_airspeed()' declared 'static' but never defined
-autogenerated:250: warning: 'void zero_airspeed()' declared 'static' but never defined
-autogenerated:261: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:269: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:270: warning: 'int read_num_from_serial()' declared 'static' but never defined
+autogenerated:245: warning: 'void readCommands()' declared 'static' but never defined
+autogenerated:246: warning: 'void parseCommand(char*)' declared 'static' but never defined
+autogenerated:247: warning: 'void ReadSCP1000()' declared 'static' but never defined
+autogenerated:248: warning: 'void init_barometer()' declared 'static' but never defined
+autogenerated:249: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:250: warning: 'long int read_barometer()' declared 'static' but never defined
+autogenerated:251: warning: 'void read_airspeed()' declared 'static' but never defined
+autogenerated:252: warning: 'void zero_airspeed()' declared 'static' but never defined
+autogenerated:263: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:264: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:271: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:272: warning: 'int read_num_from_serial()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/system.pde:439: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:284: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never defined
+autogenerated:286: warning: 'void init_optflow()' declared 'static' but never defined
+autogenerated:293: warning: 'void fake_out_gps()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:336: warning: 'old_altitude' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:354: warning: 'abs_pressure' defined but not used
@@ -64,6 +66,7 @@ autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never de
 /root/apm/ardupilot-mega/ArduCopter/test.pde:26: warning: 'int8_t test_baro(uint8_t, const Menu::arg*)' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:32: warning: 'int8_t test_xbee(uint8_t, const Menu::arg*)' declared 'static' but never defined
 %% libraries/APM_BMP085/APM_BMP085.o
+%% libraries/APM_BMP085/APM_BMP085_hil.o
 %% libraries/APM_PI/APM_PI.o
 %% libraries/APM_RC/APM_RC.o
 %% libraries/AP_ADC/AP_ADC_ADS7844.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt
index 8bdf018d18..419cdbc68e 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt
@@ -209,7 +209,6 @@
 00000007 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000008 t setup_erase(unsigned char, Menu::arg const*)
 00000008 r __menu_name__planner_menu
-00000008 W AP_IMU_Shim::update()
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -428,7 +427,6 @@
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
-0000002c B imu
 0000002e t print_divider()
 0000002e t gcs_send_text_P(gcs_severity, prog_char_t const*)
 0000002e t gcs_data_stream_send(unsigned int, unsigned int)
@@ -437,6 +435,7 @@
 0000002f r init_ardupilot()::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 B imu
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
 00000032 r Log_Read_GPS()::__c
@@ -474,6 +473,7 @@
 00000050 r log_menu_commands
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
+00000052 W AP_IMU_Shim::update()
 00000054 t print_enabled(unsigned char)
 00000055 r main_menu_help(unsigned char, Menu::arg const*)::__c
 00000056 t readSwitch()
@@ -587,6 +587,7 @@
 000001a8 t print_radio_values()
 000001c8 t setup_motors(unsigned char, Menu::arg const*)
 000001cc t start_new_log()
+000001e4 t verify_nav_wp()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
 000001ea t init_home()
 00000216 t set_mode(unsigned char)
@@ -606,5 +607,5 @@
 00000824 b g
 00000870 t process_next_command()
 000008f4 W Parameters::Parameters()
-0000152e T loop
+0000136a T loop
 00001604 T GCS_MAVLINK::handleMessage(__mavlink_message*)
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log
index 132b40a960..3da39a53f9 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log
@@ -7,36 +7,38 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77:
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()':
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h:390: warning: overflow in implicit constant conversion
 autogenerated: At global scope:
-autogenerated:107: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
-autogenerated:117: warning: 'void decrement_WP_index()' declared 'static' but never defined
+autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined
+autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined
+autogenerated:119: warning: 'void decrement_WP_index()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used
-autogenerated:179: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:180: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:181: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
-autogenerated:208: warning: 'void debug_motors()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:183: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:210: warning: 'void debug_motors()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:171: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:229: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:230: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:231: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:231: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:232: warning: 'long int cross_track_test()' declared 'static' but never defined
+autogenerated:233: warning: 'void reset_crosstrack()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:235: warning: 'long int get_altitude_above_home()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:256: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:187: warning: 'void trim_yaw()' defined but not used
-autogenerated:243: warning: 'void readCommands()' declared 'static' but never defined
-autogenerated:244: warning: 'void parseCommand(char*)' declared 'static' but never defined
+autogenerated:245: warning: 'void readCommands()' declared 'static' but never defined
+autogenerated:246: warning: 'void parseCommand(char*)' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:247: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:249: warning: 'long int read_baro_filtered()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used
-autogenerated:261: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:269: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:270: warning: 'int read_num_from_serial()' declared 'static' but never defined
+autogenerated:263: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:264: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:271: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:272: warning: 'int read_num_from_serial()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/system.pde:439: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:284: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never defined
+autogenerated:286: warning: 'void init_optflow()' declared 'static' but never defined
+autogenerated:293: warning: 'void fake_out_gps()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:428: warning: 'undo_event' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:434: warning: 'condition_rate' defined but not used
@@ -45,6 +47,7 @@ autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never de
 /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/test.pde:32: warning: 'int8_t test_xbee(uint8_t, const Menu::arg*)' declared 'static' but never defined
 %% libraries/APM_BMP085/APM_BMP085.o
+%% libraries/APM_BMP085/APM_BMP085_hil.o
 %% libraries/APM_PI/APM_PI.o
 %% libraries/APM_RC/APM_RC.o
 %% libraries/AP_ADC/AP_ADC_ADS7844.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt
index 58ab5e94d3..ae1ad47303 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt
@@ -572,9 +572,9 @@
 0000010a t send_raw_imu2(mavlink_channel_t)
 0000010a t test_gps(unsigned char, Menu::arg const*)
 0000010c t test_current(unsigned char, Menu::arg const*)
-0000010e t send_extended_status1(mavlink_channel_t, unsigned int)
 0000010e W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
 00000112 t set_next_WP(Location*)
+00000112 t send_extended_status1(mavlink_channel_t, unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000118 t set_command_with_index(Location, int)
@@ -601,6 +601,7 @@
 000001c8 t setup_motors(unsigned char, Menu::arg const*)
 000001cc t start_new_log()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
+000001e6 t verify_nav_wp()
 000001ea t init_home()
 00000216 t set_mode(unsigned char)
 0000021a t send_raw_imu1(mavlink_channel_t)
@@ -622,4 +623,4 @@
 00000870 t process_next_command()
 000008f4 W Parameters::Parameters()
 000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-00001abe T loop
+000018f0 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log
index 132b40a960..3da39a53f9 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log
@@ -7,36 +7,38 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77:
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()':
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h:390: warning: overflow in implicit constant conversion
 autogenerated: At global scope:
-autogenerated:107: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
-autogenerated:117: warning: 'void decrement_WP_index()' declared 'static' but never defined
+autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined
+autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined
+autogenerated:119: warning: 'void decrement_WP_index()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used
-autogenerated:179: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:180: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:181: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
-autogenerated:208: warning: 'void debug_motors()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:183: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:210: warning: 'void debug_motors()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:171: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:229: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:230: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:231: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:231: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:232: warning: 'long int cross_track_test()' declared 'static' but never defined
+autogenerated:233: warning: 'void reset_crosstrack()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:235: warning: 'long int get_altitude_above_home()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:256: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:187: warning: 'void trim_yaw()' defined but not used
-autogenerated:243: warning: 'void readCommands()' declared 'static' but never defined
-autogenerated:244: warning: 'void parseCommand(char*)' declared 'static' but never defined
+autogenerated:245: warning: 'void readCommands()' declared 'static' but never defined
+autogenerated:246: warning: 'void parseCommand(char*)' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:247: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:249: warning: 'long int read_baro_filtered()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used
-autogenerated:261: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:269: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:270: warning: 'int read_num_from_serial()' declared 'static' but never defined
+autogenerated:263: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:264: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:271: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:272: warning: 'int read_num_from_serial()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/system.pde:439: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:284: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never defined
+autogenerated:286: warning: 'void init_optflow()' declared 'static' but never defined
+autogenerated:293: warning: 'void fake_out_gps()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:428: warning: 'undo_event' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:434: warning: 'condition_rate' defined but not used
@@ -45,6 +47,7 @@ autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never de
 /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/test.pde:32: warning: 'int8_t test_xbee(uint8_t, const Menu::arg*)' declared 'static' but never defined
 %% libraries/APM_BMP085/APM_BMP085.o
+%% libraries/APM_BMP085/APM_BMP085_hil.o
 %% libraries/APM_PI/APM_PI.o
 %% libraries/APM_RC/APM_RC.o
 %% libraries/AP_ADC/AP_ADC_ADS7844.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt
index 8692e7af93..be0de07635 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt
@@ -572,9 +572,9 @@
 0000010a t send_raw_imu2(mavlink_channel_t)
 0000010a t test_gps(unsigned char, Menu::arg const*)
 0000010c t test_current(unsigned char, Menu::arg const*)
-0000010e t send_extended_status1(mavlink_channel_t, unsigned int)
 0000010e W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
 00000112 t set_next_WP(Location*)
+00000112 t send_extended_status1(mavlink_channel_t, unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000118 t set_command_with_index(Location, int)
@@ -600,6 +600,7 @@
 000001be t arm_motors()
 000001c8 t setup_motors(unsigned char, Menu::arg const*)
 000001cc t start_new_log()
+000001e4 t verify_nav_wp()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
 000001ea t init_home()
 00000216 t set_mode(unsigned char)
@@ -622,4 +623,4 @@
 00000870 t process_next_command()
 000008f4 W Parameters::Parameters()
 000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-00001abc T loop
+000018f0 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log
index 132b40a960..3da39a53f9 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log
@@ -7,36 +7,38 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77:
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()':
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h:390: warning: overflow in implicit constant conversion
 autogenerated: At global scope:
-autogenerated:107: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
-autogenerated:117: warning: 'void decrement_WP_index()' declared 'static' but never defined
+autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined
+autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined
+autogenerated:119: warning: 'void decrement_WP_index()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used
-autogenerated:179: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:180: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:181: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
-autogenerated:208: warning: 'void debug_motors()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:183: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:210: warning: 'void debug_motors()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:171: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:229: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:230: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:231: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:231: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:232: warning: 'long int cross_track_test()' declared 'static' but never defined
+autogenerated:233: warning: 'void reset_crosstrack()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:235: warning: 'long int get_altitude_above_home()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:256: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:187: warning: 'void trim_yaw()' defined but not used
-autogenerated:243: warning: 'void readCommands()' declared 'static' but never defined
-autogenerated:244: warning: 'void parseCommand(char*)' declared 'static' but never defined
+autogenerated:245: warning: 'void readCommands()' declared 'static' but never defined
+autogenerated:246: warning: 'void parseCommand(char*)' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:247: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:249: warning: 'long int read_baro_filtered()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used
-autogenerated:261: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:269: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:270: warning: 'int read_num_from_serial()' declared 'static' but never defined
+autogenerated:263: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:264: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:271: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:272: warning: 'int read_num_from_serial()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/system.pde:439: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:284: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never defined
+autogenerated:286: warning: 'void init_optflow()' declared 'static' but never defined
+autogenerated:293: warning: 'void fake_out_gps()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:428: warning: 'undo_event' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:434: warning: 'condition_rate' defined but not used
@@ -45,6 +47,7 @@ autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never de
 /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/test.pde:32: warning: 'int8_t test_xbee(uint8_t, const Menu::arg*)' declared 'static' but never defined
 %% libraries/APM_BMP085/APM_BMP085.o
+%% libraries/APM_BMP085/APM_BMP085_hil.o
 %% libraries/APM_PI/APM_PI.o
 %% libraries/APM_RC/APM_RC.o
 %% libraries/AP_ADC/AP_ADC_ADS7844.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt
index 9e9c40d6e2..2847fdba3d 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt
@@ -601,6 +601,7 @@
 000001be t arm_motors()
 000001cc t start_new_log()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
+000001e6 t verify_nav_wp()
 000001ea t init_home()
 00000216 t set_mode(unsigned char)
 0000021a t send_raw_imu1(mavlink_channel_t)
@@ -622,4 +623,4 @@
 00000870 t process_next_command()
 000008f4 W Parameters::Parameters()
 000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-000019fa T loop
+0000182c T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log
index 132b40a960..3da39a53f9 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log
@@ -7,36 +7,38 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77:
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()':
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h:390: warning: overflow in implicit constant conversion
 autogenerated: At global scope:
-autogenerated:107: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
-autogenerated:117: warning: 'void decrement_WP_index()' declared 'static' but never defined
+autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined
+autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined
+autogenerated:119: warning: 'void decrement_WP_index()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used
-autogenerated:179: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:180: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:181: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
-autogenerated:208: warning: 'void debug_motors()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:183: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:210: warning: 'void debug_motors()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:171: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:229: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:230: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:231: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:231: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:232: warning: 'long int cross_track_test()' declared 'static' but never defined
+autogenerated:233: warning: 'void reset_crosstrack()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:235: warning: 'long int get_altitude_above_home()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:256: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:187: warning: 'void trim_yaw()' defined but not used
-autogenerated:243: warning: 'void readCommands()' declared 'static' but never defined
-autogenerated:244: warning: 'void parseCommand(char*)' declared 'static' but never defined
+autogenerated:245: warning: 'void readCommands()' declared 'static' but never defined
+autogenerated:246: warning: 'void parseCommand(char*)' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:247: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:249: warning: 'long int read_baro_filtered()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used
-autogenerated:261: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:269: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:270: warning: 'int read_num_from_serial()' declared 'static' but never defined
+autogenerated:263: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:264: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:271: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:272: warning: 'int read_num_from_serial()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/system.pde:439: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:284: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never defined
+autogenerated:286: warning: 'void init_optflow()' declared 'static' but never defined
+autogenerated:293: warning: 'void fake_out_gps()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:428: warning: 'undo_event' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:434: warning: 'condition_rate' defined but not used
@@ -45,6 +47,7 @@ autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never de
 /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/test.pde:32: warning: 'int8_t test_xbee(uint8_t, const Menu::arg*)' declared 'static' but never defined
 %% libraries/APM_BMP085/APM_BMP085.o
+%% libraries/APM_BMP085/APM_BMP085_hil.o
 %% libraries/APM_PI/APM_PI.o
 %% libraries/APM_RC/APM_RC.o
 %% libraries/AP_ADC/AP_ADC_ADS7844.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt
index 4b71375c34..be18e39982 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt
@@ -600,6 +600,7 @@
 000001a8 t print_radio_values()
 000001be t arm_motors()
 000001cc t start_new_log()
+000001e4 t verify_nav_wp()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
 000001ea t init_home()
 00000216 t set_mode(unsigned char)
@@ -622,4 +623,4 @@
 00000870 t process_next_command()
 000008f4 W Parameters::Parameters()
 000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-000019f8 T loop
+0000182c T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log
index 132b40a960..3da39a53f9 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log
@@ -7,36 +7,38 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77:
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()':
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h:390: warning: overflow in implicit constant conversion
 autogenerated: At global scope:
-autogenerated:107: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
-autogenerated:117: warning: 'void decrement_WP_index()' declared 'static' but never defined
+autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined
+autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined
+autogenerated:119: warning: 'void decrement_WP_index()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used
-autogenerated:179: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:180: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:181: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
-autogenerated:208: warning: 'void debug_motors()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:183: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:210: warning: 'void debug_motors()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:171: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:229: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:230: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:231: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:231: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:232: warning: 'long int cross_track_test()' declared 'static' but never defined
+autogenerated:233: warning: 'void reset_crosstrack()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:235: warning: 'long int get_altitude_above_home()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:256: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:187: warning: 'void trim_yaw()' defined but not used
-autogenerated:243: warning: 'void readCommands()' declared 'static' but never defined
-autogenerated:244: warning: 'void parseCommand(char*)' declared 'static' but never defined
+autogenerated:245: warning: 'void readCommands()' declared 'static' but never defined
+autogenerated:246: warning: 'void parseCommand(char*)' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:247: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:249: warning: 'long int read_baro_filtered()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used
-autogenerated:261: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:269: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:270: warning: 'int read_num_from_serial()' declared 'static' but never defined
+autogenerated:263: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:264: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:271: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:272: warning: 'int read_num_from_serial()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/system.pde:439: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:284: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never defined
+autogenerated:286: warning: 'void init_optflow()' declared 'static' but never defined
+autogenerated:293: warning: 'void fake_out_gps()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:428: warning: 'undo_event' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:434: warning: 'condition_rate' defined but not used
@@ -45,6 +47,7 @@ autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never de
 /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/test.pde:32: warning: 'int8_t test_xbee(uint8_t, const Menu::arg*)' declared 'static' but never defined
 %% libraries/APM_BMP085/APM_BMP085.o
+%% libraries/APM_BMP085/APM_BMP085_hil.o
 %% libraries/APM_PI/APM_PI.o
 %% libraries/APM_RC/APM_RC.o
 %% libraries/AP_ADC/AP_ADC_ADS7844.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt
index fc2736e609..57e2d50514 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt
@@ -572,9 +572,9 @@
 0000010a t send_raw_imu2(mavlink_channel_t)
 0000010a t test_gps(unsigned char, Menu::arg const*)
 0000010c t test_current(unsigned char, Menu::arg const*)
-0000010e t send_extended_status1(mavlink_channel_t, unsigned int)
 0000010e W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
 00000112 t set_next_WP(Location*)
+00000112 t send_extended_status1(mavlink_channel_t, unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000118 t set_command_with_index(Location, int)
@@ -601,6 +601,7 @@
 000001be t arm_motors()
 000001cc t start_new_log()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
+000001e6 t verify_nav_wp()
 000001ea t init_home()
 00000216 t set_mode(unsigned char)
 0000021a t send_raw_imu1(mavlink_channel_t)
@@ -622,4 +623,4 @@
 00000870 t process_next_command()
 000008f4 W Parameters::Parameters()
 000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-00001ad2 T loop
+00001904 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log
index 132b40a960..3da39a53f9 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log
@@ -7,36 +7,38 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77:
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h: In constructor 'Parameters::Parameters()':
 /root/apm/ardupilot-mega/ArduCopter/Parameters.h:390: warning: overflow in implicit constant conversion
 autogenerated: At global scope:
-autogenerated:107: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
-autogenerated:117: warning: 'void decrement_WP_index()' declared 'static' but never defined
+autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+autogenerated:90: warning: 'void Log_Write_Attitude2()' declared 'static' but never defined
+autogenerated:91: warning: 'void Log_Read_Attitude2()' declared 'static' but never defined
+autogenerated:119: warning: 'void decrement_WP_index()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/commands.pde:132: warning: 'Location get_LOITER_home_wp()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:432: warning: 'bool verify_loiter_unlim()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/events.pde:46: warning: 'void low_battery_event()' defined but not used
-autogenerated:179: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:180: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:181: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
-autogenerated:208: warning: 'void debug_motors()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:183: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:210: warning: 'void debug_motors()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:171: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:229: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:230: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:231: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:231: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:232: warning: 'long int cross_track_test()' declared 'static' but never defined
+autogenerated:233: warning: 'void reset_crosstrack()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:235: warning: 'long int get_altitude_above_home()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/navigation.pde:256: warning: 'long int get_alt_distance(Location*, Location*)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/radio.pde:187: warning: 'void trim_yaw()' defined but not used
-autogenerated:243: warning: 'void readCommands()' declared 'static' but never defined
-autogenerated:244: warning: 'void parseCommand(char*)' declared 'static' but never defined
+autogenerated:245: warning: 'void readCommands()' declared 'static' but never defined
+autogenerated:246: warning: 'void parseCommand(char*)' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:247: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:249: warning: 'long int read_baro_filtered()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used
-autogenerated:261: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:269: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:270: warning: 'int read_num_from_serial()' declared 'static' but never defined
+autogenerated:263: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:264: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:271: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:272: warning: 'int read_num_from_serial()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/system.pde:439: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:284: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never defined
+autogenerated:286: warning: 'void init_optflow()' declared 'static' but never defined
+autogenerated:293: warning: 'void fake_out_gps()' declared 'static' but never defined
 /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:428: warning: 'undo_event' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:434: warning: 'condition_rate' defined but not used
@@ -45,6 +47,7 @@ autogenerated:291: warning: 'void fake_out_gps()' declared 'static' but never de
 /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/test.pde:32: warning: 'int8_t test_xbee(uint8_t, const Menu::arg*)' declared 'static' but never defined
 %% libraries/APM_BMP085/APM_BMP085.o
+%% libraries/APM_BMP085/APM_BMP085_hil.o
 %% libraries/APM_PI/APM_PI.o
 %% libraries/APM_RC/APM_RC.o
 %% libraries/AP_ADC/AP_ADC_ADS7844.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt
index 446ce2f741..19102af2aa 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt
@@ -572,9 +572,9 @@
 0000010a t send_raw_imu2(mavlink_channel_t)
 0000010a t test_gps(unsigned char, Menu::arg const*)
 0000010c t test_current(unsigned char, Menu::arg const*)
-0000010e t send_extended_status1(mavlink_channel_t, unsigned int)
 0000010e W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
 00000112 t set_next_WP(Location*)
+00000112 t send_extended_status1(mavlink_channel_t, unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000112 T GCS_MAVLINK::GCS_MAVLINK(unsigned int)
 00000118 t set_command_with_index(Location, int)
@@ -600,6 +600,7 @@
 000001a8 t print_radio_values()
 000001be t arm_motors()
 000001cc t start_new_log()
+000001e4 t verify_nav_wp()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
 000001ea t init_home()
 00000216 t set_mode(unsigned char)
@@ -622,4 +623,4 @@
 00000870 t process_next_command()
 000008f4 W Parameters::Parameters()
 000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-00001ad0 T loop
+00001904 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml
index d0e428508a..e7ec09031a 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml
+++ b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml
@@ -160,4 +160,55 @@
 </desc>
     <format_version>110</format_version>
   </Firmware>
+  <Firmware>
+    <url>http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELIHIL-1280.hex</url>
+    <url2560>http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELIHIL-2560.hex</url2560>
+    <name>Please Update</name>
+    <desc>
+
+#define HIL_MODE            HIL_MODE_ATTITUDE
+#define ALLOW_RC_OVERRIDE ENABLED
+
+#define AUTO_RESET_LOITER 0
+#define FRAME_CONFIG HELI_FRAME
+
+// DEFAULT PIDS
+
+// roll
+#define STABILIZE_ROLL_P 0.70
+#define STABILIZE_ROLL_I 0.025
+#define STABILIZE_ROLL_D 0.04
+#define STABILIZE_ROLL_IMAX 7
+
+//pitch
+#define STABILIZE_PITCH_P 0.70
+#define STABILIZE_PITCH_I 0.025
+#define STABILIZE_PITCH_D 0.04
+#define STABILIZE_PITCH_IMAX 7
+
+// yaw stablise
+#define STABILIZE_YAW_P  0.7
+#define STABILIZE_YAW_I  0.02
+#define STABILIZE_YAW_D  0.0
+
+// yaw rate
+#define RATE_YAW_P  0.135   
+#define RATE_YAW_I  0.0
+#define RATE_YAW_D  0.0
+
+// throttle
+#define THROTTLE_P 0.2
+#define THROTTLE_I 0.001
+#define THROTTLE_IMAX 100
+
+// navigation
+#define NAV_LOITER_P  1.1
+#define NAV_LOITER_I  0.03
+#define NAV_LOITER_D  0.02
+#define NAV_LOITER_IMAX  10
+
+
+</desc>
+    <format_version>0</format_version>
+  </Firmware>
 </options>
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/git.log b/Tools/ArdupilotMegaPlanner/Firmware/git.log
index a1c5d0adc8..2a7bfed917 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/git.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/git.log
@@ -1,44 +1 @@
-From https://code.google.com/p/ardupilot-mega
-   25b258c..a188536  APM_Camera -> origin/APM_Camera
-   36e7d95..9f44a2e  master     -> origin/master
-Updating 36e7d95..9f44a2e
-Fast-forward
- ArduCopter/ArduCopter.pde                          |   11 +-
- ArduCopter/commands_logic.pde                      |    2 +-
- ArduCopter/config.h                                |   10 +-
- ArduCopter/motors.pde                              |    2 +-
- ArduCopter/motors_hexa.pde                         |   14 ++-
- ArduCopter/motors_octa.pde                         |   19 ++-
- ArduCopter/motors_octa_quad.pde                    |   13 +-
- ArduCopter/motors_quad.pde                         |   11 +
- ArduCopter/motors_tri.pde                          |   13 ++
- ArduCopter/motors_y6.pde                           |   14 ++-
- ArduCopter/radio.pde                               |    1 +
- ArduCopter/system.pde                              |    4 +-
- Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj    |    1 +
- Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs       |  222 ++++++++++++++++++++
- Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs  |    7 +-
- Tools/ArdupilotMegaPlanner/JoystickSetup.cs        |    2 +-
- Tools/ArdupilotMegaPlanner/MainV2.cs               |    7 +
- .../Properties/AssemblyInfo.cs                     |    2 +-
- Tools/ArdupilotMegaPlanner/bin/Release/.gitignore  |    3 +-
- .../ArdupilotMegaPlanner/bin/Release/ArduPlane.exe |  Bin 0 -> 673167 bytes
- .../bin/Release/ArdupilotMegaPlanner.application   |    2 +-
- .../bin/Release/ArdupilotMegaPlanner.exe           |  Bin 2186240 -> 2189824 bytes
- Tools/ArdupilotMegaPlanner/bin/Release/Updater.exe |  Bin 8192 -> 8192 bytes
- Tools/ArdupilotMegaPlanner/bin/Release/cygwin1.dll |  Bin 0 -> 2666500 bytes
- Tools/ArdupilotMegaPlanner/bin/Release/resedit.exe |  Bin 20480 -> 20480 bytes
- .../ru-RU/ArdupilotMegaPlanner.resources.dll       |  Bin 53248 -> 53248 bytes
- .../zh-Hans/ArdupilotMegaPlanner.resources.dll     |  Bin 380928 -> 380928 bytes
- apo/ControllerQuad.h                               |   91 ++++----
- apo/QuadArducopter.h                               |   32 ++--
- apo/apo.pde                                        |    2 +-
- libraries/APO/APO_DefaultSetup.h                   |   17 +-
- libraries/APO/AP_Navigator.h                       |    3 +-
- libraries/APO/AP_RcChannel.h                       |    2 +-
- libraries/Desktop/Desktop.mk                       |    5 +-
- libraries/Desktop/support/Arduino.cpp              |   24 ++-
- 35 files changed, 438 insertions(+), 98 deletions(-)
- create mode 100644 Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs
- create mode 100644 Tools/ArdupilotMegaPlanner/bin/Release/ArduPlane.exe
- create mode 100644 Tools/ArdupilotMegaPlanner/bin/Release/cygwin1.dll
+Already up-to-date.
diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h
index 19e763585a..e8f3675817 100644
--- a/apo/ControllerQuad.h
+++ b/apo/ControllerQuad.h
@@ -152,8 +152,8 @@ public:
 		case MAV_MODE_MANUAL: {
 			setAllRadioChannelsManually();
 			// "mix manual"
-			cmdRoll = -1 * _hal->rc[CH_ROLL]->getPosition();
-			cmdPitch = -1 * _hal->rc[CH_PITCH]->getPosition();
+			cmdRoll = -0.5 * _hal->rc[CH_ROLL]->getPosition();
+			cmdPitch = -0.5 * _hal->rc[CH_PITCH]->getPosition();
 			cmdYawRate = -1 * _hal->rc[CH_YAW]->getPosition();
 			thrustMix = _hal->rc[CH_THRUST]->getPosition();
 			break;
diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h
index 4d1941b1f6..4c733e5013 100644
--- a/apo/QuadArducopter.h
+++ b/apo/QuadArducopter.h
@@ -11,7 +11,7 @@
 // vehicle options
 static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD;
 static const apo::halMode_t halMode = apo::MODE_LIVE;
-static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
+static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560;
 static const uint8_t heartBeatTimeout = 3;
 
 // algorithm selection
@@ -47,18 +47,13 @@ static const bool rangeFinderUpEnabled = false;
 static const bool rangeFinderDownEnabled = false;
 
 // loop rates
-static const float loop0Rate = 150;
-static const float loop1Rate = 100;
-static const float loop2Rate = 10;
-static const float loop3Rate = 1;
+static const float loop0Rate = 200; // attitude nav
+static const float loop1Rate = 50; // controller
+static const float loop2Rate = 10; 	// pos nav/ gcs fast
+static const float loop3Rate = 1; 	// gcs slow
 static const float loop4Rate = 0.1;
 
-//motor parameters
-static const float MOTOR_MAX = 1;
-static const float MOTOR_MIN = 0.1;
-
 // position control loop
-static const float PID_POS_INTERVAL = 1 / 100; // 5 hz
 static const float PID_POS_P = 0;
 static const float PID_POS_I = 0;
 static const float PID_POS_D = 0;
@@ -71,30 +66,23 @@ static const float PID_POS_Z_LIM = 0;
 static const float PID_POS_Z_AWU = 0;
 
 // attitude control loop
-static const float PID_ATT_INTERVAL = 1 / 100; // 100 hz
-static const float PID_ATT_P = 0.1; // 0.1
-static const float PID_ATT_I = 0; // 0.0
-static const float PID_ATT_D = 0.1; // 0.1
-static const float PID_ATT_LIM = 1; // 0.01 // 10 % #define MOTORs
-static const float PID_ATT_AWU = 0; // 0.0
+static const float PID_ATT_P = 0.3;
+static const float PID_ATT_I = 0.5;
+static const float PID_ATT_D = 0.08;
+static const float PID_ATT_LIM = 0.1; // 10 %
+static const float PID_ATT_AWU = 0.03; // 3 %
 static const float PID_YAWPOS_P = 0;
 static const float PID_YAWPOS_I = 0;
 static const float PID_YAWPOS_D = 0;
 static const float PID_YAWPOS_LIM = 0; // 1 rad/s
 static const float PID_YAWPOS_AWU = 0; // 1 rad/s
-static const float PID_YAWSPEED_P = .2;
+static const float PID_YAWSPEED_P = 0.5;
 static const float PID_YAWSPEED_I = 0;
 static const float PID_YAWSPEED_D = 0;
-static const float PID_YAWSPEED_LIM = .3; // 0.01 // 10 % MOTORs
+static const float PID_YAWSPEED_LIM = .1; // 10 % MOTORs
 static const float PID_YAWSPEED_AWU = 0.0;
 static const float PID_YAWSPEED_DFCUT = 3.0; // 3 Radians, about 1 Hz
 
-// mixing
-static const float MIX_REMOTE_WEIGHT = 1;
-static const float MIX_POSITION_WEIGHT = 1;
-static const float MIX_POSITION_Z_WEIGHT = 1;
-static const float MIX_POSITION_YAW_WEIGHT = 1;
-
 static const float THRUST_HOVER_OFFSET = 0.475;
 
 #include "ControllerQuad.h"
diff --git a/libraries/APM_BMP085/APM_BMP085.cpp b/libraries/APM_BMP085/APM_BMP085.cpp
index 511784240b..22b6806b57 100644
--- a/libraries/APM_BMP085/APM_BMP085.cpp
+++ b/libraries/APM_BMP085/APM_BMP085.cpp
@@ -286,44 +286,4 @@ void APM_BMP085_Class::Calculate()
 	x1 = (x1 * 3038) >> 16;
 	x2 = (-7357 * p) >> 16;
 	Press = p + ((x1 + x2 + 3791) >> 4);
-}
-
-// Constructors ////////////////////////////////////////////////////////////////
-APM_BMP085_HIL_Class::APM_BMP085_HIL_Class()
-{
-}
-
-// Public Methods //////////////////////////////////////////////////////////////
-void APM_BMP085_HIL_Class::Init(int initialiseWireLib)
-{
-  BMP085_State=1;
-}
-
-
-// Read the sensor. This is a state machine
-// We read one time Temperature (state = 1) and then 4 times Pressure (states 2-5)
-uint8_t APM_BMP085_HIL_Class::Read()
-{
-	uint8_t result = 0;
-
-	if (BMP085_State == 1){
-		BMP085_State++;
-	}else{
-
-		if (BMP085_State == 5){
-			BMP085_State = 1;				// Start again from state = 1
-			result = 1;						// New pressure reading
-		}else{
-			BMP085_State++;
-			result = 1;						// New pressure reading
-		}
-	}
-	return(result);
-}
-
-void APM_BMP085_HIL_Class::setHIL(float _Temp, float _Press)
-{
-    // TODO: map floats to raw
-	Temp 	= _Temp;
-	Press 	= _Press;
-}
+}
\ No newline at end of file
diff --git a/libraries/APM_BMP085/APM_BMP085.h b/libraries/APM_BMP085/APM_BMP085.h
index e8a5085d0f..7b75c7c865 100644
--- a/libraries/APM_BMP085/APM_BMP085.h
+++ b/libraries/APM_BMP085/APM_BMP085.h
@@ -4,6 +4,8 @@
 #define TEMP_FILTER_SIZE 16
 #define PRESS_FILTER_SIZE 10
 
+#include <APM_BMP085_hil.h>
+
 class APM_BMP085_Class
 {
   public:
@@ -43,21 +45,4 @@ class APM_BMP085_Class
 	void Calculate();
 };
 
-class APM_BMP085_HIL_Class
-{
-  private:
-    uint8_t BMP085_State;
-  public:
-	int32_t RawPress;
-	int32_t RawTemp;
-	int16_t Temp;
-	int32_t Press;
-	//int Altitude;
-	uint8_t oss;
-	APM_BMP085_HIL_Class();  // Constructor
-	void Init(int initialiseWireLib = 1);
-	uint8_t Read();
-    void setHIL(float Temp, float Press);
-};
-
 #endif
diff --git a/libraries/APM_BMP085/APM_BMP085_hil.cpp b/libraries/APM_BMP085/APM_BMP085_hil.cpp
new file mode 100644
index 0000000000..21fc3dce5a
--- /dev/null
+++ b/libraries/APM_BMP085/APM_BMP085_hil.cpp
@@ -0,0 +1,50 @@
+
+
+extern "C" {
+  // AVR LibC Includes
+  #include <inttypes.h>
+  #include <avr/interrupt.h>
+  #include "WConstants.h"
+}
+
+#include "APM_BMP085_hil.h"
+
+// Constructors ////////////////////////////////////////////////////////////////
+APM_BMP085_HIL_Class::APM_BMP085_HIL_Class()
+{
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+void APM_BMP085_HIL_Class::Init(int initialiseWireLib)
+{
+  BMP085_State=1;
+}
+
+
+// Read the sensor. This is a state machine
+// We read one time Temperature (state = 1) and then 4 times Pressure (states 2-5)
+uint8_t APM_BMP085_HIL_Class::Read()
+{
+	uint8_t result = 0;
+
+	if (BMP085_State == 1){
+		BMP085_State++;
+	}else{
+
+		if (BMP085_State == 5){
+			BMP085_State = 1;				// Start again from state = 1
+			result = 1;						// New pressure reading
+		}else{
+			BMP085_State++;
+			result = 1;						// New pressure reading
+		}
+	}
+	return(result);
+}
+
+void APM_BMP085_HIL_Class::setHIL(float _Temp, float _Press)
+{
+    // TODO: map floats to raw
+	Temp 	= _Temp;
+	Press 	= _Press;
+}
diff --git a/libraries/APM_BMP085/APM_BMP085_hil.h b/libraries/APM_BMP085/APM_BMP085_hil.h
new file mode 100644
index 0000000000..9a316b64a4
--- /dev/null
+++ b/libraries/APM_BMP085/APM_BMP085_hil.h
@@ -0,0 +1,24 @@
+#ifndef APM_BMP085_hil_h
+#define APM_BMP085_hil_h
+
+#define TEMP_FILTER_SIZE 16
+#define PRESS_FILTER_SIZE 10
+
+class APM_BMP085_HIL_Class
+{
+  private:
+    uint8_t BMP085_State;
+  public:
+  	APM_BMP085_HIL_Class();  // Constructor
+	int32_t RawPress;
+	int32_t RawTemp;
+	int16_t Temp;
+	int32_t Press;
+	//int Altitude;
+	uint8_t oss;
+	void Init(int initialiseWireLib = 1);
+	uint8_t Read();
+    void setHIL(float Temp, float Press);
+};
+
+#endif
diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp
index fa796d0626..411edc0c3c 100644
--- a/libraries/APO/AP_Autopilot.cpp
+++ b/libraries/APO/AP_Autopilot.cpp
@@ -17,7 +17,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
 	Loop(loop0Rate, callback0, this), _navigator(navigator), _guide(guide),
 			_controller(controller), _hal(hal), _loop0Rate(loop0Rate),
 			_loop1Rate(loop1Rate), _loop2Rate(loop2Rate), _loop3Rate(loop3Rate),
-			_loop4Rate(loop3Rate) {
+			_loop4Rate(loop3Rate), callback0Calls(0), clockInit(millis()) {
 
 	hal->setState(MAV_STATE_BOOT);
 	hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
@@ -111,6 +111,8 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
 	hal->debug->println_P(PSTR("initializing radio"));
 	APM_RC.Init(); // APM Radio initialization,
 	// start this after control loop is running
+	
+	clockInit = millis();
 }
 
 void AP_Autopilot::callback0(void * data) {
@@ -120,6 +122,8 @@ void AP_Autopilot::callback0(void * data) {
 	/*
 	 * ahrs update
 	 */
+
+	apo->callback0Calls++;
 	if (apo->getNavigator())
 		apo->getNavigator()->updateFast(1.0 / apo->getLoopRate(0));
 }
@@ -163,19 +167,6 @@ void AP_Autopilot::callback2(void * data) {
 		apo->getGuide()->update();
 	}
 
-	/*
-	 * send telemetry
-	 */
-	if (apo->getHal()->gcs) {
-		// send messages
-		apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW);
-		apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE);
-		//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
-		//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION);
-		apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW);
-		//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
-	}
-
 	/*
 	 * slow navigation loop update
 	 */
@@ -183,6 +174,13 @@ void AP_Autopilot::callback2(void * data) {
 		apo->getNavigator()->updateSlow(1.0 / apo->getLoopRate(2));
 	}
 
+	/*
+	 * send telemetry
+	 */
+	if (apo->getHal()->gcs) {
+		apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE);
+	}
+
 	/*
 	 * handle ground control station communication
 	 */
@@ -215,6 +213,18 @@ void AP_Autopilot::callback2(void * data) {
 void AP_Autopilot::callback3(void * data) {
 	AP_Autopilot * apo = (AP_Autopilot *) data;
 	//apo->getHal()->debug->println_P(PSTR("callback 3"));
+	
+	/*
+	 * send telemetry
+	 */
+	if (apo->getHal()->gcs) {
+		// send messages
+		apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW);
+		//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
+		//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION);
+		//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW);
+		//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
+	}
 
 	/*
 	 * send heartbeat
@@ -225,6 +235,7 @@ void AP_Autopilot::callback3(void * data) {
 	 * load/loop rate/ram debug
 	 */
 	apo->getHal()->load = apo->load();
+	apo->getHal()->debug->printf_P(PSTR("missed calls: %d\n"),uint16_t(millis()*apo->getLoopRate(0)/1000-apo->callback0Calls));
 	apo->getHal()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"),
 			apo->load(),1.0/apo->dt(),freeMemory());
 
diff --git a/libraries/APO/AP_Autopilot.h b/libraries/APO/AP_Autopilot.h
index 08daf19f57..97c53cd470 100644
--- a/libraries/APO/AP_Autopilot.h
+++ b/libraries/APO/AP_Autopilot.h
@@ -99,6 +99,12 @@ public:
 		}
 	}
 
+	/**
+	 * Loop Monitoring
+	 */
+	uint32_t callback0Calls;
+	uint32_t clockInit;
+
 private:
 
 	/**
diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h
index aabbb0eaf6..8330112541 100644
--- a/libraries/APO/AP_Navigator.h
+++ b/libraries/APO/AP_Navigator.h
@@ -279,6 +279,7 @@ public:
 		}
 	}
 	virtual void updateFast(float dt) {
+
 		if (_hal->getMode() != MODE_LIVE)
 			return;
 
diff --git a/libraries/AP_IMU/AP_IMU_Shim.h b/libraries/AP_IMU/AP_IMU_Shim.h
index 6995a22b79..4dea37330a 100644
--- a/libraries/AP_IMU/AP_IMU_Shim.h
+++ b/libraries/AP_IMU/AP_IMU_Shim.h
@@ -13,12 +13,19 @@ public:
 
 	/// @name IMU protocol
 	//@{
-	virtual void	init(Start_style style, void (*callback)(unsigned long t)) {}
+    virtual void	init(Start_style style = COLD_START, void (*callback)(unsigned long t) = delay) {};
 	virtual void	init_accel(void (*callback)(unsigned long t) = delay) {};
 	virtual void	init_gyro(void (*callback)(unsigned long t) = delay) {};
 	virtual bool	update(void) {
 		bool updated = _updated;
 		_updated = false;
+        
+        // return number of microseconds since last call
+        uint32_t us = micros();
+        uint32_t ret = us - last_ch6_micros;
+        last_ch6_micros = us;
+        
+        _sample_time = ret;
 		return updated;
 	}
 	//@}
@@ -54,6 +61,7 @@ public:
 private:
 	/// set true when new data is delivered
 	bool		_updated;
+    uint32_t    last_ch6_micros;
 };
 
 #endif
diff --git a/libraries/Desktop/Makefile.desktop b/libraries/Desktop/Makefile.desktop
index 8c82eb3fb9..2449a8b256 100644
--- a/libraries/Desktop/Makefile.desktop
+++ b/libraries/Desktop/Makefile.desktop
@@ -13,6 +13,9 @@ nogps:
 
 hil:
 	make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
+    
+hilsen:
+	make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
 
 hilnocli:
 	make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
@@ -21,4 +24,4 @@ heli:
 	make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME"
 
 helihil:
-	make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME -DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"
\ No newline at end of file
+	make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME -DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"