diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log
index b2a63cd62c..930bed821c 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.build.log
@@ -3,40 +3,38 @@
 In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
-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:399: warning: overflow in implicit constant conversion
-autogenerated: At global scope:
-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:121: 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/Attitude.pde:132: warning: 'void reset_hold_I()' defined but not used
+autogenerated:33: warning: 'void init_z_damper()' declared 'static' but never defined
+autogenerated:114: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:106: warning: 'void handle_no_commands()' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:433: 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:183: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:184: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:185: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:180: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:187: warning: 'int heli_get_angle_boost(int)' declared 'static' but never defined
 autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined
-autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/navigation.pde:98: warning: 'void calc_loiter2(int, int)' defined but not used
+autogenerated:236: warning: 'int32_t get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:238: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:240: warning: 'int32_t get_alt_distance(Location*, Location*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/radio.pde:132: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:251: warning: 'int32_t 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:262: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:263: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:270: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:271: warning: 'int read_num_from_serial()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:292: 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/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+autogenerated:265: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:266: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:273: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:274: warning: 'int read_num_from_serial()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/system.pde:486: warning: 'void set_failsafe(boolean)' defined but not used
+autogenerated:289: warning: 'void init_optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:265: warning: 'rc_override' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:266: warning: 'rc_override_active' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:267: warning: 'rc_override_fs_timer' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:488: warning: 'pmTest1' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1768: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/test.pde:34: warning: 'int8_t test_rawgps(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
@@ -77,7 +75,7 @@ In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_Optic
 %% libraries/AP_RangeFinder/RangeFinder.o
 %% libraries/AP_Relay/AP_Relay.o
 %% libraries/DataFlash/DataFlash.o
-In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35:
+In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:36:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
 %% libraries/FastSerial/BetterStream.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt
index 7dbb3e32c5..6c2208496d 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.size.txt
@@ -3,16 +3,17 @@
 00000001 b home_is_set
 00000001 b motor_armed
 00000001 b motor_light
+00000001 b CH7_wp_index
 00000001 b control_mode
 00000001 b gps_watchdog
 00000001 b simple_timer
 00000001 d yaw_tracking
 00000001 b land_complete
 00000001 b throttle_mode
-00000001 b command_may_ID
+00000001 b mavlink_active
+00000001 b prev_nav_index
 00000001 b wp_verify_byte
 00000001 d altitude_sensor
-00000001 b command_must_ID
 00000001 b command_yaw_dir
 00000001 b new_radio_frame
 00000001 b roll_pitch_mode
@@ -24,13 +25,14 @@
 00000001 b old_control_mode
 00000001 b slow_loopCounter
 00000001 b takeoff_complete
-00000001 b command_may_index
+00000001 b command_nav_index
 00000001 b oldSwitchPosition
-00000001 b command_must_index
+00000001 b command_cond_index
 00000001 d ground_start_count
 00000001 b medium_loopCounter
 00000001 b command_yaw_relative
 00000001 d jump
+00000001 b nav_ok
 00000001 b event_id
 00000001 b led_mode
 00000001 b yaw_mode
@@ -45,20 +47,25 @@
 00000002 T userhook_init()
 00000002 b climb_rate
 00000002 b loiter_sum
+00000002 b sonar_rate
+00000002 b angle_boost
 00000002 b event_delay
 00000002 b event_value
 00000002 b event_repeat
 00000002 b loiter_total
+00000002 b manual_boost
 00000002 b nav_throttle
+00000002 b old_baro_alt
 00000002 b x_rate_error
 00000002 b y_rate_error
-00000002 b altitude_rate
 00000002 b gps_fix_count
+00000002 b old_sonar_alt
 00000002 b velocity_land
 00000002 b x_actual_speed
 00000002 b y_actual_speed
 00000002 b loiter_time_max
 00000002 b command_yaw_time
+00000002 b crosstrack_error
 00000002 b event_undo_value
 00000002 b command_yaw_speed
 00000002 b auto_level_counter
@@ -69,6 +76,7 @@
 00000002 b g_gps
 00000002 b airspeed
 00000002 b baro_alt
+00000002 b baro_rate
 00000002 b sonar_alt
 00000002 b arm_motors()::arming_counter
 00000002 r setup_frame(unsigned char, Menu::arg const*)::__c
@@ -82,6 +90,9 @@
 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c
 00000003 r print_enabled(unsigned char)::__c
 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c
+00000003 r report_batt_monitor()::__c
+00000003 r report_batt_monitor()::__c
+00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 d cos_roll_x
 00000004 b land_start
 00000004 b long_error
@@ -93,15 +104,14 @@
 00000004 b sin_pitch_y
 00000004 b wp_distance
 00000004 b abs_pressure
+00000004 b circle_angle
 00000004 b current_amps
-00000004 b old_altitude
 00000004 b original_alt
 00000004 b simple_cos_x
 00000004 b simple_sin_y
 00000004 b current_total
 00000004 b nav_loopTimer
 00000004 d scaleLongDown
-00000004 t test_failsafe(unsigned char, Menu::arg const*)
 00000004 b altitude_error
 00000004 b fast_loopTimer
 00000004 b perf_mon_timer
@@ -152,7 +162,6 @@
 00000004 r setup_compass(unsigned char, Menu::arg const*)::__c
 00000004 r print_log_menu()::__c
 00000004 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
-00000004 r test_adc(unsigned char, Menu::arg const*)::__c
 00000004 V Parameters::Parameters()::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
@@ -171,7 +180,6 @@
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
-00000005 r test_adc(unsigned char, Menu::arg const*)::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
@@ -193,6 +201,7 @@
 00000006 r Log_Read_Mode()::__c
 00000006 r print_log_menu()::__c
 00000006 r print_log_menu()::__c
+00000006 r report_batt_monitor()::__c
 00000006 V Parameters::Parameters()::__c
 00000007 b setup_menu
 00000007 b planner_menu
@@ -205,6 +214,8 @@
 00000007 r report_radio()::__c
 00000007 r report_sonar()::__c
 00000007 r print_enabled(unsigned char)::__c
+00000007 r Log_Read_Nav_Tuning()::__c
+00000007 r Log_Read_Control_Tuning()::__c
 00000007 r test_wp(unsigned char, Menu::arg const*)::__c
 00000007 V Parameters::Parameters()::__c
 00000007 V Parameters::Parameters()::__c
@@ -214,6 +225,7 @@
 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 r print_done()::__c
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -221,8 +233,6 @@
 00000008 r report_tuning()::__c
 00000008 r init_ardupilot()::__c
 00000008 r print_log_menu()::__c
-00000008 r report_batt_monitor()::__c
-00000008 r report_batt_monitor()::__c
 00000008 r test_wp(unsigned char, Menu::arg const*)::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
@@ -230,7 +240,6 @@
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
-00000008 V Parameters::Parameters()::__c
 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000009 r print_switch(unsigned char, unsigned char, bool)::__c
 00000009 r print_log_menu()::__c
@@ -260,7 +269,6 @@
 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 0000000a T piezo_on()
 0000000a T piezo_off()
-0000000a r test_relay(unsigned char, Menu::arg const*)::__c
 0000000a r start_new_log()::__c
 0000000a r print_log_menu()::__c
 0000000a r Log_Read_Startup()::__c
@@ -270,9 +278,10 @@
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
 0000000a T setup
-0000000b r test_relay(unsigned char, Menu::arg const*)::__c
-0000000b r report_batt_monitor()::__c
+0000000b r setup_esc(unsigned char, Menu::arg const*)::__c
 0000000b V Parameters::Parameters()::__c
 0000000c t process_logs(unsigned char, Menu::arg const*)
 0000000c b omega
@@ -282,11 +291,13 @@
 0000000c r print_switch(unsigned char, unsigned char, bool)::__c
 0000000c r report_frame()::__c
 0000000c r setup_factory(unsigned char, Menu::arg const*)::__c
-0000000c r test_baro(unsigned char, Menu::arg const*)::__c
+0000000c r report_version()::__c
+0000000c r report_batt_monitor()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000d r select_logs(unsigned char, Menu::arg const*)::__c
+0000000d r setup_frame(unsigned char, Menu::arg const*)::__c
 0000000d r test_tuning(unsigned char, Menu::arg const*)::__c
 0000000d r test_battery(unsigned char, Menu::arg const*)::__c
 0000000d r startup_ground()::__c
@@ -298,6 +309,8 @@
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
 0000000d B sonar_mode_filter
 0000000e t global destructors keyed to Serial
 0000000e t global constructors keyed to Serial
@@ -313,6 +326,7 @@
 0000000e r setup_mode(unsigned char, Menu::arg const*)::__c
 0000000e r test_sonar(unsigned char, Menu::arg const*)::__c
 0000000e r select_logs(unsigned char, Menu::arg const*)::__c
+0000000e r setup_compass(unsigned char, Menu::arg const*)::__c
 0000000e r print_log_menu()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
@@ -321,6 +335,7 @@
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
+0000000e r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 0000000e r report_batt_monitor()::__c
 0000000e r report_flight_modes()::__c
 0000000e r dump_log(unsigned char, Menu::arg const*)::__c
@@ -332,19 +347,18 @@
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
-0000000e V Parameters::Parameters()::__c
 0000000f b current_loc
-0000000f b next_command
+0000000f b command_nav_queue
+0000000f b command_cond_queue
 0000000f b home
 0000000f b next_WP
 0000000f b prev_WP
 0000000f b guided_WP
 0000000f b target_WP
+0000000f r setup_sonar(unsigned char, Menu::arg const*)::__c
+0000000f r Log_Read_Data()::__c
 0000000f r print_log_menu()::__c
 0000000f r print_log_menu()::__c
-0000000f r report_version()::__c
-0000000f r report_batt_monitor()::__c
-0000000f r test_imu(unsigned char, Menu::arg const*)::__c
 0000000f V AP_IMU_Oilpan::AP_IMU_Oilpan(AP_ADC*, unsigned int)::__c
 00000010 r planner_menu_commands
 00000010 b motor_out
@@ -354,7 +368,6 @@
 00000010 r report_compass()::__c
 00000011 r erase_logs(unsigned char, Menu::arg const*)::__c
 00000011 r zero_eeprom()::__c
-00000011 r update_commands()::__c
 00000012 B Serial
 00000012 B Serial1
 00000012 B Serial3
@@ -368,30 +381,22 @@
 00000012 W AP_VarT<float>::~AP_VarT()
 00000012 W AP_VarT<int>::~AP_VarT()
 00000012 W AP_VarT<signed char>::serialize(void*, unsigned int) const
-00000012 r print_done()::__c
 00000012 r select_logs(unsigned char, Menu::arg const*)::__c
-00000012 r setup_frame(unsigned char, Menu::arg const*)::__c
 00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
-00000013 r setup_compass(unsigned char, Menu::arg const*)::__c
-00000013 r change_command(unsigned char)::__c
-00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 00000014 W AP_VarT<signed char>::unserialize(void*, unsigned int)
 00000014 W AP_VarT<signed char>::cast_to_float() const
 00000014 W AP_VarT<int>::cast_to_float() const
-00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c
-00000014 r test_tri(unsigned char, Menu::arg const*)::__c
 00000015 r init_ardupilot()::__c
-00000015 r Log_Read_Motors()::__c
 00000015 r print_hit_enter()::__c
 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
 00000016 T piezo_beep()
+00000016 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000016 B sonar
 00000018 t setup_tune(unsigned char, Menu::arg const*)
 00000018 t setup_accel(unsigned char, Menu::arg const*)
 00000018 r __menu_name__main_menu
 00000018 W AP_VarT<int>::serialize(void*, unsigned int) const
 0000001a r print_log_menu()::__c
-0000001a r Log_Read_Nav_Tuning()::__c
 0000001c W AP_VarA<float, (unsigned char)6>::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Matrix3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Vector3<float> >::unserialize(void*, unsigned int)
@@ -399,18 +404,21 @@
 0000001c W AP_VarA<float, (unsigned char)6>::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Matrix3<float> >::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Vector3<float> >::serialize(void*, unsigned int) const
+0000001c r print_gyro_offsets()::__c
+0000001c r print_accel_offsets()::__c
+0000001d r report_compass()::__c
+0000001d r Log_Read_Motors()::__c
 0000001d r Log_Read_Attitude()::__c
+0000001d r Log_Read_Performance()::__c
 0000001e r Log_Read_Optflow()::__c
 00000020 t gcs_send_message(ap_message)
 00000020 r test_current(unsigned char, Menu::arg const*)::__c
 00000020 t byte_swap_4
+00000021 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000021 r print_log_menu()::__c
-00000021 r report_compass()::__c
 00000021 r Log_Read_Current()::__c
-00000021 r Log_Read_Performance()::__c
 00000022 t clear_leds()
 00000022 t print_blanks(int)
-00000022 t reset_hold_I()
 00000022 t startup_ground()
 00000022 W AP_Float16::~AP_Float16()
 00000022 W AP_VarA<float, (unsigned char)6>::~AP_VarA()
@@ -419,52 +427,49 @@
 00000022 W AP_VarT<signed char>::~AP_VarT()
 00000022 W AP_VarT<float>::~AP_VarT()
 00000022 W AP_VarT<int>::~AP_VarT()
-00000023 r setup_mode(unsigned char, Menu::arg const*)::__c
-00000023 r print_gyro_offsets()::__c
+00000022 r test_imu(unsigned char, Menu::arg const*)::__c
+00000023 r test_baro(unsigned char, Menu::arg const*)::__c
 00000024 r init_ardupilot()::__c
-00000024 r print_accel_offsets()::__c
 00000026 t print_done()
+00000026 t Log_Write_Data(signed char, float)
 00000026 t print_hit_enter()
 00000026 t Log_Read_Startup()
-00000026 r Log_Read_Control_Tuning()::__c
 00000028 t test_battery(unsigned char, Menu::arg const*)
 00000028 t main_menu_help(unsigned char, Menu::arg const*)
-00000028 t help_log(unsigned char, Menu::arg const*)
 00000028 W AP_VarT<float>::unserialize(void*, unsigned int)
 00000028 W AP_VarT<float>::serialize(void*, unsigned int) const
 00000028 r Log_Read_Cmd()::__c
-00000029 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
+0000002a t _mav_put_int8_t_array
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
 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)
 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char)
+0000002e r init_ardupilot()::__c
 0000002f r test_radio(unsigned char, Menu::arg const*)::__c
-0000002f r init_ardupilot()::__c
+0000002f r setup_factory(unsigned char, Menu::arg const*)::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 r setup_radio(unsigned char, Menu::arg const*)::__c
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
 00000032 r Log_Read_GPS()::__c
 00000033 b pending_status
 00000034 W AP_Float16::serialize(void*, unsigned int) const
-00000034 t _mav_put_int8_t_array
 00000034 t mavlink_msg_statustext_send
-00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c
 00000036 t report_radio()
-00000037 r print_wp(Location*, unsigned char)::__c
-00000038 t init_throttle_cruise()
+00000036 r print_wp(Location*, unsigned char)::__c
 00000038 t send_current_waypoint(mavlink_channel_t)
-00000038 r setup_radio(unsigned char, Menu::arg const*)::__c
-00000038 r setup_factory(unsigned char, Menu::arg const*)::__c
 0000003a t report_gps()
 0000003a t report_imu()
-0000003c W RC_Channel::~RC_Channel()
+0000003c B barometer
 0000003d B g_gps_driver
 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*)
 0000003e W AP_VarT<signed char>::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char)
+00000040 r log_menu_commands
+00000040 t init_throttle_cruise()
 00000040 t read_AHRS()
 00000040 W AP_Float16::unserialize(void*, unsigned int)
 00000040 B adc
@@ -472,14 +477,13 @@
 00000042 t report_sonar()
 00000044 t setup_show(unsigned char, Menu::arg const*)
 00000044 W AP_VarT<int>::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char)
-00000048 t change_command(unsigned char)
+00000044 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
+00000046 W RC_Channel::~RC_Channel()
 00000048 t update_motor_leds()
-00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
 0000004a t send_meminfo(mavlink_channel_t)
 0000004a W AP_VarT<int>::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char)
 0000004c t update_auto_yaw()
 00000050 b mavlink_queue
-00000050 r log_menu_commands
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
 00000050 B imu
@@ -489,13 +493,13 @@
 00000056 t readSwitch()
 00000056 t dancing_light()
 00000056 T GCS_MAVLINK::queued_waypoint_send()
-00000057 r help_log(unsigned char, Menu::arg const*)::__c
+0000005a t Log_Write_Data(signed char, long)
 0000005a W AP_VarT<float>::AP_VarT(float, unsigned int, prog_char_t const*, unsigned char)
 0000005c t get_num_logs()
 0000005c t setup_esc(unsigned char, Menu::arg const*)
+0000005e t change_command(unsigned char)
 0000005e t update_GPS_light()
 0000005e T GCS_MAVLINK::_count_parameters()
-00000064 B barometer
 00000064 t mavlink_msg_param_value_send
 00000068 t zero_eeprom()
 00000068 t find_last_log_page(int)
@@ -504,13 +508,12 @@
 0000006c t setup_sonar(unsigned char, Menu::arg const*)
 0000006e T output_min()
 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*)
-00000078 t do_RTL()
 0000007a t setup_factory(unsigned char, Menu::arg const*)
 0000007a t read_control_switch()
 0000007a t report_flight_modes()
+0000007a t do_RTL()
+0000007c t Log_Read_Data()
 0000007c t send_gps_status(mavlink_channel_t)
-0000007c t test_baro(unsigned char, Menu::arg const*)
-0000007e t test_rawgps(unsigned char, Menu::arg const*)
 00000080 T __vector_25
 00000080 T __vector_36
 00000080 T __vector_54
@@ -523,107 +526,101 @@
 00000092 t test_tuning(unsigned char, Menu::arg const*)
 00000092 t report_tuning()
 00000092 T GCS_MAVLINK::queued_param_send()
-00000096 t print_wp(Location*, unsigned char)
 0000009a t planner_gcs(unsigned char, Menu::arg const*)
 0000009a t init_compass()
-0000009a t Log_Read_Motors()
-0000009b B gcs0
-0000009b B gcs3
+0000009c B gcs0
+0000009c B gcs3
 0000009e t setup_mode(unsigned char, Menu::arg const*)
-0000009e t Log_Write_Cmd(unsigned char, Location*)
 000000a0 t Log_Read_Mode()
+000000a0 r test_menu_commands
 000000a4 T __vector_26
 000000a4 T __vector_37
 000000a4 T __vector_55
 000000aa t test_sonar(unsigned char, Menu::arg const*)
-000000aa t Log_Read_Nav_Tuning()
 000000ae t report_frame()
-000000b0 t test_relay(unsigned char, Menu::arg const*)
 000000b2 t erase_logs(unsigned char, Menu::arg const*)
 000000b6 t get_log_boundaries(unsigned char, int&, int&)
 000000b7 B compass
 000000be t update_events()
 000000c2 t setup_compass(unsigned char, Menu::arg const*)
 000000c2 t send_radio_out(mavlink_channel_t)
+000000c2 t Log_Read_Motors()
 000000c2 t Log_Read_Attitude()
 000000c4 t get_distance(Location*, Location*)
-000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c
 000000c6 t test_eedump(unsigned char, Menu::arg const*)
 000000c6 t send_radio_in(mavlink_channel_t)
-000000c6 t Log_Read_Performance()
-000000c6 t test_tri(unsigned char, Menu::arg const*)
 000000c7 B dcm
 000000ca t init_barometer()
 000000d0 t read_radio()
 000000d0 t get_bearing(Location*, Location*)
+000000d0 t Log_Read_Nav_Tuning()
+000000d0 t Log_Read_Control_Tuning()
 000000d2 t print_switch(unsigned char, unsigned char, bool)
-000000d4 t Log_Read(int, int)
+000000d4 t print_wp(Location*, unsigned char)
 000000d8 t test_radio(unsigned char, Menu::arg const*)
 000000d8 t read_barometer()
-000000de t test_adc(unsigned char, Menu::arg const*)
+000000dc t Log_Read(int, int)
 000000e0 r setup_menu_commands
-000000e4 t test_radio_pwm(unsigned char, Menu::arg const*)
 000000e4 t Log_Read_Optflow()
 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)
 000000e8 t Log_Read_Current()
-000000ea t Log_Read_Control_Tuning()
 000000ee t report_batt_monitor()
+000000ee t Log_Read_Performance()
 000000f6 t Log_Read_Cmd()
-00000100 r test_menu_commands
+000000fa t calc_loiter_pitch_roll()
 0000010a t mavlink_delay(unsigned long)
 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 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)
-0000011c t get_command_with_index(int)
-0000012c t calc_loiter_pitch_roll()
+00000118 t arm_motors()
+0000011c t get_cmd_with_index(int)
 00000130 t report_compass()
-00000138 t get_stabilize_roll(long)
-00000138 t get_stabilize_pitch(long)
-00000148 t Log_Read_GPS()
+00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
+0000013a t test_baro(unsigned char, Menu::arg const*)
+0000014c t get_stabilize_roll(long)
+0000014c t get_stabilize_pitch(long)
 0000014e t send_servo_out(mavlink_channel_t)
-00000156 t update_commands()
+00000152 t init_home()
+00000156 t Log_Read_GPS()
 0000015c t update_trig()
 00000160 t send_location(mavlink_channel_t)
-00000160 t send_nav_controller_output(mavlink_channel_t)
-00000162 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
+00000160 t test_wp(unsigned char, Menu::arg const*)
+00000164 t set_cmd_with_index(Location, int)
 00000166 t select_logs(unsigned char, Menu::arg const*)
 00000166 t send_vfr_hud(mavlink_channel_t)
-00000168 T GCS_MAVLINK::update()
-0000016c t test_imu(unsigned char, Menu::arg const*)
+00000166 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
 0000016e t send_attitude(mavlink_channel_t)
 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int)
+0000017a t verify_nav_wp()
 000001a2 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int)
 000001a8 t print_radio_values()
-000001be t arm_motors()
-000001cc t start_new_log()
+000001b2 t start_new_log()
+000001b8 t send_nav_controller_output(mavlink_channel_t)
+000001ce T GCS_MAVLINK::update()
 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)
+00000210 t test_imu(unsigned char, Menu::arg const*)
 0000021a t send_raw_imu1(mavlink_channel_t)
-00000220 t test_wp(unsigned char, Menu::arg const*)
 00000228 t setup_radio(unsigned char, Menu::arg const*)
-0000022a t send_gps_raw(mavlink_channel_t)
-00000242 t calc_loiter(int, int)
+00000246 t calc_loiter(int, int)
 00000268 t send_raw_imu3(mavlink_channel_t)
-00000330 t tuning()
+0000026a t send_gps_raw(mavlink_channel_t)
+000002cc t set_mode(unsigned char)
+00000362 t tuning()
 00000384 t print_log_menu()
-0000039a T update_throttle_mode()
+00000396 T update_roll_pitch_mode()
 000003a0 t read_battery()
-00000410 T update_yaw_mode()
-0000046e T update_roll_pitch_mode()
-00000638 t init_ardupilot()
-0000071a t update_nav_wp()
-000007c8 t __static_initialization_and_destruction_0(int, int)
-00000842 b g
-00000870 t process_next_command()
-00000884 W Parameters::Parameters()
-000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-00001a00 T loop
+0000041c T update_yaw_mode()
+00000444 T update_throttle_mode()
+00000704 t init_ardupilot()
+000007d8 t __static_initialization_and_destruction_0(int, int)
+00000878 t update_nav_wp()
+000008f6 W Parameters::Parameters()
+000008fa b g
+000009be t update_commands(bool)
+000014f0 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+00001bc0 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log
index b2a63cd62c..930bed821c 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.build.log
@@ -3,40 +3,38 @@
 In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
-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:399: warning: overflow in implicit constant conversion
-autogenerated: At global scope:
-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:121: 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/Attitude.pde:132: warning: 'void reset_hold_I()' defined but not used
+autogenerated:33: warning: 'void init_z_damper()' declared 'static' but never defined
+autogenerated:114: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:106: warning: 'void handle_no_commands()' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:433: 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:183: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:184: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:185: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:180: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:187: warning: 'int heli_get_angle_boost(int)' declared 'static' but never defined
 autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined
-autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/navigation.pde:98: warning: 'void calc_loiter2(int, int)' defined but not used
+autogenerated:236: warning: 'int32_t get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:238: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:240: warning: 'int32_t get_alt_distance(Location*, Location*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/radio.pde:132: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:251: warning: 'int32_t 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:262: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:263: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:270: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:271: warning: 'int read_num_from_serial()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:292: 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/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+autogenerated:265: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:266: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:273: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:274: warning: 'int read_num_from_serial()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/system.pde:486: warning: 'void set_failsafe(boolean)' defined but not used
+autogenerated:289: warning: 'void init_optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:265: warning: 'rc_override' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:266: warning: 'rc_override_active' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:267: warning: 'rc_override_fs_timer' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:488: warning: 'pmTest1' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1768: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/test.pde:34: warning: 'int8_t test_rawgps(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
@@ -77,7 +75,7 @@ In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_Optic
 %% libraries/AP_RangeFinder/RangeFinder.o
 %% libraries/AP_Relay/AP_Relay.o
 %% libraries/DataFlash/DataFlash.o
-In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35:
+In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:36:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
 %% libraries/FastSerial/BetterStream.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt
index 2ec1484a25..ac07817565 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.size.txt
@@ -3,16 +3,17 @@
 00000001 b home_is_set
 00000001 b motor_armed
 00000001 b motor_light
+00000001 b CH7_wp_index
 00000001 b control_mode
 00000001 b gps_watchdog
 00000001 b simple_timer
 00000001 d yaw_tracking
 00000001 b land_complete
 00000001 b throttle_mode
-00000001 b command_may_ID
+00000001 b mavlink_active
+00000001 b prev_nav_index
 00000001 b wp_verify_byte
 00000001 d altitude_sensor
-00000001 b command_must_ID
 00000001 b command_yaw_dir
 00000001 b new_radio_frame
 00000001 b roll_pitch_mode
@@ -24,13 +25,14 @@
 00000001 b old_control_mode
 00000001 b slow_loopCounter
 00000001 b takeoff_complete
-00000001 b command_may_index
+00000001 b command_nav_index
 00000001 b oldSwitchPosition
-00000001 b command_must_index
+00000001 b command_cond_index
 00000001 d ground_start_count
 00000001 b medium_loopCounter
 00000001 b command_yaw_relative
 00000001 d jump
+00000001 b nav_ok
 00000001 b event_id
 00000001 b led_mode
 00000001 b yaw_mode
@@ -45,20 +47,25 @@
 00000002 T userhook_init()
 00000002 b climb_rate
 00000002 b loiter_sum
+00000002 b sonar_rate
+00000002 b angle_boost
 00000002 b event_delay
 00000002 b event_value
 00000002 b event_repeat
 00000002 b loiter_total
+00000002 b manual_boost
 00000002 b nav_throttle
+00000002 b old_baro_alt
 00000002 b x_rate_error
 00000002 b y_rate_error
-00000002 b altitude_rate
 00000002 b gps_fix_count
+00000002 b old_sonar_alt
 00000002 b velocity_land
 00000002 b x_actual_speed
 00000002 b y_actual_speed
 00000002 b loiter_time_max
 00000002 b command_yaw_time
+00000002 b crosstrack_error
 00000002 b event_undo_value
 00000002 b command_yaw_speed
 00000002 b auto_level_counter
@@ -69,6 +76,7 @@
 00000002 b g_gps
 00000002 b airspeed
 00000002 b baro_alt
+00000002 b baro_rate
 00000002 b sonar_alt
 00000002 b arm_motors()::arming_counter
 00000002 r setup_frame(unsigned char, Menu::arg const*)::__c
@@ -82,6 +90,9 @@
 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c
 00000003 r print_enabled(unsigned char)::__c
 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c
+00000003 r report_batt_monitor()::__c
+00000003 r report_batt_monitor()::__c
+00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 d cos_roll_x
 00000004 b land_start
 00000004 b long_error
@@ -93,15 +104,14 @@
 00000004 b sin_pitch_y
 00000004 b wp_distance
 00000004 b abs_pressure
+00000004 b circle_angle
 00000004 b current_amps
-00000004 b old_altitude
 00000004 b original_alt
 00000004 b simple_cos_x
 00000004 b simple_sin_y
 00000004 b current_total
 00000004 b nav_loopTimer
 00000004 d scaleLongDown
-00000004 t test_failsafe(unsigned char, Menu::arg const*)
 00000004 b altitude_error
 00000004 b fast_loopTimer
 00000004 b perf_mon_timer
@@ -152,7 +162,6 @@
 00000004 r setup_compass(unsigned char, Menu::arg const*)::__c
 00000004 r print_log_menu()::__c
 00000004 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
-00000004 r test_adc(unsigned char, Menu::arg const*)::__c
 00000004 V Parameters::Parameters()::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
@@ -171,7 +180,6 @@
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
-00000005 r test_adc(unsigned char, Menu::arg const*)::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
@@ -193,6 +201,7 @@
 00000006 r Log_Read_Mode()::__c
 00000006 r print_log_menu()::__c
 00000006 r print_log_menu()::__c
+00000006 r report_batt_monitor()::__c
 00000006 V Parameters::Parameters()::__c
 00000007 b setup_menu
 00000007 b planner_menu
@@ -205,6 +214,8 @@
 00000007 r report_radio()::__c
 00000007 r report_sonar()::__c
 00000007 r print_enabled(unsigned char)::__c
+00000007 r Log_Read_Nav_Tuning()::__c
+00000007 r Log_Read_Control_Tuning()::__c
 00000007 r test_wp(unsigned char, Menu::arg const*)::__c
 00000007 V Parameters::Parameters()::__c
 00000007 V Parameters::Parameters()::__c
@@ -214,6 +225,7 @@
 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 r print_done()::__c
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -221,8 +233,6 @@
 00000008 r report_tuning()::__c
 00000008 r init_ardupilot()::__c
 00000008 r print_log_menu()::__c
-00000008 r report_batt_monitor()::__c
-00000008 r report_batt_monitor()::__c
 00000008 r test_wp(unsigned char, Menu::arg const*)::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
@@ -230,7 +240,6 @@
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
-00000008 V Parameters::Parameters()::__c
 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000009 r print_switch(unsigned char, unsigned char, bool)::__c
 00000009 r print_log_menu()::__c
@@ -260,7 +269,6 @@
 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 0000000a T piezo_on()
 0000000a T piezo_off()
-0000000a r test_relay(unsigned char, Menu::arg const*)::__c
 0000000a r start_new_log()::__c
 0000000a r print_log_menu()::__c
 0000000a r Log_Read_Startup()::__c
@@ -270,9 +278,10 @@
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
 0000000a T setup
-0000000b r test_relay(unsigned char, Menu::arg const*)::__c
-0000000b r report_batt_monitor()::__c
+0000000b r setup_esc(unsigned char, Menu::arg const*)::__c
 0000000b V Parameters::Parameters()::__c
 0000000c t process_logs(unsigned char, Menu::arg const*)
 0000000c b omega
@@ -282,11 +291,13 @@
 0000000c r print_switch(unsigned char, unsigned char, bool)::__c
 0000000c r report_frame()::__c
 0000000c r setup_factory(unsigned char, Menu::arg const*)::__c
-0000000c r test_baro(unsigned char, Menu::arg const*)::__c
+0000000c r report_version()::__c
+0000000c r report_batt_monitor()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000d r select_logs(unsigned char, Menu::arg const*)::__c
+0000000d r setup_frame(unsigned char, Menu::arg const*)::__c
 0000000d r test_tuning(unsigned char, Menu::arg const*)::__c
 0000000d r test_battery(unsigned char, Menu::arg const*)::__c
 0000000d r startup_ground()::__c
@@ -298,6 +309,8 @@
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
 0000000d B sonar_mode_filter
 0000000e t global destructors keyed to Serial
 0000000e t global constructors keyed to Serial
@@ -313,6 +326,7 @@
 0000000e r setup_mode(unsigned char, Menu::arg const*)::__c
 0000000e r test_sonar(unsigned char, Menu::arg const*)::__c
 0000000e r select_logs(unsigned char, Menu::arg const*)::__c
+0000000e r setup_compass(unsigned char, Menu::arg const*)::__c
 0000000e r print_log_menu()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
@@ -321,6 +335,7 @@
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
+0000000e r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 0000000e r report_batt_monitor()::__c
 0000000e r report_flight_modes()::__c
 0000000e r dump_log(unsigned char, Menu::arg const*)::__c
@@ -332,19 +347,18 @@
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
-0000000e V Parameters::Parameters()::__c
 0000000f b current_loc
-0000000f b next_command
+0000000f b command_nav_queue
+0000000f b command_cond_queue
 0000000f b home
 0000000f b next_WP
 0000000f b prev_WP
 0000000f b guided_WP
 0000000f b target_WP
+0000000f r setup_sonar(unsigned char, Menu::arg const*)::__c
+0000000f r Log_Read_Data()::__c
 0000000f r print_log_menu()::__c
 0000000f r print_log_menu()::__c
-0000000f r report_version()::__c
-0000000f r report_batt_monitor()::__c
-0000000f r test_imu(unsigned char, Menu::arg const*)::__c
 0000000f V AP_IMU_Oilpan::AP_IMU_Oilpan(AP_ADC*, unsigned int)::__c
 00000010 r planner_menu_commands
 00000010 b motor_out
@@ -354,7 +368,6 @@
 00000010 r report_compass()::__c
 00000011 r erase_logs(unsigned char, Menu::arg const*)::__c
 00000011 r zero_eeprom()::__c
-00000011 r update_commands()::__c
 00000012 B Serial
 00000012 B Serial1
 00000012 B Serial3
@@ -368,30 +381,22 @@
 00000012 W AP_VarT<float>::~AP_VarT()
 00000012 W AP_VarT<int>::~AP_VarT()
 00000012 W AP_VarT<signed char>::serialize(void*, unsigned int) const
-00000012 r print_done()::__c
 00000012 r select_logs(unsigned char, Menu::arg const*)::__c
-00000012 r setup_frame(unsigned char, Menu::arg const*)::__c
 00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
-00000013 r setup_compass(unsigned char, Menu::arg const*)::__c
-00000013 r change_command(unsigned char)::__c
-00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 00000014 W AP_VarT<signed char>::unserialize(void*, unsigned int)
 00000014 W AP_VarT<signed char>::cast_to_float() const
 00000014 W AP_VarT<int>::cast_to_float() const
-00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c
-00000014 r test_tri(unsigned char, Menu::arg const*)::__c
 00000015 r init_ardupilot()::__c
-00000015 r Log_Read_Motors()::__c
 00000015 r print_hit_enter()::__c
 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
 00000016 T piezo_beep()
+00000016 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000016 B sonar
 00000018 t setup_tune(unsigned char, Menu::arg const*)
 00000018 t setup_accel(unsigned char, Menu::arg const*)
 00000018 r __menu_name__main_menu
 00000018 W AP_VarT<int>::serialize(void*, unsigned int) const
 0000001a r print_log_menu()::__c
-0000001a r Log_Read_Nav_Tuning()::__c
 0000001c W AP_VarA<float, (unsigned char)6>::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Matrix3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Vector3<float> >::unserialize(void*, unsigned int)
@@ -399,18 +404,21 @@
 0000001c W AP_VarA<float, (unsigned char)6>::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Matrix3<float> >::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Vector3<float> >::serialize(void*, unsigned int) const
+0000001c r print_gyro_offsets()::__c
+0000001c r print_accel_offsets()::__c
+0000001d r report_compass()::__c
+0000001d r Log_Read_Motors()::__c
 0000001d r Log_Read_Attitude()::__c
+0000001d r Log_Read_Performance()::__c
 0000001e r Log_Read_Optflow()::__c
 00000020 t gcs_send_message(ap_message)
 00000020 r test_current(unsigned char, Menu::arg const*)::__c
 00000020 t byte_swap_4
+00000021 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000021 r print_log_menu()::__c
-00000021 r report_compass()::__c
 00000021 r Log_Read_Current()::__c
-00000021 r Log_Read_Performance()::__c
 00000022 t clear_leds()
 00000022 t print_blanks(int)
-00000022 t reset_hold_I()
 00000022 t startup_ground()
 00000022 W AP_Float16::~AP_Float16()
 00000022 W AP_VarA<float, (unsigned char)6>::~AP_VarA()
@@ -419,52 +427,49 @@
 00000022 W AP_VarT<signed char>::~AP_VarT()
 00000022 W AP_VarT<float>::~AP_VarT()
 00000022 W AP_VarT<int>::~AP_VarT()
-00000023 r setup_mode(unsigned char, Menu::arg const*)::__c
-00000023 r print_gyro_offsets()::__c
+00000022 r test_imu(unsigned char, Menu::arg const*)::__c
+00000023 r test_baro(unsigned char, Menu::arg const*)::__c
 00000024 r init_ardupilot()::__c
-00000024 r print_accel_offsets()::__c
 00000026 t print_done()
+00000026 t Log_Write_Data(signed char, float)
 00000026 t print_hit_enter()
 00000026 t Log_Read_Startup()
-00000026 r Log_Read_Control_Tuning()::__c
 00000028 t test_battery(unsigned char, Menu::arg const*)
 00000028 t main_menu_help(unsigned char, Menu::arg const*)
-00000028 t help_log(unsigned char, Menu::arg const*)
 00000028 W AP_VarT<float>::unserialize(void*, unsigned int)
 00000028 W AP_VarT<float>::serialize(void*, unsigned int) const
 00000028 r Log_Read_Cmd()::__c
-00000029 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
+0000002a t _mav_put_int8_t_array
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
 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)
 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char)
+0000002e r init_ardupilot()::__c
 0000002f r test_radio(unsigned char, Menu::arg const*)::__c
-0000002f r init_ardupilot()::__c
+0000002f r setup_factory(unsigned char, Menu::arg const*)::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 r setup_radio(unsigned char, Menu::arg const*)::__c
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
 00000032 r Log_Read_GPS()::__c
 00000033 b pending_status
 00000034 W AP_Float16::serialize(void*, unsigned int) const
-00000034 t _mav_put_int8_t_array
 00000034 t mavlink_msg_statustext_send
-00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c
 00000036 t report_radio()
-00000037 r print_wp(Location*, unsigned char)::__c
-00000038 t init_throttle_cruise()
+00000036 r print_wp(Location*, unsigned char)::__c
 00000038 t send_current_waypoint(mavlink_channel_t)
-00000038 r setup_radio(unsigned char, Menu::arg const*)::__c
-00000038 r setup_factory(unsigned char, Menu::arg const*)::__c
 0000003a t report_gps()
 0000003a t report_imu()
-0000003c W RC_Channel::~RC_Channel()
+0000003c B barometer
 0000003d B g_gps_driver
 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*)
 0000003e W AP_VarT<signed char>::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char)
+00000040 r log_menu_commands
+00000040 t init_throttle_cruise()
 00000040 t read_AHRS()
 00000040 W AP_Float16::unserialize(void*, unsigned int)
 00000040 B adc
@@ -472,15 +477,14 @@
 00000042 t report_sonar()
 00000044 t setup_show(unsigned char, Menu::arg const*)
 00000044 W AP_VarT<int>::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char)
-00000048 t change_command(unsigned char)
+00000044 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
+00000046 W RC_Channel::~RC_Channel()
 00000048 t update_motor_leds()
-00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
 0000004a t send_meminfo(mavlink_channel_t)
 0000004a W AP_VarT<int>::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char)
 0000004c t update_auto_yaw()
 00000050 b mavlink_queue
 00000050 t report_version()
-00000050 r log_menu_commands
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
 00000050 B imu
@@ -489,13 +493,13 @@
 00000056 t readSwitch()
 00000056 t dancing_light()
 00000056 T GCS_MAVLINK::queued_waypoint_send()
-00000057 r help_log(unsigned char, Menu::arg const*)::__c
+0000005a t Log_Write_Data(signed char, long)
 0000005a W AP_VarT<float>::AP_VarT(float, unsigned int, prog_char_t const*, unsigned char)
 0000005c t get_num_logs()
 0000005c t setup_esc(unsigned char, Menu::arg const*)
+0000005e t change_command(unsigned char)
 0000005e t update_GPS_light()
 0000005e T GCS_MAVLINK::_count_parameters()
-00000064 B barometer
 00000064 t mavlink_msg_param_value_send
 00000068 t zero_eeprom()
 00000068 t find_last_log_page(int)
@@ -504,13 +508,12 @@
 0000006c t setup_sonar(unsigned char, Menu::arg const*)
 0000006e T output_min()
 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*)
-00000078 t do_RTL()
 0000007a t setup_factory(unsigned char, Menu::arg const*)
 0000007a t read_control_switch()
 0000007a t report_flight_modes()
-0000007a t test_baro(unsigned char, Menu::arg const*)
+0000007a t do_RTL()
+0000007c t Log_Read_Data()
 0000007c t send_gps_status(mavlink_channel_t)
-0000007e t test_rawgps(unsigned char, Menu::arg const*)
 00000080 T __vector_25
 00000080 T __vector_36
 00000080 T __vector_54
@@ -523,22 +526,18 @@
 00000090 t report_tuning()
 00000092 t test_tuning(unsigned char, Menu::arg const*)
 00000092 T GCS_MAVLINK::queued_param_send()
-00000096 t print_wp(Location*, unsigned char)
 0000009a t planner_gcs(unsigned char, Menu::arg const*)
 0000009a t init_compass()
-0000009a t Log_Read_Motors()
-0000009b B gcs0
-0000009b B gcs3
+0000009c B gcs0
+0000009c B gcs3
 0000009e t setup_mode(unsigned char, Menu::arg const*)
 0000009e t Log_Read_Mode()
-0000009e t Log_Write_Cmd(unsigned char, Location*)
+000000a0 r test_menu_commands
 000000a4 T __vector_26
 000000a4 T __vector_37
 000000a4 T __vector_55
 000000a8 t test_sonar(unsigned char, Menu::arg const*)
-000000aa t Log_Read_Nav_Tuning()
 000000ae t report_frame()
-000000b0 t test_relay(unsigned char, Menu::arg const*)
 000000b2 t erase_logs(unsigned char, Menu::arg const*)
 000000b6 t get_log_boundaries(unsigned char, int&, int&)
 000000b7 B compass
@@ -546,84 +545,82 @@
 000000c2 t test_eedump(unsigned char, Menu::arg const*)
 000000c2 t setup_compass(unsigned char, Menu::arg const*)
 000000c2 t send_radio_out(mavlink_channel_t)
+000000c2 t Log_Read_Motors()
 000000c2 t Log_Read_Attitude()
 000000c4 t get_distance(Location*, Location*)
-000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c
 000000c6 t send_radio_in(mavlink_channel_t)
-000000c6 t Log_Read_Performance()
-000000c6 t test_tri(unsigned char, Menu::arg const*)
 000000c7 B dcm
 000000ca t init_barometer()
+000000cc t Log_Read_Nav_Tuning()
+000000cc t Log_Read_Control_Tuning()
 000000d0 t read_radio()
 000000d0 t get_bearing(Location*, Location*)
 000000d0 t print_switch(unsigned char, unsigned char, bool)
-000000d4 t Log_Read(int, int)
+000000d4 t print_wp(Location*, unsigned char)
 000000d8 t test_radio(unsigned char, Menu::arg const*)
 000000d8 t read_barometer()
-000000dc t test_adc(unsigned char, Menu::arg const*)
+000000dc t Log_Read(int, int)
 000000e0 r setup_menu_commands
-000000e4 t test_radio_pwm(unsigned char, Menu::arg const*)
 000000e4 t Log_Read_Optflow()
 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)
 000000e8 t Log_Read_Current()
-000000ea t Log_Read_Control_Tuning()
 000000ee t report_batt_monitor()
+000000ee t Log_Read_Performance()
 000000f6 t Log_Read_Cmd()
-00000100 r test_menu_commands
+000000fa t calc_loiter_pitch_roll()
 0000010a t mavlink_delay(unsigned long)
 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 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)
-0000011c t get_command_with_index(int)
-0000012c t calc_loiter_pitch_roll()
+00000118 t arm_motors()
+0000011c t get_cmd_with_index(int)
 00000130 t report_compass()
-00000138 t get_stabilize_roll(long)
-00000138 t get_stabilize_pitch(long)
-00000148 t Log_Read_GPS()
+00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
+0000013a t test_baro(unsigned char, Menu::arg const*)
+0000014c t get_stabilize_roll(long)
+0000014c t get_stabilize_pitch(long)
 0000014e t send_servo_out(mavlink_channel_t)
-00000156 t update_commands()
+00000152 t init_home()
+00000156 t Log_Read_GPS()
 0000015c t update_trig()
+0000015c t test_wp(unsigned char, Menu::arg const*)
 00000160 t send_location(mavlink_channel_t)
-00000160 t send_nav_controller_output(mavlink_channel_t)
-00000162 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
+00000164 t set_cmd_with_index(Location, int)
 00000166 t select_logs(unsigned char, Menu::arg const*)
 00000166 t send_vfr_hud(mavlink_channel_t)
-00000168 T GCS_MAVLINK::update()
-0000016c t test_imu(unsigned char, Menu::arg const*)
+00000166 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
 0000016e t send_attitude(mavlink_channel_t)
 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int)
+00000178 t verify_nav_wp()
 000001a2 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int)
 000001a8 t print_radio_values()
-000001be t arm_motors()
-000001cc t start_new_log()
-000001e4 t verify_nav_wp()
+000001b2 t start_new_log()
+000001b8 t send_nav_controller_output(mavlink_channel_t)
+000001ce T GCS_MAVLINK::update()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
-000001ea t init_home()
 00000210 t setup_motors(unsigned char, Menu::arg const*)
-00000216 t set_mode(unsigned char)
+00000210 t test_imu(unsigned char, Menu::arg const*)
 0000021a t send_raw_imu1(mavlink_channel_t)
-0000021c t test_wp(unsigned char, Menu::arg const*)
 00000228 t setup_radio(unsigned char, Menu::arg const*)
-0000022a t send_gps_raw(mavlink_channel_t)
-00000242 t calc_loiter(int, int)
+00000246 t calc_loiter(int, int)
 00000268 t send_raw_imu3(mavlink_channel_t)
-00000330 t tuning()
+0000026a t send_gps_raw(mavlink_channel_t)
+000002cc t set_mode(unsigned char)
+00000362 t tuning()
 00000382 t print_log_menu()
-0000039a T update_throttle_mode()
+00000396 T update_roll_pitch_mode()
 000003a0 t read_battery()
-00000410 T update_yaw_mode()
-0000046e T update_roll_pitch_mode()
-00000636 t init_ardupilot()
-0000071a t update_nav_wp()
-000007c8 t __static_initialization_and_destruction_0(int, int)
-00000842 b g
-00000870 t process_next_command()
-00000884 W Parameters::Parameters()
-000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-00001a00 T loop
+0000041c T update_yaw_mode()
+00000444 T update_throttle_mode()
+00000702 t init_ardupilot()
+000007d8 t __static_initialization_and_destruction_0(int, int)
+00000878 t update_nav_wp()
+000008f6 W Parameters::Parameters()
+000008fa b g
+000009be t update_commands(bool)
+000014f0 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+00001bc0 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log
index 7490666e83..6e171525d0 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.build.log
@@ -3,59 +3,54 @@
 In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
-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:399: warning: overflow in implicit constant conversion
 /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde: In function 'bool mavlink_try_send_message(mavlink_channel_t, ap_message, uint16_t)':
-/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU1' not handled in switch
-/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU2' not handled in switch
-/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU3' not handled in switch
-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: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:121: 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/GCS_Mavlink.pde:300: warning: enumeration value 'MSG_RAW_IMU1' not handled in switch
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:300: warning: enumeration value 'MSG_RAW_IMU2' not handled in switch
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:300: warning: enumeration value 'MSG_RAW_IMU3' not handled in switch
+/root/apm/ardupilot-mega/ArduCopter/Attitude.pde: At global scope:
+/root/apm/ardupilot-mega/ArduCopter/Attitude.pde:132: warning: 'void reset_hold_I()' defined but not used
+autogenerated:33: warning: 'void init_z_damper()' declared 'static' but never defined
+autogenerated:55: warning: 'void send_raw_imu1(mavlink_channel_t)' declared 'static' but never defined
+autogenerated:56: warning: 'void send_raw_imu2(mavlink_channel_t)' declared 'static' but never defined
+autogenerated:57: warning: 'void send_raw_imu3(mavlink_channel_t)' declared 'static' but never defined
+autogenerated:110: warning: 'void Log_Write_Raw()' declared 'static' but never defined
+autogenerated:114: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:106: warning: 'void handle_no_commands()' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:433: 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:183: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:184: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:185: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:180: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:187: warning: 'int heli_get_angle_boost(int)' declared 'static' but never defined
 autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined
-autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
-autogenerated:246: warning: 'void ReadSCP1000()' declared 'static' but never defined
-autogenerated:247: warning: 'void init_barometer()' declared 'static' but never defined
-autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined
-autogenerated:249: warning: 'long int read_barometer()' declared 'static' but never defined
-autogenerated:250: warning: 'void read_airspeed()' declared 'static' but never defined
-autogenerated:251: warning: 'void zero_airspeed()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:263: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:270: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:271: warning: 'int read_num_from_serial()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:292: 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:344: warning: 'old_altitude' defined but not used
-/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:362: warning: 'abs_pressure' defined but not used
-/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:363: warning: 'ground_pressure' defined but not used
-/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:364: warning: 'ground_temperature' defined but not used
-/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:369: warning: 'baro_alt' defined but not used
-/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:13: warning: 'int8_t test_adc(uint8_t, const Menu::arg*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/navigation.pde:98: warning: 'void calc_loiter2(int, int)' defined but not used
+autogenerated:236: warning: 'int32_t get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:238: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:240: warning: 'int32_t get_alt_distance(Location*, Location*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/radio.pde:132: warning: 'void throttle_failsafe(uint16_t)' defined but not used
+autogenerated:249: warning: 'void ReadSCP1000()' declared 'static' but never defined
+autogenerated:250: warning: 'void init_barometer()' declared 'static' but never defined
+autogenerated:251: warning: 'int32_t read_baro_filtered()' declared 'static' but never defined
+autogenerated:252: warning: 'int32_t read_barometer()' declared 'static' but never defined
+autogenerated:253: warning: 'void read_airspeed()' declared 'static' but never defined
+autogenerated:254: warning: 'void zero_airspeed()' declared 'static' but never defined
+autogenerated:265: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:266: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:273: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:274: warning: 'int read_num_from_serial()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/system.pde:486: warning: 'void set_failsafe(boolean)' defined but not used
+autogenerated:289: warning: 'void init_optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:265: warning: 'rc_override' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:266: warning: 'rc_override_active' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:267: warning: 'rc_override_fs_timer' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:365: warning: 'abs_pressure' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:366: warning: 'ground_pressure' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:367: warning: 'ground_temperature' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:377: warning: 'old_sonar_alt' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:488: warning: 'pmTest1' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1768: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
 /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:34: warning: 'int8_t test_rawgps(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
@@ -96,7 +91,7 @@ In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_Optic
 %% libraries/AP_RangeFinder/RangeFinder.o
 %% libraries/AP_Relay/AP_Relay.o
 %% libraries/DataFlash/DataFlash.o
-In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35:
+In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:36:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
 %% libraries/FastSerial/BetterStream.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt
index 824b833eac..ce952b7fa0 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.size.txt
@@ -3,16 +3,17 @@
 00000001 b home_is_set
 00000001 b motor_armed
 00000001 b motor_light
+00000001 b CH7_wp_index
 00000001 b control_mode
 00000001 b gps_watchdog
 00000001 b simple_timer
 00000001 d yaw_tracking
 00000001 b land_complete
 00000001 b throttle_mode
-00000001 b command_may_ID
+00000001 b mavlink_active
+00000001 b prev_nav_index
 00000001 b wp_verify_byte
 00000001 d altitude_sensor
-00000001 b command_must_ID
 00000001 b command_yaw_dir
 00000001 b new_radio_frame
 00000001 b roll_pitch_mode
@@ -24,13 +25,14 @@
 00000001 b old_control_mode
 00000001 b slow_loopCounter
 00000001 b takeoff_complete
-00000001 b command_may_index
+00000001 b command_nav_index
 00000001 b oldSwitchPosition
-00000001 b command_must_index
+00000001 b command_cond_index
 00000001 d ground_start_count
 00000001 b medium_loopCounter
 00000001 b command_yaw_relative
 00000001 d jump
+00000001 b nav_ok
 00000001 b event_id
 00000001 b led_mode
 00000001 b yaw_mode
@@ -45,20 +47,24 @@
 00000002 T userhook_init()
 00000002 b climb_rate
 00000002 b loiter_sum
+00000002 b sonar_rate
+00000002 b angle_boost
 00000002 b event_delay
 00000002 b event_value
 00000002 b event_repeat
 00000002 b loiter_total
+00000002 b manual_boost
 00000002 b nav_throttle
+00000002 b old_baro_alt
 00000002 b x_rate_error
 00000002 b y_rate_error
-00000002 b altitude_rate
 00000002 b gps_fix_count
 00000002 b velocity_land
 00000002 b x_actual_speed
 00000002 b y_actual_speed
 00000002 b loiter_time_max
 00000002 b command_yaw_time
+00000002 b crosstrack_error
 00000002 b event_undo_value
 00000002 b command_yaw_speed
 00000002 b auto_level_counter
@@ -67,6 +73,8 @@
 00000002 r comma
 00000002 b g_gps
 00000002 b airspeed
+00000002 b baro_alt
+00000002 b baro_rate
 00000002 b sonar_alt
 00000002 W AP_IMU_Shim::init_accel(void (*)(unsigned long))
 00000002 W AP_IMU_Shim::init(IMU::Start_style, void (*)(unsigned long))
@@ -83,6 +91,9 @@
 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c
 00000003 r print_enabled(unsigned char)::__c
 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c
+00000003 r report_batt_monitor()::__c
+00000003 r report_batt_monitor()::__c
+00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 d cos_roll_x
 00000004 b land_start
 00000004 b long_error
@@ -93,6 +104,7 @@
 00000004 d scaleLongUp
 00000004 b sin_pitch_y
 00000004 b wp_distance
+00000004 b circle_angle
 00000004 b current_amps
 00000004 b gps_base_alt
 00000004 b original_alt
@@ -101,7 +113,6 @@
 00000004 b current_total
 00000004 b nav_loopTimer
 00000004 d scaleLongDown
-00000004 t test_failsafe(unsigned char, Menu::arg const*)
 00000004 b altitude_error
 00000004 b fast_loopTimer
 00000004 b perf_mon_timer
@@ -190,6 +201,7 @@
 00000006 r Log_Read_Mode()::__c
 00000006 r print_log_menu()::__c
 00000006 r print_log_menu()::__c
+00000006 r report_batt_monitor()::__c
 00000006 V Parameters::Parameters()::__c
 00000007 b setup_menu
 00000007 b planner_menu
@@ -202,6 +214,8 @@
 00000007 r report_radio()::__c
 00000007 r report_sonar()::__c
 00000007 r print_enabled(unsigned char)::__c
+00000007 r Log_Read_Nav_Tuning()::__c
+00000007 r Log_Read_Control_Tuning()::__c
 00000007 r test_wp(unsigned char, Menu::arg const*)::__c
 00000007 V Parameters::Parameters()::__c
 00000007 V Parameters::Parameters()::__c
@@ -211,6 +225,7 @@
 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 r print_done()::__c
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -218,8 +233,6 @@
 00000008 r report_tuning()::__c
 00000008 r init_ardupilot()::__c
 00000008 r print_log_menu()::__c
-00000008 r report_batt_monitor()::__c
-00000008 r report_batt_monitor()::__c
 00000008 r test_wp(unsigned char, Menu::arg const*)::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
@@ -227,7 +240,6 @@
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
-00000008 V Parameters::Parameters()::__c
 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000009 r print_switch(unsigned char, unsigned char, bool)::__c
 00000009 r print_log_menu()::__c
@@ -257,7 +269,6 @@
 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 0000000a T piezo_on()
 0000000a T piezo_off()
-0000000a r test_relay(unsigned char, Menu::arg const*)::__c
 0000000a r start_new_log()::__c
 0000000a r print_log_menu()::__c
 0000000a r Log_Read_Startup()::__c
@@ -267,9 +278,10 @@
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
 0000000a T setup
-0000000b r test_relay(unsigned char, Menu::arg const*)::__c
-0000000b r report_batt_monitor()::__c
+0000000b r setup_esc(unsigned char, Menu::arg const*)::__c
 0000000b V Parameters::Parameters()::__c
 0000000c t setup_accel(unsigned char, Menu::arg const*)
 0000000c t process_logs(unsigned char, Menu::arg const*)
@@ -281,10 +293,13 @@
 0000000c r print_switch(unsigned char, unsigned char, bool)::__c
 0000000c r report_frame()::__c
 0000000c r setup_factory(unsigned char, Menu::arg const*)::__c
+0000000c r report_version()::__c
+0000000c r report_batt_monitor()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000d r select_logs(unsigned char, Menu::arg const*)::__c
+0000000d r setup_frame(unsigned char, Menu::arg const*)::__c
 0000000d r test_tuning(unsigned char, Menu::arg const*)::__c
 0000000d r test_battery(unsigned char, Menu::arg const*)::__c
 0000000d r startup_ground()::__c
@@ -296,6 +311,8 @@
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
 0000000d B sonar_mode_filter
 0000000e t global destructors keyed to Serial
 0000000e t global constructors keyed to Serial
@@ -306,12 +323,13 @@
 0000000e V vtable for AP_VarT<signed char>
 0000000e V vtable for AP_VarT<float>
 0000000e V vtable for AP_VarT<int>
-0000000e r arm_motors()::__c
 0000000e r erase_logs(unsigned char, Menu::arg const*)::__c
 0000000e r setup_mode(unsigned char, Menu::arg const*)::__c
 0000000e r test_sonar(unsigned char, Menu::arg const*)::__c
 0000000e r select_logs(unsigned char, Menu::arg const*)::__c
+0000000e r setup_compass(unsigned char, Menu::arg const*)::__c
 0000000e r print_log_menu()::__c
+0000000e r init_arm_motors()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
@@ -319,6 +337,7 @@
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
+0000000e r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 0000000e r report_batt_monitor()::__c
 0000000e r report_flight_modes()::__c
 0000000e r dump_log(unsigned char, Menu::arg const*)::__c
@@ -330,29 +349,27 @@
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
-0000000e V Parameters::Parameters()::__c
 0000000f b current_loc
-0000000f b next_command
+0000000f b command_nav_queue
+0000000f b command_cond_queue
 0000000f b home
 0000000f b next_WP
 0000000f b prev_WP
 0000000f b guided_WP
 0000000f b target_WP
+0000000f r setup_sonar(unsigned char, Menu::arg const*)::__c
+0000000f r Log_Read_Data()::__c
 0000000f r print_log_menu()::__c
 0000000f r print_log_menu()::__c
-0000000f r report_version()::__c
-0000000f r report_batt_monitor()::__c
-0000000f r test_imu(unsigned char, Menu::arg const*)::__c
 00000010 r planner_menu_commands
 00000010 b motor_out
 00000010 T GCS_MAVLINK::send_message(ap_message)
 00000010 W AP_VarT<float>::cast_to_float() const
 00000010 r test_sonar(unsigned char, Menu::arg const*)::__c
 00000010 r report_compass()::__c
-00000011 r arm_motors()::__c
 00000011 r erase_logs(unsigned char, Menu::arg const*)::__c
 00000011 r zero_eeprom()::__c
-00000011 r update_commands()::__c
+00000011 r init_disarm_motors()::__c
 00000012 B Serial
 00000012 B Serial1
 00000012 B Serial3
@@ -366,79 +383,74 @@
 00000012 W AP_VarT<float>::~AP_VarT()
 00000012 W AP_VarT<int>::~AP_VarT()
 00000012 W AP_VarT<signed char>::serialize(void*, unsigned int) const
-00000012 r print_done()::__c
 00000012 r select_logs(unsigned char, Menu::arg const*)::__c
-00000012 r setup_frame(unsigned char, Menu::arg const*)::__c
 00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
-00000013 r setup_compass(unsigned char, Menu::arg const*)::__c
-00000013 r change_command(unsigned char)::__c
-00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 00000014 W AP_VarT<signed char>::unserialize(void*, unsigned int)
 00000014 W AP_VarT<signed char>::cast_to_float() const
 00000014 W AP_VarT<int>::cast_to_float() const
-00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c
-00000014 r test_tri(unsigned char, Menu::arg const*)::__c
 00000015 r init_ardupilot()::__c
 00000015 r Log_Read_Motors()::__c
 00000015 r print_hit_enter()::__c
 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
 00000016 T piezo_beep()
+00000016 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000016 B adc
 00000016 B sonar
 00000018 t setup_tune(unsigned char, Menu::arg const*)
 00000018 r __menu_name__main_menu
 00000018 W AP_VarT<int>::serialize(void*, unsigned int) const
 0000001a r print_log_menu()::__c
-0000001a r Log_Read_Nav_Tuning()::__c
 0000001c W AP_VarS<Matrix3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Vector3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarT<int>::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Matrix3<float> >::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Vector3<float> >::serialize(void*, unsigned int) const
+0000001c r print_gyro_offsets()::__c
+0000001c r print_accel_offsets()::__c
+0000001d r report_compass()::__c
 0000001d r Log_Read_Attitude()::__c
+0000001d r Log_Read_Performance()::__c
+0000001e t init_disarm_motors()
 0000001e r Log_Read_Optflow()::__c
 00000020 t gcs_send_message(ap_message)
 00000020 r test_current(unsigned char, Menu::arg const*)::__c
 00000020 t byte_swap_4
+00000021 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000021 r print_log_menu()::__c
-00000021 r report_compass()::__c
 00000021 r Log_Read_Current()::__c
-00000021 r Log_Read_Performance()::__c
 00000022 t clear_leds()
 00000022 t print_blanks(int)
-00000022 t reset_hold_I()
 00000022 W AP_Float16::~AP_Float16()
 00000022 W AP_VarS<Matrix3<float> >::~AP_VarS()
 00000022 W AP_VarS<Vector3<float> >::~AP_VarS()
 00000022 W AP_VarT<signed char>::~AP_VarT()
 00000022 W AP_VarT<float>::~AP_VarT()
 00000022 W AP_VarT<int>::~AP_VarT()
-00000023 r setup_mode(unsigned char, Menu::arg const*)::__c
-00000023 r print_gyro_offsets()::__c
+00000022 r test_imu(unsigned char, Menu::arg const*)::__c
 00000024 r init_ardupilot()::__c
-00000024 r print_accel_offsets()::__c
 00000026 t print_done()
+00000026 t Log_Write_Data(signed char, float)
 00000026 t print_hit_enter()
 00000026 t Log_Read_Startup()
-00000026 r Log_Read_Control_Tuning()::__c
 00000028 t test_battery(unsigned char, Menu::arg const*)
 00000028 t main_menu_help(unsigned char, Menu::arg const*)
-00000028 t help_log(unsigned char, Menu::arg const*)
 00000028 W AP_VarT<float>::unserialize(void*, unsigned int)
 00000028 W AP_VarT<float>::serialize(void*, unsigned int) const
 00000028 r Log_Read_Cmd()::__c
-00000029 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
+0000002a t _mav_put_int8_t_array
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
 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)
 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char)
+0000002e r init_ardupilot()::__c
 0000002f r test_radio(unsigned char, Menu::arg const*)::__c
-0000002f r init_ardupilot()::__c
+0000002f r setup_factory(unsigned char, Menu::arg const*)::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 r setup_radio(unsigned char, Menu::arg const*)::__c
 00000030 B imu
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
@@ -446,34 +458,30 @@
 00000033 b pending_status
 00000034 t _MAV_RETURN_float
 00000034 W AP_Float16::serialize(void*, unsigned int) const
-00000034 t _mav_put_int8_t_array
 00000034 t mavlink_msg_statustext_send
-00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c
 00000036 t report_radio()
-00000037 r print_wp(Location*, unsigned char)::__c
-00000038 t init_throttle_cruise()
+00000036 r print_wp(Location*, unsigned char)::__c
 00000038 t send_current_waypoint(mavlink_channel_t)
-00000038 r setup_radio(unsigned char, Menu::arg const*)::__c
-00000038 r setup_factory(unsigned char, Menu::arg const*)::__c
 0000003a t report_gps()
 0000003a t report_imu()
 0000003a B g_gps_driver
-0000003c W RC_Channel::~RC_Channel()
+0000003c t read_AHRS()
 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*)
 0000003e W AP_VarT<signed char>::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char)
+00000040 r log_menu_commands
+00000040 t init_throttle_cruise()
 00000040 W AP_Float16::unserialize(void*, unsigned int)
 00000040 t byte_swap_8
 00000042 t report_sonar()
 00000044 t setup_show(unsigned char, Menu::arg const*)
 00000044 W AP_VarT<int>::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char)
-00000048 t change_command(unsigned char)
+00000044 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
+00000046 W RC_Channel::~RC_Channel()
 00000048 t update_motor_leds()
-00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
 0000004a t send_meminfo(mavlink_channel_t)
 0000004a W AP_VarT<int>::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char)
 0000004c t update_auto_yaw()
 00000050 b mavlink_queue
-00000050 r log_menu_commands
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
 00000052 t report_version()
@@ -483,55 +491,55 @@
 00000056 t readSwitch()
 00000056 t dancing_light()
 00000056 T GCS_MAVLINK::queued_waypoint_send()
-00000057 r help_log(unsigned char, Menu::arg const*)::__c
 00000057 B dcm
+0000005a t Log_Write_Data(signed char, long)
 0000005a W AP_VarT<float>::AP_VarT(float, unsigned int, prog_char_t const*, unsigned char)
 0000005c t get_num_logs()
 0000005c t setup_esc(unsigned char, Menu::arg const*)
+0000005e t change_command(unsigned char)
 0000005e t update_GPS_light()
 0000005e T GCS_MAVLINK::_count_parameters()
 00000064 t print_gyro_offsets()
 00000064 t print_accel_offsets()
 00000064 t mavlink_msg_param_value_send
 00000068 t zero_eeprom()
+00000068 t init_arm_motors()
 00000068 t find_last_log_page(int)
 0000006a T mavlink_send_text(mavlink_channel_t, gcs_severity, char const*)
 0000006a W GCS_MAVLINK::~GCS_MAVLINK()
 0000006c t setup_sonar(unsigned char, Menu::arg const*)
 0000006e T output_min()
 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*)
-00000078 t do_RTL()
 0000007a t setup_factory(unsigned char, Menu::arg const*)
 0000007a t read_control_switch()
 0000007a t report_flight_modes()
+0000007a t do_RTL()
+0000007c t Log_Read_Data()
 0000007c t send_gps_status(mavlink_channel_t)
-0000007e t test_rawgps(unsigned char, Menu::arg const*)
 00000080 T __vector_25
 00000080 T __vector_36
 00000080 T __vector_54
 00000088 t Log_Read_Raw()
 0000008c t setup_frame(unsigned char, Menu::arg const*)
 00000090 t init_compass()
+00000090 r test_menu_commands
 00000090 t dump_log(unsigned char, Menu::arg const*)
 00000092 t test_tuning(unsigned char, Menu::arg const*)
 00000092 t report_tuning()
 00000092 T GCS_MAVLINK::queued_param_send()
-00000096 t print_wp(Location*, unsigned char)
+0000009a t arm_motors()
 0000009a t planner_gcs(unsigned char, Menu::arg const*)
 0000009a t Log_Read_Motors()
-0000009b B gcs0
-0000009b B gcs3
+0000009c B gcs0
+0000009c B gcs3
 0000009e t setup_mode(unsigned char, Menu::arg const*)
-0000009e t Log_Write_Cmd(unsigned char, Location*)
 000000a0 t Log_Read_Mode()
 000000a4 T __vector_26
 000000a4 T __vector_37
 000000a4 T __vector_55
 000000aa t test_sonar(unsigned char, Menu::arg const*)
-000000aa t Log_Read_Nav_Tuning()
 000000ab B compass
 000000ae t report_frame()
-000000b0 t test_relay(unsigned char, Menu::arg const*)
 000000b2 t erase_logs(unsigned char, Menu::arg const*)
 000000b6 t get_log_boundaries(unsigned char, int&, int&)
 000000be t update_events()
@@ -539,76 +547,72 @@
 000000c2 t send_radio_out(mavlink_channel_t)
 000000c2 t Log_Read_Attitude()
 000000c4 t get_distance(Location*, Location*)
-000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c
 000000c6 t test_eedump(unsigned char, Menu::arg const*)
 000000c6 t send_radio_in(mavlink_channel_t)
-000000c6 t Log_Read_Performance()
-000000c6 t test_tri(unsigned char, Menu::arg const*)
 000000d0 t read_radio()
 000000d0 t get_bearing(Location*, Location*)
+000000d0 t Log_Read_Nav_Tuning()
+000000d0 t Log_Read_Control_Tuning()
 000000d2 t print_switch(unsigned char, unsigned char, bool)
-000000d4 t Log_Read(int, int)
+000000d4 t print_wp(Location*, unsigned char)
 000000d8 t test_radio(unsigned char, Menu::arg const*)
-000000e0 r test_menu_commands
+000000dc t Log_Read(int, int)
 000000e0 r setup_menu_commands
-000000e4 t test_radio_pwm(unsigned char, Menu::arg const*)
 000000e4 t Log_Read_Optflow()
 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)
 000000e8 t Log_Read_Current()
-000000ea t Log_Read_Control_Tuning()
 000000ee t report_batt_monitor()
+000000ee t Log_Read_Performance()
 000000f6 t Log_Read_Cmd()
+000000fa t calc_loiter_pitch_roll()
 0000010a t mavlink_delay(unsigned long)
 0000010a t test_gps(unsigned char, Menu::arg const*)
 0000010c t test_current(unsigned char, Menu::arg const*)
-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)
-0000011c t get_command_with_index(int)
-0000012c t calc_loiter_pitch_roll()
-0000012e t arm_motors()
+0000011c t get_cmd_with_index(int)
 00000130 t report_compass()
-00000138 t get_stabilize_roll(long)
-00000138 t get_stabilize_pitch(long)
-00000148 t Log_Read_GPS()
+00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
+0000014c t get_stabilize_roll(long)
+0000014c t get_stabilize_pitch(long)
 0000014e t send_servo_out(mavlink_channel_t)
-00000156 t update_commands()
+00000152 t init_home()
+00000156 t Log_Read_GPS()
 0000015c t update_trig()
 00000160 t send_location(mavlink_channel_t)
-00000160 t send_nav_controller_output(mavlink_channel_t)
+00000160 t test_wp(unsigned char, Menu::arg const*)
+00000164 t set_cmd_with_index(Location, int)
 00000166 t select_logs(unsigned char, Menu::arg const*)
 00000166 t send_vfr_hud(mavlink_channel_t)
-00000168 T GCS_MAVLINK::update()
 0000016e t send_attitude(mavlink_channel_t)
 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int)
 00000178 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int)
-00000184 t test_imu(unsigned char, Menu::arg const*)
-00000188 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
+0000017a t verify_nav_wp()
+0000018c T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
 000001a8 t print_radio_values()
+000001b2 t start_new_log()
+000001b8 t send_nav_controller_output(mavlink_channel_t)
 000001c8 t setup_motors(unsigned char, Menu::arg const*)
-000001cc t start_new_log()
+000001ce T GCS_MAVLINK::update()
 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*)
+00000204 t test_imu(unsigned char, Menu::arg const*)
 00000228 t setup_radio(unsigned char, Menu::arg const*)
-0000022a t send_gps_raw(mavlink_channel_t)
-00000242 t calc_loiter(int, int)
-00000330 t tuning()
+00000246 t calc_loiter(int, int)
+0000026a t send_gps_raw(mavlink_channel_t)
+000002cc t set_mode(unsigned char)
+00000362 t tuning()
 00000384 t print_log_menu()
-0000039a T update_throttle_mode()
+00000396 T update_roll_pitch_mode()
 000003a0 t read_battery()
-00000410 T update_yaw_mode()
-0000046e T update_roll_pitch_mode()
-00000598 t __static_initialization_and_destruction_0(int, int)
-0000061e t init_ardupilot()
-0000071a t update_nav_wp()
-00000842 b g
-00000870 t process_next_command()
-00000884 W Parameters::Parameters()
-00001372 T loop
-00001604 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+0000041c T update_yaw_mode()
+00000444 T update_throttle_mode()
+000005a8 t __static_initialization_and_destruction_0(int, int)
+00000640 t init_ardupilot()
+00000878 t update_nav_wp()
+000008f6 W Parameters::Parameters()
+000008fa b g
+000009be t update_commands(bool)
+000017b8 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+000017d4 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log
index 7490666e83..6e171525d0 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.build.log
@@ -3,59 +3,54 @@
 In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
-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:399: warning: overflow in implicit constant conversion
 /root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde: In function 'bool mavlink_try_send_message(mavlink_channel_t, ap_message, uint16_t)':
-/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU1' not handled in switch
-/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU2' not handled in switch
-/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:296: warning: enumeration value 'MSG_RAW_IMU3' not handled in switch
-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: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:121: 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/GCS_Mavlink.pde:300: warning: enumeration value 'MSG_RAW_IMU1' not handled in switch
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:300: warning: enumeration value 'MSG_RAW_IMU2' not handled in switch
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:300: warning: enumeration value 'MSG_RAW_IMU3' not handled in switch
+/root/apm/ardupilot-mega/ArduCopter/Attitude.pde: At global scope:
+/root/apm/ardupilot-mega/ArduCopter/Attitude.pde:132: warning: 'void reset_hold_I()' defined but not used
+autogenerated:33: warning: 'void init_z_damper()' declared 'static' but never defined
+autogenerated:55: warning: 'void send_raw_imu1(mavlink_channel_t)' declared 'static' but never defined
+autogenerated:56: warning: 'void send_raw_imu2(mavlink_channel_t)' declared 'static' but never defined
+autogenerated:57: warning: 'void send_raw_imu3(mavlink_channel_t)' declared 'static' but never defined
+autogenerated:110: warning: 'void Log_Write_Raw()' declared 'static' but never defined
+autogenerated:114: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:106: warning: 'void handle_no_commands()' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:433: 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:183: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:184: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:185: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:180: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:187: warning: 'int heli_get_angle_boost(int)' declared 'static' but never defined
 autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined
-autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
-autogenerated:246: warning: 'void ReadSCP1000()' declared 'static' but never defined
-autogenerated:247: warning: 'void init_barometer()' declared 'static' but never defined
-autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined
-autogenerated:249: warning: 'long int read_barometer()' declared 'static' but never defined
-autogenerated:250: warning: 'void read_airspeed()' declared 'static' but never defined
-autogenerated:251: warning: 'void zero_airspeed()' declared 'static' but never defined
-autogenerated:262: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:263: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:270: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:271: warning: 'int read_num_from_serial()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:292: 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:344: warning: 'old_altitude' defined but not used
-/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:362: warning: 'abs_pressure' defined but not used
-/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:363: warning: 'ground_pressure' defined but not used
-/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:364: warning: 'ground_temperature' defined but not used
-/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:369: warning: 'baro_alt' defined but not used
-/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:13: warning: 'int8_t test_adc(uint8_t, const Menu::arg*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/navigation.pde:98: warning: 'void calc_loiter2(int, int)' defined but not used
+autogenerated:236: warning: 'int32_t get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:238: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:240: warning: 'int32_t get_alt_distance(Location*, Location*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/radio.pde:132: warning: 'void throttle_failsafe(uint16_t)' defined but not used
+autogenerated:249: warning: 'void ReadSCP1000()' declared 'static' but never defined
+autogenerated:250: warning: 'void init_barometer()' declared 'static' but never defined
+autogenerated:251: warning: 'int32_t read_baro_filtered()' declared 'static' but never defined
+autogenerated:252: warning: 'int32_t read_barometer()' declared 'static' but never defined
+autogenerated:253: warning: 'void read_airspeed()' declared 'static' but never defined
+autogenerated:254: warning: 'void zero_airspeed()' declared 'static' but never defined
+autogenerated:265: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:266: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:273: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:274: warning: 'int read_num_from_serial()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/system.pde:486: warning: 'void set_failsafe(boolean)' defined but not used
+autogenerated:289: warning: 'void init_optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:265: warning: 'rc_override' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:266: warning: 'rc_override_active' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:267: warning: 'rc_override_fs_timer' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:365: warning: 'abs_pressure' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:366: warning: 'ground_pressure' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:367: warning: 'ground_temperature' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:377: warning: 'old_sonar_alt' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:488: warning: 'pmTest1' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1768: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
 /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:34: warning: 'int8_t test_rawgps(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
@@ -96,7 +91,7 @@ In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_Optic
 %% libraries/AP_RangeFinder/RangeFinder.o
 %% libraries/AP_Relay/AP_Relay.o
 %% libraries/DataFlash/DataFlash.o
-In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35:
+In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:36:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
 %% libraries/FastSerial/BetterStream.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt
index 1fa3c06703..b61e4b36b8 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.size.txt
@@ -3,16 +3,17 @@
 00000001 b home_is_set
 00000001 b motor_armed
 00000001 b motor_light
+00000001 b CH7_wp_index
 00000001 b control_mode
 00000001 b gps_watchdog
 00000001 b simple_timer
 00000001 d yaw_tracking
 00000001 b land_complete
 00000001 b throttle_mode
-00000001 b command_may_ID
+00000001 b mavlink_active
+00000001 b prev_nav_index
 00000001 b wp_verify_byte
 00000001 d altitude_sensor
-00000001 b command_must_ID
 00000001 b command_yaw_dir
 00000001 b new_radio_frame
 00000001 b roll_pitch_mode
@@ -24,13 +25,14 @@
 00000001 b old_control_mode
 00000001 b slow_loopCounter
 00000001 b takeoff_complete
-00000001 b command_may_index
+00000001 b command_nav_index
 00000001 b oldSwitchPosition
-00000001 b command_must_index
+00000001 b command_cond_index
 00000001 d ground_start_count
 00000001 b medium_loopCounter
 00000001 b command_yaw_relative
 00000001 d jump
+00000001 b nav_ok
 00000001 b event_id
 00000001 b led_mode
 00000001 b yaw_mode
@@ -45,20 +47,24 @@
 00000002 T userhook_init()
 00000002 b climb_rate
 00000002 b loiter_sum
+00000002 b sonar_rate
+00000002 b angle_boost
 00000002 b event_delay
 00000002 b event_value
 00000002 b event_repeat
 00000002 b loiter_total
+00000002 b manual_boost
 00000002 b nav_throttle
+00000002 b old_baro_alt
 00000002 b x_rate_error
 00000002 b y_rate_error
-00000002 b altitude_rate
 00000002 b gps_fix_count
 00000002 b velocity_land
 00000002 b x_actual_speed
 00000002 b y_actual_speed
 00000002 b loiter_time_max
 00000002 b command_yaw_time
+00000002 b crosstrack_error
 00000002 b event_undo_value
 00000002 b command_yaw_speed
 00000002 b auto_level_counter
@@ -67,6 +73,8 @@
 00000002 r comma
 00000002 b g_gps
 00000002 b airspeed
+00000002 b baro_alt
+00000002 b baro_rate
 00000002 b sonar_alt
 00000002 W AP_IMU_Shim::init_accel(void (*)(unsigned long))
 00000002 W AP_IMU_Shim::init(IMU::Start_style, void (*)(unsigned long))
@@ -83,6 +91,9 @@
 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c
 00000003 r print_enabled(unsigned char)::__c
 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c
+00000003 r report_batt_monitor()::__c
+00000003 r report_batt_monitor()::__c
+00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 d cos_roll_x
 00000004 b land_start
 00000004 b long_error
@@ -93,6 +104,7 @@
 00000004 d scaleLongUp
 00000004 b sin_pitch_y
 00000004 b wp_distance
+00000004 b circle_angle
 00000004 b current_amps
 00000004 b gps_base_alt
 00000004 b original_alt
@@ -101,7 +113,6 @@
 00000004 b current_total
 00000004 b nav_loopTimer
 00000004 d scaleLongDown
-00000004 t test_failsafe(unsigned char, Menu::arg const*)
 00000004 b altitude_error
 00000004 b fast_loopTimer
 00000004 b perf_mon_timer
@@ -190,6 +201,7 @@
 00000006 r Log_Read_Mode()::__c
 00000006 r print_log_menu()::__c
 00000006 r print_log_menu()::__c
+00000006 r report_batt_monitor()::__c
 00000006 V Parameters::Parameters()::__c
 00000007 b setup_menu
 00000007 b planner_menu
@@ -202,6 +214,8 @@
 00000007 r report_radio()::__c
 00000007 r report_sonar()::__c
 00000007 r print_enabled(unsigned char)::__c
+00000007 r Log_Read_Nav_Tuning()::__c
+00000007 r Log_Read_Control_Tuning()::__c
 00000007 r test_wp(unsigned char, Menu::arg const*)::__c
 00000007 V Parameters::Parameters()::__c
 00000007 V Parameters::Parameters()::__c
@@ -211,6 +225,7 @@
 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 r print_done()::__c
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -218,8 +233,6 @@
 00000008 r report_tuning()::__c
 00000008 r init_ardupilot()::__c
 00000008 r print_log_menu()::__c
-00000008 r report_batt_monitor()::__c
-00000008 r report_batt_monitor()::__c
 00000008 r test_wp(unsigned char, Menu::arg const*)::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
@@ -227,7 +240,6 @@
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
-00000008 V Parameters::Parameters()::__c
 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000009 r print_switch(unsigned char, unsigned char, bool)::__c
 00000009 r print_log_menu()::__c
@@ -257,7 +269,6 @@
 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 0000000a T piezo_on()
 0000000a T piezo_off()
-0000000a r test_relay(unsigned char, Menu::arg const*)::__c
 0000000a r start_new_log()::__c
 0000000a r print_log_menu()::__c
 0000000a r Log_Read_Startup()::__c
@@ -267,9 +278,10 @@
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
 0000000a T setup
-0000000b r test_relay(unsigned char, Menu::arg const*)::__c
-0000000b r report_batt_monitor()::__c
+0000000b r setup_esc(unsigned char, Menu::arg const*)::__c
 0000000b V Parameters::Parameters()::__c
 0000000c t setup_accel(unsigned char, Menu::arg const*)
 0000000c t process_logs(unsigned char, Menu::arg const*)
@@ -281,10 +293,13 @@
 0000000c r print_switch(unsigned char, unsigned char, bool)::__c
 0000000c r report_frame()::__c
 0000000c r setup_factory(unsigned char, Menu::arg const*)::__c
+0000000c r report_version()::__c
+0000000c r report_batt_monitor()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000d r select_logs(unsigned char, Menu::arg const*)::__c
+0000000d r setup_frame(unsigned char, Menu::arg const*)::__c
 0000000d r test_tuning(unsigned char, Menu::arg const*)::__c
 0000000d r test_battery(unsigned char, Menu::arg const*)::__c
 0000000d r startup_ground()::__c
@@ -296,6 +311,8 @@
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
 0000000d B sonar_mode_filter
 0000000e t global destructors keyed to Serial
 0000000e t global constructors keyed to Serial
@@ -306,12 +323,13 @@
 0000000e V vtable for AP_VarT<signed char>
 0000000e V vtable for AP_VarT<float>
 0000000e V vtable for AP_VarT<int>
-0000000e r arm_motors()::__c
 0000000e r erase_logs(unsigned char, Menu::arg const*)::__c
 0000000e r setup_mode(unsigned char, Menu::arg const*)::__c
 0000000e r test_sonar(unsigned char, Menu::arg const*)::__c
 0000000e r select_logs(unsigned char, Menu::arg const*)::__c
+0000000e r setup_compass(unsigned char, Menu::arg const*)::__c
 0000000e r print_log_menu()::__c
+0000000e r init_arm_motors()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
@@ -319,6 +337,7 @@
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
+0000000e r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 0000000e r report_batt_monitor()::__c
 0000000e r report_flight_modes()::__c
 0000000e r dump_log(unsigned char, Menu::arg const*)::__c
@@ -330,29 +349,27 @@
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
-0000000e V Parameters::Parameters()::__c
 0000000f b current_loc
-0000000f b next_command
+0000000f b command_nav_queue
+0000000f b command_cond_queue
 0000000f b home
 0000000f b next_WP
 0000000f b prev_WP
 0000000f b guided_WP
 0000000f b target_WP
+0000000f r setup_sonar(unsigned char, Menu::arg const*)::__c
+0000000f r Log_Read_Data()::__c
 0000000f r print_log_menu()::__c
 0000000f r print_log_menu()::__c
-0000000f r report_version()::__c
-0000000f r report_batt_monitor()::__c
-0000000f r test_imu(unsigned char, Menu::arg const*)::__c
 00000010 r planner_menu_commands
 00000010 b motor_out
 00000010 T GCS_MAVLINK::send_message(ap_message)
 00000010 W AP_VarT<float>::cast_to_float() const
 00000010 r test_sonar(unsigned char, Menu::arg const*)::__c
 00000010 r report_compass()::__c
-00000011 r arm_motors()::__c
 00000011 r erase_logs(unsigned char, Menu::arg const*)::__c
 00000011 r zero_eeprom()::__c
-00000011 r update_commands()::__c
+00000011 r init_disarm_motors()::__c
 00000012 B Serial
 00000012 B Serial1
 00000012 B Serial3
@@ -366,79 +383,74 @@
 00000012 W AP_VarT<float>::~AP_VarT()
 00000012 W AP_VarT<int>::~AP_VarT()
 00000012 W AP_VarT<signed char>::serialize(void*, unsigned int) const
-00000012 r print_done()::__c
 00000012 r select_logs(unsigned char, Menu::arg const*)::__c
-00000012 r setup_frame(unsigned char, Menu::arg const*)::__c
 00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
-00000013 r setup_compass(unsigned char, Menu::arg const*)::__c
-00000013 r change_command(unsigned char)::__c
-00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 00000014 W AP_VarT<signed char>::unserialize(void*, unsigned int)
 00000014 W AP_VarT<signed char>::cast_to_float() const
 00000014 W AP_VarT<int>::cast_to_float() const
-00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c
-00000014 r test_tri(unsigned char, Menu::arg const*)::__c
 00000015 r init_ardupilot()::__c
 00000015 r Log_Read_Motors()::__c
 00000015 r print_hit_enter()::__c
 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
 00000016 T piezo_beep()
+00000016 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000016 B adc
 00000016 B sonar
 00000018 t setup_tune(unsigned char, Menu::arg const*)
 00000018 r __menu_name__main_menu
 00000018 W AP_VarT<int>::serialize(void*, unsigned int) const
 0000001a r print_log_menu()::__c
-0000001a r Log_Read_Nav_Tuning()::__c
 0000001c W AP_VarS<Matrix3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Vector3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarT<int>::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Matrix3<float> >::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Vector3<float> >::serialize(void*, unsigned int) const
+0000001c r print_gyro_offsets()::__c
+0000001c r print_accel_offsets()::__c
+0000001d r report_compass()::__c
 0000001d r Log_Read_Attitude()::__c
+0000001d r Log_Read_Performance()::__c
+0000001e t init_disarm_motors()
 0000001e r Log_Read_Optflow()::__c
 00000020 t gcs_send_message(ap_message)
 00000020 r test_current(unsigned char, Menu::arg const*)::__c
 00000020 t byte_swap_4
+00000021 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000021 r print_log_menu()::__c
-00000021 r report_compass()::__c
 00000021 r Log_Read_Current()::__c
-00000021 r Log_Read_Performance()::__c
 00000022 t clear_leds()
 00000022 t print_blanks(int)
-00000022 t reset_hold_I()
 00000022 W AP_Float16::~AP_Float16()
 00000022 W AP_VarS<Matrix3<float> >::~AP_VarS()
 00000022 W AP_VarS<Vector3<float> >::~AP_VarS()
 00000022 W AP_VarT<signed char>::~AP_VarT()
 00000022 W AP_VarT<float>::~AP_VarT()
 00000022 W AP_VarT<int>::~AP_VarT()
-00000023 r setup_mode(unsigned char, Menu::arg const*)::__c
-00000023 r print_gyro_offsets()::__c
+00000022 r test_imu(unsigned char, Menu::arg const*)::__c
 00000024 r init_ardupilot()::__c
-00000024 r print_accel_offsets()::__c
 00000026 t print_done()
+00000026 t Log_Write_Data(signed char, float)
 00000026 t print_hit_enter()
 00000026 t Log_Read_Startup()
-00000026 r Log_Read_Control_Tuning()::__c
 00000028 t test_battery(unsigned char, Menu::arg const*)
 00000028 t main_menu_help(unsigned char, Menu::arg const*)
-00000028 t help_log(unsigned char, Menu::arg const*)
 00000028 W AP_VarT<float>::unserialize(void*, unsigned int)
 00000028 W AP_VarT<float>::serialize(void*, unsigned int) const
 00000028 r Log_Read_Cmd()::__c
-00000029 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
+0000002a t _mav_put_int8_t_array
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
 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)
 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char)
+0000002e r init_ardupilot()::__c
 0000002f r test_radio(unsigned char, Menu::arg const*)::__c
-0000002f r init_ardupilot()::__c
+0000002f r setup_factory(unsigned char, Menu::arg const*)::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 r setup_radio(unsigned char, Menu::arg const*)::__c
 00000030 B imu
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
@@ -446,35 +458,31 @@
 00000033 b pending_status
 00000034 t _MAV_RETURN_float
 00000034 W AP_Float16::serialize(void*, unsigned int) const
-00000034 t _mav_put_int8_t_array
 00000034 t mavlink_msg_statustext_send
-00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c
 00000036 t report_radio()
-00000037 r print_wp(Location*, unsigned char)::__c
-00000038 t init_throttle_cruise()
+00000036 r print_wp(Location*, unsigned char)::__c
 00000038 t send_current_waypoint(mavlink_channel_t)
-00000038 r setup_radio(unsigned char, Menu::arg const*)::__c
-00000038 r setup_factory(unsigned char, Menu::arg const*)::__c
 0000003a t report_gps()
 0000003a t report_imu()
 0000003a B g_gps_driver
-0000003c W RC_Channel::~RC_Channel()
+0000003c t read_AHRS()
 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*)
 0000003e W AP_VarT<signed char>::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char)
+00000040 r log_menu_commands
+00000040 t init_throttle_cruise()
 00000040 W AP_Float16::unserialize(void*, unsigned int)
 00000040 t byte_swap_8
 00000042 t report_sonar()
 00000044 t setup_show(unsigned char, Menu::arg const*)
 00000044 W AP_VarT<int>::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char)
-00000048 t change_command(unsigned char)
+00000044 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
+00000046 W RC_Channel::~RC_Channel()
 00000048 t update_motor_leds()
-00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
 0000004a t send_meminfo(mavlink_channel_t)
 0000004a W AP_VarT<int>::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char)
 0000004c t update_auto_yaw()
 00000050 b mavlink_queue
 00000050 t report_version()
-00000050 r log_menu_commands
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
 00000052 W AP_IMU_Shim::update()
@@ -483,29 +491,31 @@
 00000056 t readSwitch()
 00000056 t dancing_light()
 00000056 T GCS_MAVLINK::queued_waypoint_send()
-00000057 r help_log(unsigned char, Menu::arg const*)::__c
 00000057 B dcm
+0000005a t Log_Write_Data(signed char, long)
 0000005a W AP_VarT<float>::AP_VarT(float, unsigned int, prog_char_t const*, unsigned char)
 0000005c t get_num_logs()
 0000005c t setup_esc(unsigned char, Menu::arg const*)
+0000005e t change_command(unsigned char)
 0000005e t update_GPS_light()
 0000005e T GCS_MAVLINK::_count_parameters()
 00000064 t print_gyro_offsets()
 00000064 t print_accel_offsets()
 00000064 t mavlink_msg_param_value_send
 00000068 t zero_eeprom()
+00000068 t init_arm_motors()
 00000068 t find_last_log_page(int)
 0000006a T mavlink_send_text(mavlink_channel_t, gcs_severity, char const*)
 0000006a W GCS_MAVLINK::~GCS_MAVLINK()
 0000006c t setup_sonar(unsigned char, Menu::arg const*)
 0000006e T output_min()
 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*)
-00000078 t do_RTL()
 0000007a t setup_factory(unsigned char, Menu::arg const*)
 0000007a t read_control_switch()
 0000007a t report_flight_modes()
+0000007a t do_RTL()
+0000007c t Log_Read_Data()
 0000007c t send_gps_status(mavlink_channel_t)
-0000007e t test_rawgps(unsigned char, Menu::arg const*)
 00000080 T __vector_25
 00000080 T __vector_36
 00000080 T __vector_54
@@ -514,24 +524,22 @@
 0000008e t dump_log(unsigned char, Menu::arg const*)
 00000090 t init_compass()
 00000090 t report_tuning()
+00000090 r test_menu_commands
 00000092 t test_tuning(unsigned char, Menu::arg const*)
 00000092 T GCS_MAVLINK::queued_param_send()
-00000096 t print_wp(Location*, unsigned char)
+0000009a t arm_motors()
 0000009a t planner_gcs(unsigned char, Menu::arg const*)
 0000009a t Log_Read_Motors()
-0000009b B gcs0
-0000009b B gcs3
+0000009c B gcs0
+0000009c B gcs3
 0000009e t setup_mode(unsigned char, Menu::arg const*)
 0000009e t Log_Read_Mode()
-0000009e t Log_Write_Cmd(unsigned char, Location*)
 000000a4 T __vector_26
 000000a4 T __vector_37
 000000a4 T __vector_55
 000000a8 t test_sonar(unsigned char, Menu::arg const*)
-000000aa t Log_Read_Nav_Tuning()
 000000ab B compass
 000000ae t report_frame()
-000000b0 t test_relay(unsigned char, Menu::arg const*)
 000000b2 t erase_logs(unsigned char, Menu::arg const*)
 000000b6 t get_log_boundaries(unsigned char, int&, int&)
 000000be t update_events()
@@ -540,75 +548,71 @@
 000000c2 t send_radio_out(mavlink_channel_t)
 000000c2 t Log_Read_Attitude()
 000000c4 t get_distance(Location*, Location*)
-000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c
 000000c6 t send_radio_in(mavlink_channel_t)
-000000c6 t Log_Read_Performance()
-000000c6 t test_tri(unsigned char, Menu::arg const*)
+000000cc t Log_Read_Nav_Tuning()
+000000cc t Log_Read_Control_Tuning()
 000000d0 t read_radio()
 000000d0 t get_bearing(Location*, Location*)
 000000d0 t print_switch(unsigned char, unsigned char, bool)
-000000d4 t Log_Read(int, int)
+000000d4 t print_wp(Location*, unsigned char)
 000000d8 t test_radio(unsigned char, Menu::arg const*)
-000000e0 r test_menu_commands
+000000dc t Log_Read(int, int)
 000000e0 r setup_menu_commands
-000000e4 t test_radio_pwm(unsigned char, Menu::arg const*)
 000000e4 t Log_Read_Optflow()
 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)
 000000e8 t Log_Read_Current()
-000000ea t Log_Read_Control_Tuning()
 000000ee t report_batt_monitor()
+000000ee t Log_Read_Performance()
 000000f6 t Log_Read_Cmd()
+000000fa t calc_loiter_pitch_roll()
 0000010a t mavlink_delay(unsigned long)
 0000010a t test_gps(unsigned char, Menu::arg const*)
 0000010c t test_current(unsigned char, Menu::arg const*)
-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)
-0000011c t get_command_with_index(int)
-0000012c t calc_loiter_pitch_roll()
-0000012e t arm_motors()
+0000011c t get_cmd_with_index(int)
 00000130 t report_compass()
-00000138 t get_stabilize_roll(long)
-00000138 t get_stabilize_pitch(long)
-00000148 t Log_Read_GPS()
+00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
+0000014c t get_stabilize_roll(long)
+0000014c t get_stabilize_pitch(long)
 0000014e t send_servo_out(mavlink_channel_t)
-00000156 t update_commands()
+00000152 t init_home()
+00000156 t Log_Read_GPS()
 0000015c t update_trig()
+0000015c t test_wp(unsigned char, Menu::arg const*)
 00000160 t send_location(mavlink_channel_t)
-00000160 t send_nav_controller_output(mavlink_channel_t)
+00000164 t set_cmd_with_index(Location, int)
 00000166 t select_logs(unsigned char, Menu::arg const*)
 00000166 t send_vfr_hud(mavlink_channel_t)
-00000168 T GCS_MAVLINK::update()
 0000016e t send_attitude(mavlink_channel_t)
 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int)
+00000178 t verify_nav_wp()
 00000178 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int)
-00000184 t test_imu(unsigned char, Menu::arg const*)
-00000188 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
+0000018c T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
 000001a8 t print_radio_values()
+000001b2 t start_new_log()
+000001b8 t send_nav_controller_output(mavlink_channel_t)
 000001c8 t setup_motors(unsigned char, Menu::arg const*)
-000001cc t start_new_log()
-000001e4 t verify_nav_wp()
+000001ce T GCS_MAVLINK::update()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
-000001ea t init_home()
-00000216 t set_mode(unsigned char)
-0000021c t test_wp(unsigned char, Menu::arg const*)
+00000204 t test_imu(unsigned char, Menu::arg const*)
 00000228 t setup_radio(unsigned char, Menu::arg const*)
-0000022a t send_gps_raw(mavlink_channel_t)
-00000242 t calc_loiter(int, int)
-00000330 t tuning()
+00000246 t calc_loiter(int, int)
+0000026a t send_gps_raw(mavlink_channel_t)
+000002cc t set_mode(unsigned char)
+00000362 t tuning()
 00000382 t print_log_menu()
-0000039a T update_throttle_mode()
+00000396 T update_roll_pitch_mode()
 000003a0 t read_battery()
-00000410 T update_yaw_mode()
-0000046e T update_roll_pitch_mode()
-00000598 t __static_initialization_and_destruction_0(int, int)
-0000061c t init_ardupilot()
-0000071a t update_nav_wp()
-00000842 b g
-00000870 t process_next_command()
-00000884 W Parameters::Parameters()
-00001372 T loop
-00001604 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+0000041c T update_yaw_mode()
+00000444 T update_throttle_mode()
+000005a8 t __static_initialization_and_destruction_0(int, int)
+0000063e t init_ardupilot()
+00000878 t update_nav_wp()
+000008f6 W Parameters::Parameters()
+000008fa b g
+000009be t update_commands(bool)
+000017b8 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+000017d4 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log
index b2a63cd62c..930bed821c 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.build.log
@@ -3,40 +3,38 @@
 In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
-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:399: warning: overflow in implicit constant conversion
-autogenerated: At global scope:
-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:121: 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/Attitude.pde:132: warning: 'void reset_hold_I()' defined but not used
+autogenerated:33: warning: 'void init_z_damper()' declared 'static' but never defined
+autogenerated:114: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:106: warning: 'void handle_no_commands()' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:433: 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:183: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:184: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:185: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:180: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:187: warning: 'int heli_get_angle_boost(int)' declared 'static' but never defined
 autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined
-autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/navigation.pde:98: warning: 'void calc_loiter2(int, int)' defined but not used
+autogenerated:236: warning: 'int32_t get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:238: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:240: warning: 'int32_t get_alt_distance(Location*, Location*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/radio.pde:132: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:251: warning: 'int32_t 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:262: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:263: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:270: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:271: warning: 'int read_num_from_serial()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:292: 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/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+autogenerated:265: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:266: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:273: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:274: warning: 'int read_num_from_serial()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/system.pde:486: warning: 'void set_failsafe(boolean)' defined but not used
+autogenerated:289: warning: 'void init_optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:265: warning: 'rc_override' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:266: warning: 'rc_override_active' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:267: warning: 'rc_override_fs_timer' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:488: warning: 'pmTest1' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1768: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/test.pde:34: warning: 'int8_t test_rawgps(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
@@ -77,7 +75,7 @@ In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_Optic
 %% libraries/AP_RangeFinder/RangeFinder.o
 %% libraries/AP_Relay/AP_Relay.o
 %% libraries/DataFlash/DataFlash.o
-In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35:
+In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:36:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
 %% libraries/FastSerial/BetterStream.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt
index f0da4a64e5..ac89d2a8a3 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.size.txt
@@ -3,16 +3,17 @@
 00000001 b home_is_set
 00000001 b motor_armed
 00000001 b motor_light
+00000001 b CH7_wp_index
 00000001 b control_mode
 00000001 b gps_watchdog
 00000001 b simple_timer
 00000001 d yaw_tracking
 00000001 b land_complete
 00000001 b throttle_mode
-00000001 b command_may_ID
+00000001 b mavlink_active
+00000001 b prev_nav_index
 00000001 b wp_verify_byte
 00000001 d altitude_sensor
-00000001 b command_must_ID
 00000001 b command_yaw_dir
 00000001 b new_radio_frame
 00000001 b roll_pitch_mode
@@ -24,13 +25,14 @@
 00000001 b old_control_mode
 00000001 b slow_loopCounter
 00000001 b takeoff_complete
-00000001 b command_may_index
+00000001 b command_nav_index
 00000001 b oldSwitchPosition
-00000001 b command_must_index
+00000001 b command_cond_index
 00000001 d ground_start_count
 00000001 b medium_loopCounter
 00000001 b command_yaw_relative
 00000001 d jump
+00000001 b nav_ok
 00000001 b event_id
 00000001 b led_mode
 00000001 b yaw_mode
@@ -45,20 +47,25 @@
 00000002 T userhook_init()
 00000002 b climb_rate
 00000002 b loiter_sum
+00000002 b sonar_rate
+00000002 b angle_boost
 00000002 b event_delay
 00000002 b event_value
 00000002 b event_repeat
 00000002 b loiter_total
+00000002 b manual_boost
 00000002 b nav_throttle
+00000002 b old_baro_alt
 00000002 b x_rate_error
 00000002 b y_rate_error
-00000002 b altitude_rate
 00000002 b gps_fix_count
+00000002 b old_sonar_alt
 00000002 b velocity_land
 00000002 b x_actual_speed
 00000002 b y_actual_speed
 00000002 b loiter_time_max
 00000002 b command_yaw_time
+00000002 b crosstrack_error
 00000002 b event_undo_value
 00000002 b command_yaw_speed
 00000002 b auto_level_counter
@@ -69,6 +76,7 @@
 00000002 b g_gps
 00000002 b airspeed
 00000002 b baro_alt
+00000002 b baro_rate
 00000002 b sonar_alt
 00000002 b arm_motors()::arming_counter
 00000002 r setup_frame(unsigned char, Menu::arg const*)::__c
@@ -82,6 +90,9 @@
 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c
 00000003 r print_enabled(unsigned char)::__c
 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c
+00000003 r report_batt_monitor()::__c
+00000003 r report_batt_monitor()::__c
+00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 d cos_roll_x
 00000004 b land_start
 00000004 b long_error
@@ -93,15 +104,14 @@
 00000004 b sin_pitch_y
 00000004 b wp_distance
 00000004 b abs_pressure
+00000004 b circle_angle
 00000004 b current_amps
-00000004 b old_altitude
 00000004 b original_alt
 00000004 b simple_cos_x
 00000004 b simple_sin_y
 00000004 b current_total
 00000004 b nav_loopTimer
 00000004 d scaleLongDown
-00000004 t test_failsafe(unsigned char, Menu::arg const*)
 00000004 b altitude_error
 00000004 b fast_loopTimer
 00000004 b perf_mon_timer
@@ -152,7 +162,6 @@
 00000004 r setup_compass(unsigned char, Menu::arg const*)::__c
 00000004 r print_log_menu()::__c
 00000004 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
-00000004 r test_adc(unsigned char, Menu::arg const*)::__c
 00000004 V Parameters::Parameters()::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
@@ -171,7 +180,6 @@
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
-00000005 r test_adc(unsigned char, Menu::arg const*)::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
@@ -193,6 +201,7 @@
 00000006 r Log_Read_Mode()::__c
 00000006 r print_log_menu()::__c
 00000006 r print_log_menu()::__c
+00000006 r report_batt_monitor()::__c
 00000006 V Parameters::Parameters()::__c
 00000007 b setup_menu
 00000007 b planner_menu
@@ -205,6 +214,8 @@
 00000007 r report_radio()::__c
 00000007 r report_sonar()::__c
 00000007 r print_enabled(unsigned char)::__c
+00000007 r Log_Read_Nav_Tuning()::__c
+00000007 r Log_Read_Control_Tuning()::__c
 00000007 r test_wp(unsigned char, Menu::arg const*)::__c
 00000007 V Parameters::Parameters()::__c
 00000007 V Parameters::Parameters()::__c
@@ -214,6 +225,7 @@
 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 r print_done()::__c
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -221,8 +233,6 @@
 00000008 r report_tuning()::__c
 00000008 r init_ardupilot()::__c
 00000008 r print_log_menu()::__c
-00000008 r report_batt_monitor()::__c
-00000008 r report_batt_monitor()::__c
 00000008 r test_wp(unsigned char, Menu::arg const*)::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
@@ -230,7 +240,6 @@
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
-00000008 V Parameters::Parameters()::__c
 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000009 r print_switch(unsigned char, unsigned char, bool)::__c
 00000009 r print_log_menu()::__c
@@ -260,7 +269,6 @@
 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 0000000a T piezo_on()
 0000000a T piezo_off()
-0000000a r test_relay(unsigned char, Menu::arg const*)::__c
 0000000a r start_new_log()::__c
 0000000a r print_log_menu()::__c
 0000000a r Log_Read_Startup()::__c
@@ -270,9 +278,10 @@
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
 0000000a T setup
-0000000b r test_relay(unsigned char, Menu::arg const*)::__c
-0000000b r report_batt_monitor()::__c
+0000000b r setup_esc(unsigned char, Menu::arg const*)::__c
 0000000b V Parameters::Parameters()::__c
 0000000c t process_logs(unsigned char, Menu::arg const*)
 0000000c b omega
@@ -282,11 +291,13 @@
 0000000c r print_switch(unsigned char, unsigned char, bool)::__c
 0000000c r report_frame()::__c
 0000000c r setup_factory(unsigned char, Menu::arg const*)::__c
-0000000c r test_baro(unsigned char, Menu::arg const*)::__c
+0000000c r report_version()::__c
+0000000c r report_batt_monitor()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000d r select_logs(unsigned char, Menu::arg const*)::__c
+0000000d r setup_frame(unsigned char, Menu::arg const*)::__c
 0000000d r test_tuning(unsigned char, Menu::arg const*)::__c
 0000000d r test_battery(unsigned char, Menu::arg const*)::__c
 0000000d r startup_ground()::__c
@@ -298,6 +309,8 @@
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
 0000000d B sonar_mode_filter
 0000000e t global destructors keyed to Serial
 0000000e t global constructors keyed to Serial
@@ -313,6 +326,7 @@
 0000000e r setup_mode(unsigned char, Menu::arg const*)::__c
 0000000e r test_sonar(unsigned char, Menu::arg const*)::__c
 0000000e r select_logs(unsigned char, Menu::arg const*)::__c
+0000000e r setup_compass(unsigned char, Menu::arg const*)::__c
 0000000e r print_log_menu()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
@@ -321,6 +335,7 @@
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
+0000000e r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 0000000e r report_batt_monitor()::__c
 0000000e r report_flight_modes()::__c
 0000000e r dump_log(unsigned char, Menu::arg const*)::__c
@@ -332,19 +347,18 @@
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
-0000000e V Parameters::Parameters()::__c
 0000000f b current_loc
-0000000f b next_command
+0000000f b command_nav_queue
+0000000f b command_cond_queue
 0000000f b home
 0000000f b next_WP
 0000000f b prev_WP
 0000000f b guided_WP
 0000000f b target_WP
+0000000f r setup_sonar(unsigned char, Menu::arg const*)::__c
+0000000f r Log_Read_Data()::__c
 0000000f r print_log_menu()::__c
 0000000f r print_log_menu()::__c
-0000000f r report_version()::__c
-0000000f r report_batt_monitor()::__c
-0000000f r test_imu(unsigned char, Menu::arg const*)::__c
 0000000f V AP_IMU_Oilpan::AP_IMU_Oilpan(AP_ADC*, unsigned int)::__c
 00000010 r planner_menu_commands
 00000010 b motor_out
@@ -354,7 +368,6 @@
 00000010 r report_compass()::__c
 00000011 r erase_logs(unsigned char, Menu::arg const*)::__c
 00000011 r zero_eeprom()::__c
-00000011 r update_commands()::__c
 00000012 B Serial
 00000012 B Serial1
 00000012 B Serial3
@@ -368,30 +381,23 @@
 00000012 W AP_VarT<float>::~AP_VarT()
 00000012 W AP_VarT<int>::~AP_VarT()
 00000012 W AP_VarT<signed char>::serialize(void*, unsigned int) const
-00000012 r print_done()::__c
 00000012 r select_logs(unsigned char, Menu::arg const*)::__c
-00000012 r setup_frame(unsigned char, Menu::arg const*)::__c
 00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
-00000013 r setup_compass(unsigned char, Menu::arg const*)::__c
-00000013 r change_command(unsigned char)::__c
-00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 00000014 W AP_VarT<signed char>::unserialize(void*, unsigned int)
 00000014 W AP_VarT<signed char>::cast_to_float() const
 00000014 W AP_VarT<int>::cast_to_float() const
-00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c
-00000014 r test_tri(unsigned char, Menu::arg const*)::__c
 00000015 r init_ardupilot()::__c
 00000015 r Log_Read_Motors()::__c
 00000015 r print_hit_enter()::__c
 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
 00000016 T piezo_beep()
+00000016 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000016 B sonar
 00000018 t setup_tune(unsigned char, Menu::arg const*)
 00000018 t setup_accel(unsigned char, Menu::arg const*)
 00000018 r __menu_name__main_menu
 00000018 W AP_VarT<int>::serialize(void*, unsigned int) const
 0000001a r print_log_menu()::__c
-0000001a r Log_Read_Nav_Tuning()::__c
 0000001c W AP_VarA<float, (unsigned char)6>::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Matrix3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Vector3<float> >::unserialize(void*, unsigned int)
@@ -399,18 +405,20 @@
 0000001c W AP_VarA<float, (unsigned char)6>::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Matrix3<float> >::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Vector3<float> >::serialize(void*, unsigned int) const
+0000001c r print_gyro_offsets()::__c
+0000001c r print_accel_offsets()::__c
+0000001d r report_compass()::__c
 0000001d r Log_Read_Attitude()::__c
+0000001d r Log_Read_Performance()::__c
 0000001e r Log_Read_Optflow()::__c
 00000020 t gcs_send_message(ap_message)
 00000020 r test_current(unsigned char, Menu::arg const*)::__c
 00000020 t byte_swap_4
+00000021 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000021 r print_log_menu()::__c
-00000021 r report_compass()::__c
 00000021 r Log_Read_Current()::__c
-00000021 r Log_Read_Performance()::__c
 00000022 t clear_leds()
 00000022 t print_blanks(int)
-00000022 t reset_hold_I()
 00000022 t startup_ground()
 00000022 W AP_Float16::~AP_Float16()
 00000022 W AP_VarA<float, (unsigned char)6>::~AP_VarA()
@@ -419,52 +427,49 @@
 00000022 W AP_VarT<signed char>::~AP_VarT()
 00000022 W AP_VarT<float>::~AP_VarT()
 00000022 W AP_VarT<int>::~AP_VarT()
-00000023 r setup_mode(unsigned char, Menu::arg const*)::__c
-00000023 r print_gyro_offsets()::__c
+00000022 r test_imu(unsigned char, Menu::arg const*)::__c
+00000023 r test_baro(unsigned char, Menu::arg const*)::__c
 00000024 r init_ardupilot()::__c
-00000024 r print_accel_offsets()::__c
 00000026 t print_done()
+00000026 t Log_Write_Data(signed char, float)
 00000026 t print_hit_enter()
 00000026 t Log_Read_Startup()
-00000026 r Log_Read_Control_Tuning()::__c
 00000028 t test_battery(unsigned char, Menu::arg const*)
 00000028 t main_menu_help(unsigned char, Menu::arg const*)
-00000028 t help_log(unsigned char, Menu::arg const*)
 00000028 W AP_VarT<float>::unserialize(void*, unsigned int)
 00000028 W AP_VarT<float>::serialize(void*, unsigned int) const
 00000028 r Log_Read_Cmd()::__c
-00000029 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
+0000002a t _mav_put_int8_t_array
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
 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)
 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char)
+0000002e r init_ardupilot()::__c
 0000002f r test_radio(unsigned char, Menu::arg const*)::__c
-0000002f r init_ardupilot()::__c
+0000002f r setup_factory(unsigned char, Menu::arg const*)::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 r setup_radio(unsigned char, Menu::arg const*)::__c
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
 00000032 r Log_Read_GPS()::__c
 00000033 b pending_status
 00000034 W AP_Float16::serialize(void*, unsigned int) const
-00000034 t _mav_put_int8_t_array
 00000034 t mavlink_msg_statustext_send
-00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c
 00000036 t report_radio()
-00000037 r print_wp(Location*, unsigned char)::__c
-00000038 t init_throttle_cruise()
+00000036 r print_wp(Location*, unsigned char)::__c
 00000038 t send_current_waypoint(mavlink_channel_t)
-00000038 r setup_radio(unsigned char, Menu::arg const*)::__c
-00000038 r setup_factory(unsigned char, Menu::arg const*)::__c
 0000003a t report_gps()
 0000003a t report_imu()
-0000003c W RC_Channel::~RC_Channel()
+0000003c B barometer
 0000003d B g_gps_driver
 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*)
 0000003e W AP_VarT<signed char>::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char)
+00000040 r log_menu_commands
+00000040 t init_throttle_cruise()
 00000040 t read_AHRS()
 00000040 W AP_Float16::unserialize(void*, unsigned int)
 00000040 B adc
@@ -472,14 +477,13 @@
 00000042 t report_sonar()
 00000044 t setup_show(unsigned char, Menu::arg const*)
 00000044 W AP_VarT<int>::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char)
-00000048 t change_command(unsigned char)
+00000044 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
+00000046 W RC_Channel::~RC_Channel()
 00000048 t update_motor_leds()
-00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
 0000004a t send_meminfo(mavlink_channel_t)
 0000004a W AP_VarT<int>::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char)
 0000004c t update_auto_yaw()
 00000050 b mavlink_queue
-00000050 r log_menu_commands
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
 00000050 B imu
@@ -489,13 +493,13 @@
 00000056 t readSwitch()
 00000056 t dancing_light()
 00000056 T GCS_MAVLINK::queued_waypoint_send()
-00000057 r help_log(unsigned char, Menu::arg const*)::__c
+0000005a t Log_Write_Data(signed char, long)
 0000005a W AP_VarT<float>::AP_VarT(float, unsigned int, prog_char_t const*, unsigned char)
 0000005c t get_num_logs()
 0000005c t setup_esc(unsigned char, Menu::arg const*)
+0000005e t change_command(unsigned char)
 0000005e t update_GPS_light()
 0000005e T GCS_MAVLINK::_count_parameters()
-00000064 B barometer
 00000064 t mavlink_msg_param_value_send
 00000068 t zero_eeprom()
 00000068 t find_last_log_page(int)
@@ -504,13 +508,12 @@
 0000006c t setup_sonar(unsigned char, Menu::arg const*)
 0000006e T output_min()
 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*)
-00000078 t do_RTL()
 0000007a t setup_factory(unsigned char, Menu::arg const*)
 0000007a t read_control_switch()
 0000007a t report_flight_modes()
+0000007a t do_RTL()
+0000007c t Log_Read_Data()
 0000007c t send_gps_status(mavlink_channel_t)
-0000007c t test_baro(unsigned char, Menu::arg const*)
-0000007e t test_rawgps(unsigned char, Menu::arg const*)
 00000080 T __vector_25
 00000080 T __vector_36
 00000080 T __vector_54
@@ -523,22 +526,19 @@
 00000092 t test_tuning(unsigned char, Menu::arg const*)
 00000092 t report_tuning()
 00000092 T GCS_MAVLINK::queued_param_send()
-00000096 t print_wp(Location*, unsigned char)
 0000009a t planner_gcs(unsigned char, Menu::arg const*)
 0000009a t init_compass()
 0000009a t Log_Read_Motors()
-0000009b B gcs0
-0000009b B gcs3
+0000009c B gcs0
+0000009c B gcs3
 0000009e t setup_mode(unsigned char, Menu::arg const*)
-0000009e t Log_Write_Cmd(unsigned char, Location*)
 000000a0 t Log_Read_Mode()
+000000a0 r test_menu_commands
 000000a4 T __vector_26
 000000a4 T __vector_37
 000000a4 T __vector_55
 000000aa t test_sonar(unsigned char, Menu::arg const*)
-000000aa t Log_Read_Nav_Tuning()
 000000ae t report_frame()
-000000b0 t test_relay(unsigned char, Menu::arg const*)
 000000b2 t erase_logs(unsigned char, Menu::arg const*)
 000000b6 t get_log_boundaries(unsigned char, int&, int&)
 000000b7 B compass
@@ -547,83 +547,80 @@
 000000c2 t send_radio_out(mavlink_channel_t)
 000000c2 t Log_Read_Attitude()
 000000c4 t get_distance(Location*, Location*)
-000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c
 000000c6 t test_eedump(unsigned char, Menu::arg const*)
 000000c6 t send_radio_in(mavlink_channel_t)
-000000c6 t Log_Read_Performance()
-000000c6 t test_tri(unsigned char, Menu::arg const*)
 000000c7 B dcm
 000000ca t init_barometer()
 000000d0 t read_radio()
 000000d0 t get_bearing(Location*, Location*)
+000000d0 t Log_Read_Nav_Tuning()
+000000d0 t Log_Read_Control_Tuning()
 000000d2 t print_switch(unsigned char, unsigned char, bool)
-000000d4 t Log_Read(int, int)
+000000d4 t print_wp(Location*, unsigned char)
 000000d8 t test_radio(unsigned char, Menu::arg const*)
 000000d8 t read_barometer()
-000000de t test_adc(unsigned char, Menu::arg const*)
+000000dc t Log_Read(int, int)
 000000e0 r setup_menu_commands
-000000e4 t test_radio_pwm(unsigned char, Menu::arg const*)
 000000e4 t Log_Read_Optflow()
 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)
 000000e8 t Log_Read_Current()
-000000ea t Log_Read_Control_Tuning()
 000000ee t report_batt_monitor()
+000000ee t Log_Read_Performance()
 000000f6 t Log_Read_Cmd()
-00000100 r test_menu_commands
+000000fa t calc_loiter_pitch_roll()
 0000010a t mavlink_delay(unsigned long)
 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 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)
-0000011c t get_command_with_index(int)
-0000012c t calc_loiter_pitch_roll()
+00000118 t arm_motors()
+0000011c t get_cmd_with_index(int)
 00000130 t report_compass()
-00000138 t get_stabilize_roll(long)
-00000138 t get_stabilize_pitch(long)
-00000148 t Log_Read_GPS()
+00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
+0000013a t test_baro(unsigned char, Menu::arg const*)
+0000014c t get_stabilize_roll(long)
+0000014c t get_stabilize_pitch(long)
 0000014e t send_servo_out(mavlink_channel_t)
-00000156 t update_commands()
+00000152 t init_home()
+00000156 t Log_Read_GPS()
 0000015c t update_trig()
 00000160 t send_location(mavlink_channel_t)
-00000160 t send_nav_controller_output(mavlink_channel_t)
-00000162 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
+00000160 t test_wp(unsigned char, Menu::arg const*)
+00000164 t set_cmd_with_index(Location, int)
 00000166 t select_logs(unsigned char, Menu::arg const*)
 00000166 t send_vfr_hud(mavlink_channel_t)
-00000168 T GCS_MAVLINK::update()
-0000016c t test_imu(unsigned char, Menu::arg const*)
+00000166 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
 0000016e t send_attitude(mavlink_channel_t)
 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int)
+0000017a t verify_nav_wp()
 000001a2 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int)
 000001a8 t print_radio_values()
-000001be t arm_motors()
+000001b2 t start_new_log()
+000001b8 t send_nav_controller_output(mavlink_channel_t)
 000001c8 t setup_motors(unsigned char, Menu::arg const*)
-000001cc t start_new_log()
+000001ce T GCS_MAVLINK::update()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
-000001e6 t verify_nav_wp()
-000001ea t init_home()
-00000216 t set_mode(unsigned char)
+00000210 t test_imu(unsigned char, Menu::arg const*)
 0000021a t send_raw_imu1(mavlink_channel_t)
-00000220 t test_wp(unsigned char, Menu::arg const*)
 00000228 t setup_radio(unsigned char, Menu::arg const*)
-0000022a t send_gps_raw(mavlink_channel_t)
-00000242 t calc_loiter(int, int)
+00000246 t calc_loiter(int, int)
 00000268 t send_raw_imu3(mavlink_channel_t)
-00000330 t tuning()
+0000026a t send_gps_raw(mavlink_channel_t)
+000002cc t set_mode(unsigned char)
+00000362 t tuning()
 00000384 t print_log_menu()
-0000039a T update_throttle_mode()
+00000396 T update_roll_pitch_mode()
 000003a0 t read_battery()
-00000410 T update_yaw_mode()
-0000046e T update_roll_pitch_mode()
-0000063c t init_ardupilot()
-0000071a t update_nav_wp()
-000007c8 t __static_initialization_and_destruction_0(int, int)
-00000842 b g
-00000870 t process_next_command()
-00000884 W Parameters::Parameters()
-000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-000018fa T loop
+0000041c T update_yaw_mode()
+00000444 T update_throttle_mode()
+00000708 t init_ardupilot()
+000007d8 t __static_initialization_and_destruction_0(int, int)
+00000878 t update_nav_wp()
+000008f6 W Parameters::Parameters()
+000008fa b g
+000009be t update_commands(bool)
+000014f0 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+00001a9a T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log
index b2a63cd62c..930bed821c 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.build.log
@@ -3,40 +3,38 @@
 In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
-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:399: warning: overflow in implicit constant conversion
-autogenerated: At global scope:
-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:121: 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/Attitude.pde:132: warning: 'void reset_hold_I()' defined but not used
+autogenerated:33: warning: 'void init_z_damper()' declared 'static' but never defined
+autogenerated:114: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:106: warning: 'void handle_no_commands()' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:433: 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:183: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:184: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:185: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:180: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:187: warning: 'int heli_get_angle_boost(int)' declared 'static' but never defined
 autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined
-autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/navigation.pde:98: warning: 'void calc_loiter2(int, int)' defined but not used
+autogenerated:236: warning: 'int32_t get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:238: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:240: warning: 'int32_t get_alt_distance(Location*, Location*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/radio.pde:132: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:251: warning: 'int32_t 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:262: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:263: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:270: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:271: warning: 'int read_num_from_serial()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:292: 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/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+autogenerated:265: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:266: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:273: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:274: warning: 'int read_num_from_serial()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/system.pde:486: warning: 'void set_failsafe(boolean)' defined but not used
+autogenerated:289: warning: 'void init_optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:265: warning: 'rc_override' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:266: warning: 'rc_override_active' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:267: warning: 'rc_override_fs_timer' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:488: warning: 'pmTest1' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1768: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/test.pde:34: warning: 'int8_t test_rawgps(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
@@ -77,7 +75,7 @@ In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_Optic
 %% libraries/AP_RangeFinder/RangeFinder.o
 %% libraries/AP_Relay/AP_Relay.o
 %% libraries/DataFlash/DataFlash.o
-In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35:
+In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:36:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
 %% libraries/FastSerial/BetterStream.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt
index ff4be8e9be..2d636affaa 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.size.txt
@@ -3,16 +3,17 @@
 00000001 b home_is_set
 00000001 b motor_armed
 00000001 b motor_light
+00000001 b CH7_wp_index
 00000001 b control_mode
 00000001 b gps_watchdog
 00000001 b simple_timer
 00000001 d yaw_tracking
 00000001 b land_complete
 00000001 b throttle_mode
-00000001 b command_may_ID
+00000001 b mavlink_active
+00000001 b prev_nav_index
 00000001 b wp_verify_byte
 00000001 d altitude_sensor
-00000001 b command_must_ID
 00000001 b command_yaw_dir
 00000001 b new_radio_frame
 00000001 b roll_pitch_mode
@@ -24,13 +25,14 @@
 00000001 b old_control_mode
 00000001 b slow_loopCounter
 00000001 b takeoff_complete
-00000001 b command_may_index
+00000001 b command_nav_index
 00000001 b oldSwitchPosition
-00000001 b command_must_index
+00000001 b command_cond_index
 00000001 d ground_start_count
 00000001 b medium_loopCounter
 00000001 b command_yaw_relative
 00000001 d jump
+00000001 b nav_ok
 00000001 b event_id
 00000001 b led_mode
 00000001 b yaw_mode
@@ -45,20 +47,25 @@
 00000002 T userhook_init()
 00000002 b climb_rate
 00000002 b loiter_sum
+00000002 b sonar_rate
+00000002 b angle_boost
 00000002 b event_delay
 00000002 b event_value
 00000002 b event_repeat
 00000002 b loiter_total
+00000002 b manual_boost
 00000002 b nav_throttle
+00000002 b old_baro_alt
 00000002 b x_rate_error
 00000002 b y_rate_error
-00000002 b altitude_rate
 00000002 b gps_fix_count
+00000002 b old_sonar_alt
 00000002 b velocity_land
 00000002 b x_actual_speed
 00000002 b y_actual_speed
 00000002 b loiter_time_max
 00000002 b command_yaw_time
+00000002 b crosstrack_error
 00000002 b event_undo_value
 00000002 b command_yaw_speed
 00000002 b auto_level_counter
@@ -69,6 +76,7 @@
 00000002 b g_gps
 00000002 b airspeed
 00000002 b baro_alt
+00000002 b baro_rate
 00000002 b sonar_alt
 00000002 b arm_motors()::arming_counter
 00000002 r setup_frame(unsigned char, Menu::arg const*)::__c
@@ -82,6 +90,9 @@
 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c
 00000003 r print_enabled(unsigned char)::__c
 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c
+00000003 r report_batt_monitor()::__c
+00000003 r report_batt_monitor()::__c
+00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 d cos_roll_x
 00000004 b land_start
 00000004 b long_error
@@ -93,15 +104,14 @@
 00000004 b sin_pitch_y
 00000004 b wp_distance
 00000004 b abs_pressure
+00000004 b circle_angle
 00000004 b current_amps
-00000004 b old_altitude
 00000004 b original_alt
 00000004 b simple_cos_x
 00000004 b simple_sin_y
 00000004 b current_total
 00000004 b nav_loopTimer
 00000004 d scaleLongDown
-00000004 t test_failsafe(unsigned char, Menu::arg const*)
 00000004 b altitude_error
 00000004 b fast_loopTimer
 00000004 b perf_mon_timer
@@ -152,7 +162,6 @@
 00000004 r setup_compass(unsigned char, Menu::arg const*)::__c
 00000004 r print_log_menu()::__c
 00000004 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
-00000004 r test_adc(unsigned char, Menu::arg const*)::__c
 00000004 V Parameters::Parameters()::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
@@ -171,7 +180,6 @@
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
-00000005 r test_adc(unsigned char, Menu::arg const*)::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
@@ -193,6 +201,7 @@
 00000006 r Log_Read_Mode()::__c
 00000006 r print_log_menu()::__c
 00000006 r print_log_menu()::__c
+00000006 r report_batt_monitor()::__c
 00000006 V Parameters::Parameters()::__c
 00000007 b setup_menu
 00000007 b planner_menu
@@ -205,6 +214,8 @@
 00000007 r report_radio()::__c
 00000007 r report_sonar()::__c
 00000007 r print_enabled(unsigned char)::__c
+00000007 r Log_Read_Nav_Tuning()::__c
+00000007 r Log_Read_Control_Tuning()::__c
 00000007 r test_wp(unsigned char, Menu::arg const*)::__c
 00000007 V Parameters::Parameters()::__c
 00000007 V Parameters::Parameters()::__c
@@ -214,6 +225,7 @@
 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 r print_done()::__c
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -221,8 +233,6 @@
 00000008 r report_tuning()::__c
 00000008 r init_ardupilot()::__c
 00000008 r print_log_menu()::__c
-00000008 r report_batt_monitor()::__c
-00000008 r report_batt_monitor()::__c
 00000008 r test_wp(unsigned char, Menu::arg const*)::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
@@ -230,7 +240,6 @@
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
-00000008 V Parameters::Parameters()::__c
 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000009 r print_switch(unsigned char, unsigned char, bool)::__c
 00000009 r print_log_menu()::__c
@@ -260,7 +269,6 @@
 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 0000000a T piezo_on()
 0000000a T piezo_off()
-0000000a r test_relay(unsigned char, Menu::arg const*)::__c
 0000000a r start_new_log()::__c
 0000000a r print_log_menu()::__c
 0000000a r Log_Read_Startup()::__c
@@ -270,9 +278,10 @@
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
 0000000a T setup
-0000000b r test_relay(unsigned char, Menu::arg const*)::__c
-0000000b r report_batt_monitor()::__c
+0000000b r setup_esc(unsigned char, Menu::arg const*)::__c
 0000000b V Parameters::Parameters()::__c
 0000000c t process_logs(unsigned char, Menu::arg const*)
 0000000c b omega
@@ -282,11 +291,13 @@
 0000000c r print_switch(unsigned char, unsigned char, bool)::__c
 0000000c r report_frame()::__c
 0000000c r setup_factory(unsigned char, Menu::arg const*)::__c
-0000000c r test_baro(unsigned char, Menu::arg const*)::__c
+0000000c r report_version()::__c
+0000000c r report_batt_monitor()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000d r select_logs(unsigned char, Menu::arg const*)::__c
+0000000d r setup_frame(unsigned char, Menu::arg const*)::__c
 0000000d r test_tuning(unsigned char, Menu::arg const*)::__c
 0000000d r test_battery(unsigned char, Menu::arg const*)::__c
 0000000d r startup_ground()::__c
@@ -298,6 +309,8 @@
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
 0000000d B sonar_mode_filter
 0000000e t global destructors keyed to Serial
 0000000e t global constructors keyed to Serial
@@ -313,6 +326,7 @@
 0000000e r setup_mode(unsigned char, Menu::arg const*)::__c
 0000000e r test_sonar(unsigned char, Menu::arg const*)::__c
 0000000e r select_logs(unsigned char, Menu::arg const*)::__c
+0000000e r setup_compass(unsigned char, Menu::arg const*)::__c
 0000000e r print_log_menu()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
@@ -321,6 +335,7 @@
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
+0000000e r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 0000000e r report_batt_monitor()::__c
 0000000e r report_flight_modes()::__c
 0000000e r dump_log(unsigned char, Menu::arg const*)::__c
@@ -332,19 +347,18 @@
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
-0000000e V Parameters::Parameters()::__c
 0000000f b current_loc
-0000000f b next_command
+0000000f b command_nav_queue
+0000000f b command_cond_queue
 0000000f b home
 0000000f b next_WP
 0000000f b prev_WP
 0000000f b guided_WP
 0000000f b target_WP
+0000000f r setup_sonar(unsigned char, Menu::arg const*)::__c
+0000000f r Log_Read_Data()::__c
 0000000f r print_log_menu()::__c
 0000000f r print_log_menu()::__c
-0000000f r report_version()::__c
-0000000f r report_batt_monitor()::__c
-0000000f r test_imu(unsigned char, Menu::arg const*)::__c
 0000000f V AP_IMU_Oilpan::AP_IMU_Oilpan(AP_ADC*, unsigned int)::__c
 00000010 r planner_menu_commands
 00000010 b motor_out
@@ -354,7 +368,6 @@
 00000010 r report_compass()::__c
 00000011 r erase_logs(unsigned char, Menu::arg const*)::__c
 00000011 r zero_eeprom()::__c
-00000011 r update_commands()::__c
 00000012 B Serial
 00000012 B Serial1
 00000012 B Serial3
@@ -368,30 +381,23 @@
 00000012 W AP_VarT<float>::~AP_VarT()
 00000012 W AP_VarT<int>::~AP_VarT()
 00000012 W AP_VarT<signed char>::serialize(void*, unsigned int) const
-00000012 r print_done()::__c
 00000012 r select_logs(unsigned char, Menu::arg const*)::__c
-00000012 r setup_frame(unsigned char, Menu::arg const*)::__c
 00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
-00000013 r setup_compass(unsigned char, Menu::arg const*)::__c
-00000013 r change_command(unsigned char)::__c
-00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 00000014 W AP_VarT<signed char>::unserialize(void*, unsigned int)
 00000014 W AP_VarT<signed char>::cast_to_float() const
 00000014 W AP_VarT<int>::cast_to_float() const
-00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c
-00000014 r test_tri(unsigned char, Menu::arg const*)::__c
 00000015 r init_ardupilot()::__c
 00000015 r Log_Read_Motors()::__c
 00000015 r print_hit_enter()::__c
 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
 00000016 T piezo_beep()
+00000016 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000016 B sonar
 00000018 t setup_tune(unsigned char, Menu::arg const*)
 00000018 t setup_accel(unsigned char, Menu::arg const*)
 00000018 r __menu_name__main_menu
 00000018 W AP_VarT<int>::serialize(void*, unsigned int) const
 0000001a r print_log_menu()::__c
-0000001a r Log_Read_Nav_Tuning()::__c
 0000001c W AP_VarA<float, (unsigned char)6>::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Matrix3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Vector3<float> >::unserialize(void*, unsigned int)
@@ -399,18 +405,20 @@
 0000001c W AP_VarA<float, (unsigned char)6>::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Matrix3<float> >::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Vector3<float> >::serialize(void*, unsigned int) const
+0000001c r print_gyro_offsets()::__c
+0000001c r print_accel_offsets()::__c
+0000001d r report_compass()::__c
 0000001d r Log_Read_Attitude()::__c
+0000001d r Log_Read_Performance()::__c
 0000001e r Log_Read_Optflow()::__c
 00000020 t gcs_send_message(ap_message)
 00000020 r test_current(unsigned char, Menu::arg const*)::__c
 00000020 t byte_swap_4
+00000021 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000021 r print_log_menu()::__c
-00000021 r report_compass()::__c
 00000021 r Log_Read_Current()::__c
-00000021 r Log_Read_Performance()::__c
 00000022 t clear_leds()
 00000022 t print_blanks(int)
-00000022 t reset_hold_I()
 00000022 t startup_ground()
 00000022 W AP_Float16::~AP_Float16()
 00000022 W AP_VarA<float, (unsigned char)6>::~AP_VarA()
@@ -419,52 +427,49 @@
 00000022 W AP_VarT<signed char>::~AP_VarT()
 00000022 W AP_VarT<float>::~AP_VarT()
 00000022 W AP_VarT<int>::~AP_VarT()
-00000023 r setup_mode(unsigned char, Menu::arg const*)::__c
-00000023 r print_gyro_offsets()::__c
+00000022 r test_imu(unsigned char, Menu::arg const*)::__c
+00000023 r test_baro(unsigned char, Menu::arg const*)::__c
 00000024 r init_ardupilot()::__c
-00000024 r print_accel_offsets()::__c
 00000026 t print_done()
+00000026 t Log_Write_Data(signed char, float)
 00000026 t print_hit_enter()
 00000026 t Log_Read_Startup()
-00000026 r Log_Read_Control_Tuning()::__c
 00000028 t test_battery(unsigned char, Menu::arg const*)
 00000028 t main_menu_help(unsigned char, Menu::arg const*)
-00000028 t help_log(unsigned char, Menu::arg const*)
 00000028 W AP_VarT<float>::unserialize(void*, unsigned int)
 00000028 W AP_VarT<float>::serialize(void*, unsigned int) const
 00000028 r Log_Read_Cmd()::__c
-00000029 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
+0000002a t _mav_put_int8_t_array
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
 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)
 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char)
+0000002e r init_ardupilot()::__c
 0000002f r test_radio(unsigned char, Menu::arg const*)::__c
-0000002f r init_ardupilot()::__c
+0000002f r setup_factory(unsigned char, Menu::arg const*)::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 r setup_radio(unsigned char, Menu::arg const*)::__c
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
 00000032 r Log_Read_GPS()::__c
 00000033 b pending_status
 00000034 W AP_Float16::serialize(void*, unsigned int) const
-00000034 t _mav_put_int8_t_array
 00000034 t mavlink_msg_statustext_send
-00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c
 00000036 t report_radio()
-00000037 r print_wp(Location*, unsigned char)::__c
-00000038 t init_throttle_cruise()
+00000036 r print_wp(Location*, unsigned char)::__c
 00000038 t send_current_waypoint(mavlink_channel_t)
-00000038 r setup_radio(unsigned char, Menu::arg const*)::__c
-00000038 r setup_factory(unsigned char, Menu::arg const*)::__c
 0000003a t report_gps()
 0000003a t report_imu()
-0000003c W RC_Channel::~RC_Channel()
+0000003c B barometer
 0000003d B g_gps_driver
 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*)
 0000003e W AP_VarT<signed char>::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char)
+00000040 r log_menu_commands
+00000040 t init_throttle_cruise()
 00000040 t read_AHRS()
 00000040 W AP_Float16::unserialize(void*, unsigned int)
 00000040 B adc
@@ -472,15 +477,14 @@
 00000042 t report_sonar()
 00000044 t setup_show(unsigned char, Menu::arg const*)
 00000044 W AP_VarT<int>::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char)
-00000048 t change_command(unsigned char)
+00000044 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
+00000046 W RC_Channel::~RC_Channel()
 00000048 t update_motor_leds()
-00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
 0000004a t send_meminfo(mavlink_channel_t)
 0000004a W AP_VarT<int>::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char)
 0000004c t update_auto_yaw()
 00000050 b mavlink_queue
 00000050 t report_version()
-00000050 r log_menu_commands
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
 00000050 B imu
@@ -489,13 +493,13 @@
 00000056 t readSwitch()
 00000056 t dancing_light()
 00000056 T GCS_MAVLINK::queued_waypoint_send()
-00000057 r help_log(unsigned char, Menu::arg const*)::__c
+0000005a t Log_Write_Data(signed char, long)
 0000005a W AP_VarT<float>::AP_VarT(float, unsigned int, prog_char_t const*, unsigned char)
 0000005c t get_num_logs()
 0000005c t setup_esc(unsigned char, Menu::arg const*)
+0000005e t change_command(unsigned char)
 0000005e t update_GPS_light()
 0000005e T GCS_MAVLINK::_count_parameters()
-00000064 B barometer
 00000064 t mavlink_msg_param_value_send
 00000068 t zero_eeprom()
 00000068 t find_last_log_page(int)
@@ -504,13 +508,12 @@
 0000006c t setup_sonar(unsigned char, Menu::arg const*)
 0000006e T output_min()
 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*)
-00000078 t do_RTL()
 0000007a t setup_factory(unsigned char, Menu::arg const*)
 0000007a t read_control_switch()
 0000007a t report_flight_modes()
-0000007a t test_baro(unsigned char, Menu::arg const*)
+0000007a t do_RTL()
+0000007c t Log_Read_Data()
 0000007c t send_gps_status(mavlink_channel_t)
-0000007e t test_rawgps(unsigned char, Menu::arg const*)
 00000080 T __vector_25
 00000080 T __vector_36
 00000080 T __vector_54
@@ -523,22 +526,19 @@
 00000090 t report_tuning()
 00000092 t test_tuning(unsigned char, Menu::arg const*)
 00000092 T GCS_MAVLINK::queued_param_send()
-00000096 t print_wp(Location*, unsigned char)
 0000009a t planner_gcs(unsigned char, Menu::arg const*)
 0000009a t init_compass()
 0000009a t Log_Read_Motors()
-0000009b B gcs0
-0000009b B gcs3
+0000009c B gcs0
+0000009c B gcs3
 0000009e t setup_mode(unsigned char, Menu::arg const*)
 0000009e t Log_Read_Mode()
-0000009e t Log_Write_Cmd(unsigned char, Location*)
+000000a0 r test_menu_commands
 000000a4 T __vector_26
 000000a4 T __vector_37
 000000a4 T __vector_55
 000000a8 t test_sonar(unsigned char, Menu::arg const*)
-000000aa t Log_Read_Nav_Tuning()
 000000ae t report_frame()
-000000b0 t test_relay(unsigned char, Menu::arg const*)
 000000b2 t erase_logs(unsigned char, Menu::arg const*)
 000000b6 t get_log_boundaries(unsigned char, int&, int&)
 000000b7 B compass
@@ -548,82 +548,79 @@
 000000c2 t send_radio_out(mavlink_channel_t)
 000000c2 t Log_Read_Attitude()
 000000c4 t get_distance(Location*, Location*)
-000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c
 000000c6 t send_radio_in(mavlink_channel_t)
-000000c6 t Log_Read_Performance()
-000000c6 t test_tri(unsigned char, Menu::arg const*)
 000000c7 B dcm
 000000ca t init_barometer()
+000000cc t Log_Read_Nav_Tuning()
+000000cc t Log_Read_Control_Tuning()
 000000d0 t read_radio()
 000000d0 t get_bearing(Location*, Location*)
 000000d0 t print_switch(unsigned char, unsigned char, bool)
-000000d4 t Log_Read(int, int)
+000000d4 t print_wp(Location*, unsigned char)
 000000d8 t test_radio(unsigned char, Menu::arg const*)
 000000d8 t read_barometer()
-000000dc t test_adc(unsigned char, Menu::arg const*)
+000000dc t Log_Read(int, int)
 000000e0 r setup_menu_commands
-000000e4 t test_radio_pwm(unsigned char, Menu::arg const*)
 000000e4 t Log_Read_Optflow()
 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)
 000000e8 t Log_Read_Current()
-000000ea t Log_Read_Control_Tuning()
 000000ee t report_batt_monitor()
+000000ee t Log_Read_Performance()
 000000f6 t Log_Read_Cmd()
-00000100 r test_menu_commands
+000000fa t calc_loiter_pitch_roll()
 0000010a t mavlink_delay(unsigned long)
 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 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)
-0000011c t get_command_with_index(int)
-0000012c t calc_loiter_pitch_roll()
+00000118 t arm_motors()
+0000011c t get_cmd_with_index(int)
 00000130 t report_compass()
-00000138 t get_stabilize_roll(long)
-00000138 t get_stabilize_pitch(long)
-00000148 t Log_Read_GPS()
+00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
+0000013a t test_baro(unsigned char, Menu::arg const*)
+0000014c t get_stabilize_roll(long)
+0000014c t get_stabilize_pitch(long)
 0000014e t send_servo_out(mavlink_channel_t)
-00000156 t update_commands()
+00000152 t init_home()
+00000156 t Log_Read_GPS()
 0000015c t update_trig()
+0000015c t test_wp(unsigned char, Menu::arg const*)
 00000160 t send_location(mavlink_channel_t)
-00000160 t send_nav_controller_output(mavlink_channel_t)
-00000162 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
+00000164 t set_cmd_with_index(Location, int)
 00000166 t select_logs(unsigned char, Menu::arg const*)
 00000166 t send_vfr_hud(mavlink_channel_t)
-00000168 T GCS_MAVLINK::update()
-0000016c t test_imu(unsigned char, Menu::arg const*)
+00000166 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
 0000016e t send_attitude(mavlink_channel_t)
 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int)
+00000178 t verify_nav_wp()
 000001a2 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int)
 000001a8 t print_radio_values()
-000001be t arm_motors()
+000001b2 t start_new_log()
+000001b8 t send_nav_controller_output(mavlink_channel_t)
 000001c8 t setup_motors(unsigned char, Menu::arg const*)
-000001cc t start_new_log()
-000001e4 t verify_nav_wp()
+000001ce T GCS_MAVLINK::update()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
-000001ea t init_home()
-00000216 t set_mode(unsigned char)
+00000210 t test_imu(unsigned char, Menu::arg const*)
 0000021a t send_raw_imu1(mavlink_channel_t)
-0000021c t test_wp(unsigned char, Menu::arg const*)
 00000228 t setup_radio(unsigned char, Menu::arg const*)
-0000022a t send_gps_raw(mavlink_channel_t)
-00000242 t calc_loiter(int, int)
+00000246 t calc_loiter(int, int)
 00000268 t send_raw_imu3(mavlink_channel_t)
-00000330 t tuning()
+0000026a t send_gps_raw(mavlink_channel_t)
+000002cc t set_mode(unsigned char)
+00000362 t tuning()
 00000382 t print_log_menu()
-0000039a T update_throttle_mode()
+00000396 T update_roll_pitch_mode()
 000003a0 t read_battery()
-00000410 T update_yaw_mode()
-0000046e T update_roll_pitch_mode()
-0000063a t init_ardupilot()
-0000071a t update_nav_wp()
-000007c8 t __static_initialization_and_destruction_0(int, int)
-00000842 b g
-00000870 t process_next_command()
-00000884 W Parameters::Parameters()
-000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-000018fa T loop
+0000041c T update_yaw_mode()
+00000444 T update_throttle_mode()
+00000706 t init_ardupilot()
+000007d8 t __static_initialization_and_destruction_0(int, int)
+00000878 t update_nav_wp()
+000008f6 W Parameters::Parameters()
+000008fa b g
+000009be t update_commands(bool)
+000014f0 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+00001a9a T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log
index b2a63cd62c..930bed821c 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.build.log
@@ -3,40 +3,38 @@
 In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
-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:399: warning: overflow in implicit constant conversion
-autogenerated: At global scope:
-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:121: 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/Attitude.pde:132: warning: 'void reset_hold_I()' defined but not used
+autogenerated:33: warning: 'void init_z_damper()' declared 'static' but never defined
+autogenerated:114: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:106: warning: 'void handle_no_commands()' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:433: 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:183: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:184: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:185: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:180: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:187: warning: 'int heli_get_angle_boost(int)' declared 'static' but never defined
 autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined
-autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/navigation.pde:98: warning: 'void calc_loiter2(int, int)' defined but not used
+autogenerated:236: warning: 'int32_t get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:238: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:240: warning: 'int32_t get_alt_distance(Location*, Location*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/radio.pde:132: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:251: warning: 'int32_t 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:262: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:263: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:270: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:271: warning: 'int read_num_from_serial()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:292: 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/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+autogenerated:265: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:266: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:273: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:274: warning: 'int read_num_from_serial()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/system.pde:486: warning: 'void set_failsafe(boolean)' defined but not used
+autogenerated:289: warning: 'void init_optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:265: warning: 'rc_override' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:266: warning: 'rc_override_active' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:267: warning: 'rc_override_fs_timer' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:488: warning: 'pmTest1' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1768: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/test.pde:34: warning: 'int8_t test_rawgps(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
@@ -77,7 +75,7 @@ In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_Optic
 %% libraries/AP_RangeFinder/RangeFinder.o
 %% libraries/AP_Relay/AP_Relay.o
 %% libraries/DataFlash/DataFlash.o
-In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35:
+In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:36:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
 %% libraries/FastSerial/BetterStream.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt
index 34c74448ee..803ae4acd8 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.size.txt
@@ -3,16 +3,17 @@
 00000001 b home_is_set
 00000001 b motor_armed
 00000001 b motor_light
+00000001 b CH7_wp_index
 00000001 b control_mode
 00000001 b gps_watchdog
 00000001 b simple_timer
 00000001 d yaw_tracking
 00000001 b land_complete
 00000001 b throttle_mode
-00000001 b command_may_ID
+00000001 b mavlink_active
+00000001 b prev_nav_index
 00000001 b wp_verify_byte
 00000001 d altitude_sensor
-00000001 b command_must_ID
 00000001 b command_yaw_dir
 00000001 b new_radio_frame
 00000001 b roll_pitch_mode
@@ -24,13 +25,14 @@
 00000001 b old_control_mode
 00000001 b slow_loopCounter
 00000001 b takeoff_complete
-00000001 b command_may_index
+00000001 b command_nav_index
 00000001 b oldSwitchPosition
-00000001 b command_must_index
+00000001 b command_cond_index
 00000001 d ground_start_count
 00000001 b medium_loopCounter
 00000001 b command_yaw_relative
 00000001 d jump
+00000001 b nav_ok
 00000001 b event_id
 00000001 b led_mode
 00000001 b yaw_mode
@@ -45,20 +47,25 @@
 00000002 T userhook_init()
 00000002 b climb_rate
 00000002 b loiter_sum
+00000002 b sonar_rate
+00000002 b angle_boost
 00000002 b event_delay
 00000002 b event_value
 00000002 b event_repeat
 00000002 b loiter_total
+00000002 b manual_boost
 00000002 b nav_throttle
+00000002 b old_baro_alt
 00000002 b x_rate_error
 00000002 b y_rate_error
-00000002 b altitude_rate
 00000002 b gps_fix_count
+00000002 b old_sonar_alt
 00000002 b velocity_land
 00000002 b x_actual_speed
 00000002 b y_actual_speed
 00000002 b loiter_time_max
 00000002 b command_yaw_time
+00000002 b crosstrack_error
 00000002 b event_undo_value
 00000002 b command_yaw_speed
 00000002 b auto_level_counter
@@ -69,6 +76,7 @@
 00000002 b g_gps
 00000002 b airspeed
 00000002 b baro_alt
+00000002 b baro_rate
 00000002 b sonar_alt
 00000002 b arm_motors()::arming_counter
 00000002 r setup_frame(unsigned char, Menu::arg const*)::__c
@@ -82,6 +90,9 @@
 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c
 00000003 r print_enabled(unsigned char)::__c
 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c
+00000003 r report_batt_monitor()::__c
+00000003 r report_batt_monitor()::__c
+00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 d cos_roll_x
 00000004 b land_start
 00000004 b long_error
@@ -93,15 +104,14 @@
 00000004 b sin_pitch_y
 00000004 b wp_distance
 00000004 b abs_pressure
+00000004 b circle_angle
 00000004 b current_amps
-00000004 b old_altitude
 00000004 b original_alt
 00000004 b simple_cos_x
 00000004 b simple_sin_y
 00000004 b current_total
 00000004 b nav_loopTimer
 00000004 d scaleLongDown
-00000004 t test_failsafe(unsigned char, Menu::arg const*)
 00000004 b altitude_error
 00000004 b fast_loopTimer
 00000004 b perf_mon_timer
@@ -152,7 +162,6 @@
 00000004 r setup_compass(unsigned char, Menu::arg const*)::__c
 00000004 r print_log_menu()::__c
 00000004 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
-00000004 r test_adc(unsigned char, Menu::arg const*)::__c
 00000004 V Parameters::Parameters()::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
@@ -171,7 +180,6 @@
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
-00000005 r test_adc(unsigned char, Menu::arg const*)::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
@@ -193,6 +201,7 @@
 00000006 r Log_Read_Mode()::__c
 00000006 r print_log_menu()::__c
 00000006 r print_log_menu()::__c
+00000006 r report_batt_monitor()::__c
 00000006 V Parameters::Parameters()::__c
 00000007 b setup_menu
 00000007 b planner_menu
@@ -205,6 +214,8 @@
 00000007 r report_radio()::__c
 00000007 r report_sonar()::__c
 00000007 r print_enabled(unsigned char)::__c
+00000007 r Log_Read_Nav_Tuning()::__c
+00000007 r Log_Read_Control_Tuning()::__c
 00000007 r test_wp(unsigned char, Menu::arg const*)::__c
 00000007 V Parameters::Parameters()::__c
 00000007 V Parameters::Parameters()::__c
@@ -214,6 +225,7 @@
 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 r print_done()::__c
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -221,8 +233,6 @@
 00000008 r report_tuning()::__c
 00000008 r init_ardupilot()::__c
 00000008 r print_log_menu()::__c
-00000008 r report_batt_monitor()::__c
-00000008 r report_batt_monitor()::__c
 00000008 r test_wp(unsigned char, Menu::arg const*)::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
@@ -230,7 +240,6 @@
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
-00000008 V Parameters::Parameters()::__c
 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000009 r print_switch(unsigned char, unsigned char, bool)::__c
 00000009 r print_log_menu()::__c
@@ -260,7 +269,6 @@
 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 0000000a T piezo_on()
 0000000a T piezo_off()
-0000000a r test_relay(unsigned char, Menu::arg const*)::__c
 0000000a r start_new_log()::__c
 0000000a r print_log_menu()::__c
 0000000a r Log_Read_Startup()::__c
@@ -270,10 +278,11 @@
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
 0000000a T setup
-0000000b r test_relay(unsigned char, Menu::arg const*)::__c
 0000000b r report_frame()::__c
-0000000b r report_batt_monitor()::__c
+0000000b r setup_esc(unsigned char, Menu::arg const*)::__c
 0000000b V Parameters::Parameters()::__c
 0000000c t process_logs(unsigned char, Menu::arg const*)
 0000000c b omega
@@ -282,11 +291,13 @@
 0000000c V vtable for IMU
 0000000c r print_switch(unsigned char, unsigned char, bool)::__c
 0000000c r setup_factory(unsigned char, Menu::arg const*)::__c
-0000000c r test_baro(unsigned char, Menu::arg const*)::__c
+0000000c r report_version()::__c
+0000000c r report_batt_monitor()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000d r select_logs(unsigned char, Menu::arg const*)::__c
+0000000d r setup_frame(unsigned char, Menu::arg const*)::__c
 0000000d r test_tuning(unsigned char, Menu::arg const*)::__c
 0000000d r test_battery(unsigned char, Menu::arg const*)::__c
 0000000d r startup_ground()::__c
@@ -298,6 +309,8 @@
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
 0000000d B sonar_mode_filter
 0000000e t global destructors keyed to Serial
 0000000e t global constructors keyed to Serial
@@ -313,6 +326,7 @@
 0000000e r setup_mode(unsigned char, Menu::arg const*)::__c
 0000000e r test_sonar(unsigned char, Menu::arg const*)::__c
 0000000e r select_logs(unsigned char, Menu::arg const*)::__c
+0000000e r setup_compass(unsigned char, Menu::arg const*)::__c
 0000000e r print_log_menu()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
@@ -321,6 +335,7 @@
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
+0000000e r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 0000000e r report_batt_monitor()::__c
 0000000e r report_flight_modes()::__c
 0000000e r dump_log(unsigned char, Menu::arg const*)::__c
@@ -332,19 +347,18 @@
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
-0000000e V Parameters::Parameters()::__c
 0000000f b current_loc
-0000000f b next_command
+0000000f b command_nav_queue
+0000000f b command_cond_queue
 0000000f b home
 0000000f b next_WP
 0000000f b prev_WP
 0000000f b guided_WP
 0000000f b target_WP
+0000000f r setup_sonar(unsigned char, Menu::arg const*)::__c
+0000000f r Log_Read_Data()::__c
 0000000f r print_log_menu()::__c
 0000000f r print_log_menu()::__c
-0000000f r report_version()::__c
-0000000f r report_batt_monitor()::__c
-0000000f r test_imu(unsigned char, Menu::arg const*)::__c
 0000000f V AP_IMU_Oilpan::AP_IMU_Oilpan(AP_ADC*, unsigned int)::__c
 00000010 r planner_menu_commands
 00000010 b motor_out
@@ -354,7 +368,6 @@
 00000010 r report_compass()::__c
 00000011 r erase_logs(unsigned char, Menu::arg const*)::__c
 00000011 r zero_eeprom()::__c
-00000011 r update_commands()::__c
 00000012 B Serial
 00000012 B Serial1
 00000012 B Serial3
@@ -368,30 +381,23 @@
 00000012 W AP_VarT<float>::~AP_VarT()
 00000012 W AP_VarT<int>::~AP_VarT()
 00000012 W AP_VarT<signed char>::serialize(void*, unsigned int) const
-00000012 r print_done()::__c
 00000012 r select_logs(unsigned char, Menu::arg const*)::__c
-00000012 r setup_frame(unsigned char, Menu::arg const*)::__c
 00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
-00000013 r setup_compass(unsigned char, Menu::arg const*)::__c
-00000013 r change_command(unsigned char)::__c
-00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 00000014 W AP_VarT<signed char>::unserialize(void*, unsigned int)
 00000014 W AP_VarT<signed char>::cast_to_float() const
 00000014 W AP_VarT<int>::cast_to_float() const
-00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c
-00000014 r test_tri(unsigned char, Menu::arg const*)::__c
 00000015 r init_ardupilot()::__c
 00000015 r Log_Read_Motors()::__c
 00000015 r print_hit_enter()::__c
 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
 00000016 T piezo_beep()
+00000016 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000016 B sonar
 00000018 t setup_tune(unsigned char, Menu::arg const*)
 00000018 t setup_accel(unsigned char, Menu::arg const*)
 00000018 r __menu_name__main_menu
 00000018 W AP_VarT<int>::serialize(void*, unsigned int) const
 0000001a r print_log_menu()::__c
-0000001a r Log_Read_Nav_Tuning()::__c
 0000001c W AP_VarA<float, (unsigned char)6>::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Matrix3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Vector3<float> >::unserialize(void*, unsigned int)
@@ -399,18 +405,20 @@
 0000001c W AP_VarA<float, (unsigned char)6>::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Matrix3<float> >::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Vector3<float> >::serialize(void*, unsigned int) const
+0000001c r print_gyro_offsets()::__c
+0000001c r print_accel_offsets()::__c
+0000001d r report_compass()::__c
 0000001d r Log_Read_Attitude()::__c
+0000001d r Log_Read_Performance()::__c
 0000001e r Log_Read_Optflow()::__c
 00000020 t gcs_send_message(ap_message)
 00000020 r test_current(unsigned char, Menu::arg const*)::__c
 00000020 t byte_swap_4
+00000021 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000021 r print_log_menu()::__c
-00000021 r report_compass()::__c
 00000021 r Log_Read_Current()::__c
-00000021 r Log_Read_Performance()::__c
 00000022 t clear_leds()
 00000022 t print_blanks(int)
-00000022 t reset_hold_I()
 00000022 t startup_ground()
 00000022 W AP_Float16::~AP_Float16()
 00000022 W AP_VarA<float, (unsigned char)6>::~AP_VarA()
@@ -419,52 +427,49 @@
 00000022 W AP_VarT<signed char>::~AP_VarT()
 00000022 W AP_VarT<float>::~AP_VarT()
 00000022 W AP_VarT<int>::~AP_VarT()
-00000023 r setup_mode(unsigned char, Menu::arg const*)::__c
-00000023 r print_gyro_offsets()::__c
+00000022 r test_imu(unsigned char, Menu::arg const*)::__c
+00000023 r test_baro(unsigned char, Menu::arg const*)::__c
 00000024 r init_ardupilot()::__c
-00000024 r print_accel_offsets()::__c
 00000026 t print_done()
+00000026 t Log_Write_Data(signed char, float)
 00000026 t print_hit_enter()
 00000026 t Log_Read_Startup()
-00000026 r Log_Read_Control_Tuning()::__c
 00000028 t test_battery(unsigned char, Menu::arg const*)
 00000028 t main_menu_help(unsigned char, Menu::arg const*)
-00000028 t help_log(unsigned char, Menu::arg const*)
 00000028 W AP_VarT<float>::unserialize(void*, unsigned int)
 00000028 W AP_VarT<float>::serialize(void*, unsigned int) const
 00000028 r Log_Read_Cmd()::__c
-00000029 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
+0000002a t _mav_put_int8_t_array
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
 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)
 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char)
+0000002e r init_ardupilot()::__c
 0000002f r test_radio(unsigned char, Menu::arg const*)::__c
-0000002f r init_ardupilot()::__c
+0000002f r setup_factory(unsigned char, Menu::arg const*)::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 r setup_radio(unsigned char, Menu::arg const*)::__c
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
 00000032 r Log_Read_GPS()::__c
 00000033 b pending_status
 00000034 W AP_Float16::serialize(void*, unsigned int) const
-00000034 t _mav_put_int8_t_array
 00000034 t mavlink_msg_statustext_send
-00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c
 00000036 t report_radio()
-00000037 r print_wp(Location*, unsigned char)::__c
-00000038 t init_throttle_cruise()
+00000036 r print_wp(Location*, unsigned char)::__c
 00000038 t send_current_waypoint(mavlink_channel_t)
-00000038 r setup_radio(unsigned char, Menu::arg const*)::__c
-00000038 r setup_factory(unsigned char, Menu::arg const*)::__c
 0000003a t report_gps()
 0000003a t report_imu()
-0000003c W RC_Channel::~RC_Channel()
+0000003c B barometer
 0000003d B g_gps_driver
 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*)
 0000003e W AP_VarT<signed char>::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char)
+00000040 r log_menu_commands
+00000040 t init_throttle_cruise()
 00000040 t read_AHRS()
 00000040 W AP_Float16::unserialize(void*, unsigned int)
 00000040 B adc
@@ -472,14 +477,13 @@
 00000042 t report_sonar()
 00000044 t setup_show(unsigned char, Menu::arg const*)
 00000044 W AP_VarT<int>::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char)
-00000048 t change_command(unsigned char)
+00000044 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
+00000046 W RC_Channel::~RC_Channel()
 00000048 t update_motor_leds()
-00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
 0000004a t send_meminfo(mavlink_channel_t)
 0000004a W AP_VarT<int>::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char)
 0000004c t update_auto_yaw()
 00000050 b mavlink_queue
-00000050 r log_menu_commands
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
 00000050 B imu
@@ -489,13 +493,13 @@
 00000056 t readSwitch()
 00000056 t dancing_light()
 00000056 T GCS_MAVLINK::queued_waypoint_send()
-00000057 r help_log(unsigned char, Menu::arg const*)::__c
+0000005a t Log_Write_Data(signed char, long)
 0000005a W AP_VarT<float>::AP_VarT(float, unsigned int, prog_char_t const*, unsigned char)
 0000005c t get_num_logs()
 0000005c t setup_esc(unsigned char, Menu::arg const*)
+0000005e t change_command(unsigned char)
 0000005e t update_GPS_light()
 0000005e T GCS_MAVLINK::_count_parameters()
-00000064 B barometer
 00000064 t mavlink_msg_param_value_send
 00000068 t zero_eeprom()
 00000068 t find_last_log_page(int)
@@ -504,13 +508,12 @@
 0000006c t setup_sonar(unsigned char, Menu::arg const*)
 0000006e T output_min()
 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*)
-00000078 t do_RTL()
 0000007a t setup_factory(unsigned char, Menu::arg const*)
 0000007a t read_control_switch()
 0000007a t report_flight_modes()
+0000007a t do_RTL()
+0000007c t Log_Read_Data()
 0000007c t send_gps_status(mavlink_channel_t)
-0000007c t test_baro(unsigned char, Menu::arg const*)
-0000007e t test_rawgps(unsigned char, Menu::arg const*)
 00000080 T __vector_25
 00000080 T __vector_36
 00000080 T __vector_54
@@ -523,22 +526,19 @@
 00000092 t test_tuning(unsigned char, Menu::arg const*)
 00000092 t report_tuning()
 00000092 T GCS_MAVLINK::queued_param_send()
-00000096 t print_wp(Location*, unsigned char)
 0000009a t planner_gcs(unsigned char, Menu::arg const*)
 0000009a t init_compass()
 0000009a t Log_Read_Motors()
-0000009b B gcs0
-0000009b B gcs3
+0000009c B gcs0
+0000009c B gcs3
 0000009e t setup_mode(unsigned char, Menu::arg const*)
-0000009e t Log_Write_Cmd(unsigned char, Location*)
 000000a0 t Log_Read_Mode()
+000000a0 r test_menu_commands
 000000a4 T __vector_26
 000000a4 T __vector_37
 000000a4 T __vector_55
 000000aa t test_sonar(unsigned char, Menu::arg const*)
-000000aa t Log_Read_Nav_Tuning()
 000000ae t report_frame()
-000000b0 t test_relay(unsigned char, Menu::arg const*)
 000000b2 t erase_logs(unsigned char, Menu::arg const*)
 000000b6 t get_log_boundaries(unsigned char, int&, int&)
 000000b7 B compass
@@ -547,83 +547,80 @@
 000000c2 t send_radio_out(mavlink_channel_t)
 000000c2 t Log_Read_Attitude()
 000000c4 t get_distance(Location*, Location*)
-000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c
 000000c6 t test_eedump(unsigned char, Menu::arg const*)
 000000c6 t send_radio_in(mavlink_channel_t)
-000000c6 t Log_Read_Performance()
-000000c6 t test_tri(unsigned char, Menu::arg const*)
 000000c7 B dcm
 000000ca t init_barometer()
 000000d0 t read_radio()
 000000d0 t get_bearing(Location*, Location*)
+000000d0 t Log_Read_Nav_Tuning()
+000000d0 t Log_Read_Control_Tuning()
 000000d2 t print_switch(unsigned char, unsigned char, bool)
-000000d4 t Log_Read(int, int)
+000000d4 t print_wp(Location*, unsigned char)
 000000d8 t test_radio(unsigned char, Menu::arg const*)
 000000d8 t read_barometer()
+000000dc t Log_Read(int, int)
 000000de t setup_motors(unsigned char, Menu::arg const*)
-000000de t test_adc(unsigned char, Menu::arg const*)
 000000e0 r setup_menu_commands
-000000e4 t test_radio_pwm(unsigned char, Menu::arg const*)
 000000e4 t Log_Read_Optflow()
 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)
 000000e8 t Log_Read_Current()
-000000ea t Log_Read_Control_Tuning()
 000000ee t report_batt_monitor()
+000000ee t Log_Read_Performance()
 000000f6 t Log_Read_Cmd()
-00000100 r test_menu_commands
+000000fa t calc_loiter_pitch_roll()
 0000010a t mavlink_delay(unsigned long)
 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 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)
-0000011c t get_command_with_index(int)
-0000012c t calc_loiter_pitch_roll()
+00000118 t arm_motors()
+0000011c t get_cmd_with_index(int)
 00000130 t report_compass()
-00000138 t get_stabilize_roll(long)
-00000138 t get_stabilize_pitch(long)
-00000148 t Log_Read_GPS()
+00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
+0000013a t test_baro(unsigned char, Menu::arg const*)
+0000014c t get_stabilize_roll(long)
+0000014c t get_stabilize_pitch(long)
 0000014e t send_servo_out(mavlink_channel_t)
-00000156 t update_commands()
+00000152 t init_home()
+00000156 t Log_Read_GPS()
 0000015c t update_trig()
 00000160 t send_location(mavlink_channel_t)
-00000160 t send_nav_controller_output(mavlink_channel_t)
-00000162 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
+00000160 t test_wp(unsigned char, Menu::arg const*)
+00000164 t set_cmd_with_index(Location, int)
 00000166 t select_logs(unsigned char, Menu::arg const*)
 00000166 t send_vfr_hud(mavlink_channel_t)
-00000168 T GCS_MAVLINK::update()
-0000016c t test_imu(unsigned char, Menu::arg const*)
+00000166 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
 0000016e t send_attitude(mavlink_channel_t)
 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int)
+0000017a t verify_nav_wp()
 000001a2 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int)
 000001a8 t print_radio_values()
-000001be t arm_motors()
-000001cc t start_new_log()
+000001b2 t start_new_log()
+000001b8 t send_nav_controller_output(mavlink_channel_t)
+000001ce T GCS_MAVLINK::update()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
-000001e6 t verify_nav_wp()
-000001ea t init_home()
-00000216 t set_mode(unsigned char)
+00000210 t test_imu(unsigned char, Menu::arg const*)
 0000021a t send_raw_imu1(mavlink_channel_t)
-00000220 t test_wp(unsigned char, Menu::arg const*)
 00000228 t setup_radio(unsigned char, Menu::arg const*)
-0000022a t send_gps_raw(mavlink_channel_t)
-00000242 t calc_loiter(int, int)
+00000246 t calc_loiter(int, int)
 00000268 t send_raw_imu3(mavlink_channel_t)
-00000330 t tuning()
+0000026a t send_gps_raw(mavlink_channel_t)
+000002cc t set_mode(unsigned char)
+00000362 t tuning()
 00000384 t print_log_menu()
-0000039a T update_throttle_mode()
+00000396 T update_roll_pitch_mode()
 000003a0 t read_battery()
-00000410 T update_yaw_mode()
-0000046e T update_roll_pitch_mode()
-0000063c t init_ardupilot()
-0000071a t update_nav_wp()
-000007c8 t __static_initialization_and_destruction_0(int, int)
-00000842 b g
-00000870 t process_next_command()
-00000884 W Parameters::Parameters()
-000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-00001836 T loop
+0000041c T update_yaw_mode()
+00000444 T update_throttle_mode()
+00000708 t init_ardupilot()
+000007d8 t __static_initialization_and_destruction_0(int, int)
+00000878 t update_nav_wp()
+000008f6 W Parameters::Parameters()
+000008fa b g
+000009be t update_commands(bool)
+000014f0 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+000019d6 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log
index b2a63cd62c..930bed821c 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.build.log
@@ -3,40 +3,38 @@
 In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
-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:399: warning: overflow in implicit constant conversion
-autogenerated: At global scope:
-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:121: 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/Attitude.pde:132: warning: 'void reset_hold_I()' defined but not used
+autogenerated:33: warning: 'void init_z_damper()' declared 'static' but never defined
+autogenerated:114: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:106: warning: 'void handle_no_commands()' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:433: 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:183: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:184: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:185: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:180: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:187: warning: 'int heli_get_angle_boost(int)' declared 'static' but never defined
 autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined
-autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/navigation.pde:98: warning: 'void calc_loiter2(int, int)' defined but not used
+autogenerated:236: warning: 'int32_t get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:238: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:240: warning: 'int32_t get_alt_distance(Location*, Location*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/radio.pde:132: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:251: warning: 'int32_t 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:262: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:263: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:270: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:271: warning: 'int read_num_from_serial()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:292: 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/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+autogenerated:265: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:266: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:273: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:274: warning: 'int read_num_from_serial()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/system.pde:486: warning: 'void set_failsafe(boolean)' defined but not used
+autogenerated:289: warning: 'void init_optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:265: warning: 'rc_override' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:266: warning: 'rc_override_active' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:267: warning: 'rc_override_fs_timer' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:488: warning: 'pmTest1' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1768: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/test.pde:34: warning: 'int8_t test_rawgps(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
@@ -77,7 +75,7 @@ In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_Optic
 %% libraries/AP_RangeFinder/RangeFinder.o
 %% libraries/AP_Relay/AP_Relay.o
 %% libraries/DataFlash/DataFlash.o
-In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35:
+In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:36:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
 %% libraries/FastSerial/BetterStream.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt
index 0b2a85e7fc..6ff9d78809 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.size.txt
@@ -3,16 +3,17 @@
 00000001 b home_is_set
 00000001 b motor_armed
 00000001 b motor_light
+00000001 b CH7_wp_index
 00000001 b control_mode
 00000001 b gps_watchdog
 00000001 b simple_timer
 00000001 d yaw_tracking
 00000001 b land_complete
 00000001 b throttle_mode
-00000001 b command_may_ID
+00000001 b mavlink_active
+00000001 b prev_nav_index
 00000001 b wp_verify_byte
 00000001 d altitude_sensor
-00000001 b command_must_ID
 00000001 b command_yaw_dir
 00000001 b new_radio_frame
 00000001 b roll_pitch_mode
@@ -24,13 +25,14 @@
 00000001 b old_control_mode
 00000001 b slow_loopCounter
 00000001 b takeoff_complete
-00000001 b command_may_index
+00000001 b command_nav_index
 00000001 b oldSwitchPosition
-00000001 b command_must_index
+00000001 b command_cond_index
 00000001 d ground_start_count
 00000001 b medium_loopCounter
 00000001 b command_yaw_relative
 00000001 d jump
+00000001 b nav_ok
 00000001 b event_id
 00000001 b led_mode
 00000001 b yaw_mode
@@ -45,20 +47,25 @@
 00000002 T userhook_init()
 00000002 b climb_rate
 00000002 b loiter_sum
+00000002 b sonar_rate
+00000002 b angle_boost
 00000002 b event_delay
 00000002 b event_value
 00000002 b event_repeat
 00000002 b loiter_total
+00000002 b manual_boost
 00000002 b nav_throttle
+00000002 b old_baro_alt
 00000002 b x_rate_error
 00000002 b y_rate_error
-00000002 b altitude_rate
 00000002 b gps_fix_count
+00000002 b old_sonar_alt
 00000002 b velocity_land
 00000002 b x_actual_speed
 00000002 b y_actual_speed
 00000002 b loiter_time_max
 00000002 b command_yaw_time
+00000002 b crosstrack_error
 00000002 b event_undo_value
 00000002 b command_yaw_speed
 00000002 b auto_level_counter
@@ -69,6 +76,7 @@
 00000002 b g_gps
 00000002 b airspeed
 00000002 b baro_alt
+00000002 b baro_rate
 00000002 b sonar_alt
 00000002 b arm_motors()::arming_counter
 00000002 r setup_frame(unsigned char, Menu::arg const*)::__c
@@ -82,6 +90,9 @@
 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c
 00000003 r print_enabled(unsigned char)::__c
 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c
+00000003 r report_batt_monitor()::__c
+00000003 r report_batt_monitor()::__c
+00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 d cos_roll_x
 00000004 b land_start
 00000004 b long_error
@@ -93,15 +104,14 @@
 00000004 b sin_pitch_y
 00000004 b wp_distance
 00000004 b abs_pressure
+00000004 b circle_angle
 00000004 b current_amps
-00000004 b old_altitude
 00000004 b original_alt
 00000004 b simple_cos_x
 00000004 b simple_sin_y
 00000004 b current_total
 00000004 b nav_loopTimer
 00000004 d scaleLongDown
-00000004 t test_failsafe(unsigned char, Menu::arg const*)
 00000004 b altitude_error
 00000004 b fast_loopTimer
 00000004 b perf_mon_timer
@@ -152,7 +162,6 @@
 00000004 r setup_compass(unsigned char, Menu::arg const*)::__c
 00000004 r print_log_menu()::__c
 00000004 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
-00000004 r test_adc(unsigned char, Menu::arg const*)::__c
 00000004 V Parameters::Parameters()::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
@@ -171,7 +180,6 @@
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
-00000005 r test_adc(unsigned char, Menu::arg const*)::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
@@ -193,6 +201,7 @@
 00000006 r Log_Read_Mode()::__c
 00000006 r print_log_menu()::__c
 00000006 r print_log_menu()::__c
+00000006 r report_batt_monitor()::__c
 00000006 V Parameters::Parameters()::__c
 00000007 b setup_menu
 00000007 b planner_menu
@@ -205,6 +214,8 @@
 00000007 r report_radio()::__c
 00000007 r report_sonar()::__c
 00000007 r print_enabled(unsigned char)::__c
+00000007 r Log_Read_Nav_Tuning()::__c
+00000007 r Log_Read_Control_Tuning()::__c
 00000007 r test_wp(unsigned char, Menu::arg const*)::__c
 00000007 V Parameters::Parameters()::__c
 00000007 V Parameters::Parameters()::__c
@@ -214,6 +225,7 @@
 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 r print_done()::__c
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -221,8 +233,6 @@
 00000008 r report_tuning()::__c
 00000008 r init_ardupilot()::__c
 00000008 r print_log_menu()::__c
-00000008 r report_batt_monitor()::__c
-00000008 r report_batt_monitor()::__c
 00000008 r test_wp(unsigned char, Menu::arg const*)::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
@@ -230,7 +240,6 @@
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
-00000008 V Parameters::Parameters()::__c
 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000009 r print_switch(unsigned char, unsigned char, bool)::__c
 00000009 r print_log_menu()::__c
@@ -260,7 +269,6 @@
 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 0000000a T piezo_on()
 0000000a T piezo_off()
-0000000a r test_relay(unsigned char, Menu::arg const*)::__c
 0000000a r start_new_log()::__c
 0000000a r print_log_menu()::__c
 0000000a r Log_Read_Startup()::__c
@@ -270,10 +278,11 @@
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
 0000000a T setup
-0000000b r test_relay(unsigned char, Menu::arg const*)::__c
 0000000b r report_frame()::__c
-0000000b r report_batt_monitor()::__c
+0000000b r setup_esc(unsigned char, Menu::arg const*)::__c
 0000000b V Parameters::Parameters()::__c
 0000000c t process_logs(unsigned char, Menu::arg const*)
 0000000c b omega
@@ -282,11 +291,13 @@
 0000000c V vtable for IMU
 0000000c r print_switch(unsigned char, unsigned char, bool)::__c
 0000000c r setup_factory(unsigned char, Menu::arg const*)::__c
-0000000c r test_baro(unsigned char, Menu::arg const*)::__c
+0000000c r report_version()::__c
+0000000c r report_batt_monitor()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000d r select_logs(unsigned char, Menu::arg const*)::__c
+0000000d r setup_frame(unsigned char, Menu::arg const*)::__c
 0000000d r test_tuning(unsigned char, Menu::arg const*)::__c
 0000000d r test_battery(unsigned char, Menu::arg const*)::__c
 0000000d r startup_ground()::__c
@@ -298,6 +309,8 @@
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
 0000000d B sonar_mode_filter
 0000000e t global destructors keyed to Serial
 0000000e t global constructors keyed to Serial
@@ -313,6 +326,7 @@
 0000000e r setup_mode(unsigned char, Menu::arg const*)::__c
 0000000e r test_sonar(unsigned char, Menu::arg const*)::__c
 0000000e r select_logs(unsigned char, Menu::arg const*)::__c
+0000000e r setup_compass(unsigned char, Menu::arg const*)::__c
 0000000e r print_log_menu()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
@@ -321,6 +335,7 @@
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
+0000000e r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 0000000e r report_batt_monitor()::__c
 0000000e r report_flight_modes()::__c
 0000000e r dump_log(unsigned char, Menu::arg const*)::__c
@@ -332,19 +347,18 @@
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
-0000000e V Parameters::Parameters()::__c
 0000000f b current_loc
-0000000f b next_command
+0000000f b command_nav_queue
+0000000f b command_cond_queue
 0000000f b home
 0000000f b next_WP
 0000000f b prev_WP
 0000000f b guided_WP
 0000000f b target_WP
+0000000f r setup_sonar(unsigned char, Menu::arg const*)::__c
+0000000f r Log_Read_Data()::__c
 0000000f r print_log_menu()::__c
 0000000f r print_log_menu()::__c
-0000000f r report_version()::__c
-0000000f r report_batt_monitor()::__c
-0000000f r test_imu(unsigned char, Menu::arg const*)::__c
 0000000f V AP_IMU_Oilpan::AP_IMU_Oilpan(AP_ADC*, unsigned int)::__c
 00000010 r planner_menu_commands
 00000010 b motor_out
@@ -354,7 +368,6 @@
 00000010 r report_compass()::__c
 00000011 r erase_logs(unsigned char, Menu::arg const*)::__c
 00000011 r zero_eeprom()::__c
-00000011 r update_commands()::__c
 00000012 B Serial
 00000012 B Serial1
 00000012 B Serial3
@@ -368,30 +381,23 @@
 00000012 W AP_VarT<float>::~AP_VarT()
 00000012 W AP_VarT<int>::~AP_VarT()
 00000012 W AP_VarT<signed char>::serialize(void*, unsigned int) const
-00000012 r print_done()::__c
 00000012 r select_logs(unsigned char, Menu::arg const*)::__c
-00000012 r setup_frame(unsigned char, Menu::arg const*)::__c
 00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
-00000013 r setup_compass(unsigned char, Menu::arg const*)::__c
-00000013 r change_command(unsigned char)::__c
-00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 00000014 W AP_VarT<signed char>::unserialize(void*, unsigned int)
 00000014 W AP_VarT<signed char>::cast_to_float() const
 00000014 W AP_VarT<int>::cast_to_float() const
-00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c
-00000014 r test_tri(unsigned char, Menu::arg const*)::__c
 00000015 r init_ardupilot()::__c
 00000015 r Log_Read_Motors()::__c
 00000015 r print_hit_enter()::__c
 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
 00000016 T piezo_beep()
+00000016 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000016 B sonar
 00000018 t setup_tune(unsigned char, Menu::arg const*)
 00000018 t setup_accel(unsigned char, Menu::arg const*)
 00000018 r __menu_name__main_menu
 00000018 W AP_VarT<int>::serialize(void*, unsigned int) const
 0000001a r print_log_menu()::__c
-0000001a r Log_Read_Nav_Tuning()::__c
 0000001c W AP_VarA<float, (unsigned char)6>::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Matrix3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Vector3<float> >::unserialize(void*, unsigned int)
@@ -399,18 +405,20 @@
 0000001c W AP_VarA<float, (unsigned char)6>::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Matrix3<float> >::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Vector3<float> >::serialize(void*, unsigned int) const
+0000001c r print_gyro_offsets()::__c
+0000001c r print_accel_offsets()::__c
+0000001d r report_compass()::__c
 0000001d r Log_Read_Attitude()::__c
+0000001d r Log_Read_Performance()::__c
 0000001e r Log_Read_Optflow()::__c
 00000020 t gcs_send_message(ap_message)
 00000020 r test_current(unsigned char, Menu::arg const*)::__c
 00000020 t byte_swap_4
+00000021 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000021 r print_log_menu()::__c
-00000021 r report_compass()::__c
 00000021 r Log_Read_Current()::__c
-00000021 r Log_Read_Performance()::__c
 00000022 t clear_leds()
 00000022 t print_blanks(int)
-00000022 t reset_hold_I()
 00000022 t startup_ground()
 00000022 W AP_Float16::~AP_Float16()
 00000022 W AP_VarA<float, (unsigned char)6>::~AP_VarA()
@@ -419,52 +427,49 @@
 00000022 W AP_VarT<signed char>::~AP_VarT()
 00000022 W AP_VarT<float>::~AP_VarT()
 00000022 W AP_VarT<int>::~AP_VarT()
-00000023 r setup_mode(unsigned char, Menu::arg const*)::__c
-00000023 r print_gyro_offsets()::__c
+00000022 r test_imu(unsigned char, Menu::arg const*)::__c
+00000023 r test_baro(unsigned char, Menu::arg const*)::__c
 00000024 r init_ardupilot()::__c
-00000024 r print_accel_offsets()::__c
 00000026 t print_done()
+00000026 t Log_Write_Data(signed char, float)
 00000026 t print_hit_enter()
 00000026 t Log_Read_Startup()
-00000026 r Log_Read_Control_Tuning()::__c
 00000028 t test_battery(unsigned char, Menu::arg const*)
 00000028 t main_menu_help(unsigned char, Menu::arg const*)
-00000028 t help_log(unsigned char, Menu::arg const*)
 00000028 W AP_VarT<float>::unserialize(void*, unsigned int)
 00000028 W AP_VarT<float>::serialize(void*, unsigned int) const
 00000028 r Log_Read_Cmd()::__c
-00000029 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
+0000002a t _mav_put_int8_t_array
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
 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)
 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char)
+0000002e r init_ardupilot()::__c
 0000002f r test_radio(unsigned char, Menu::arg const*)::__c
-0000002f r init_ardupilot()::__c
+0000002f r setup_factory(unsigned char, Menu::arg const*)::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 r setup_radio(unsigned char, Menu::arg const*)::__c
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
 00000032 r Log_Read_GPS()::__c
 00000033 b pending_status
 00000034 W AP_Float16::serialize(void*, unsigned int) const
-00000034 t _mav_put_int8_t_array
 00000034 t mavlink_msg_statustext_send
-00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c
 00000036 t report_radio()
-00000037 r print_wp(Location*, unsigned char)::__c
-00000038 t init_throttle_cruise()
+00000036 r print_wp(Location*, unsigned char)::__c
 00000038 t send_current_waypoint(mavlink_channel_t)
-00000038 r setup_radio(unsigned char, Menu::arg const*)::__c
-00000038 r setup_factory(unsigned char, Menu::arg const*)::__c
 0000003a t report_gps()
 0000003a t report_imu()
-0000003c W RC_Channel::~RC_Channel()
+0000003c B barometer
 0000003d B g_gps_driver
 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*)
 0000003e W AP_VarT<signed char>::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char)
+00000040 r log_menu_commands
+00000040 t init_throttle_cruise()
 00000040 t read_AHRS()
 00000040 W AP_Float16::unserialize(void*, unsigned int)
 00000040 B adc
@@ -472,15 +477,14 @@
 00000042 t report_sonar()
 00000044 t setup_show(unsigned char, Menu::arg const*)
 00000044 W AP_VarT<int>::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char)
-00000048 t change_command(unsigned char)
+00000044 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
+00000046 W RC_Channel::~RC_Channel()
 00000048 t update_motor_leds()
-00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
 0000004a t send_meminfo(mavlink_channel_t)
 0000004a W AP_VarT<int>::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char)
 0000004c t update_auto_yaw()
 00000050 b mavlink_queue
 00000050 t report_version()
-00000050 r log_menu_commands
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
 00000050 B imu
@@ -489,13 +493,13 @@
 00000056 t readSwitch()
 00000056 t dancing_light()
 00000056 T GCS_MAVLINK::queued_waypoint_send()
-00000057 r help_log(unsigned char, Menu::arg const*)::__c
+0000005a t Log_Write_Data(signed char, long)
 0000005a W AP_VarT<float>::AP_VarT(float, unsigned int, prog_char_t const*, unsigned char)
 0000005c t get_num_logs()
 0000005c t setup_esc(unsigned char, Menu::arg const*)
+0000005e t change_command(unsigned char)
 0000005e t update_GPS_light()
 0000005e T GCS_MAVLINK::_count_parameters()
-00000064 B barometer
 00000064 t mavlink_msg_param_value_send
 00000068 t zero_eeprom()
 00000068 t find_last_log_page(int)
@@ -504,13 +508,12 @@
 0000006c t setup_sonar(unsigned char, Menu::arg const*)
 0000006e T output_min()
 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*)
-00000078 t do_RTL()
 0000007a t setup_factory(unsigned char, Menu::arg const*)
 0000007a t read_control_switch()
 0000007a t report_flight_modes()
-0000007a t test_baro(unsigned char, Menu::arg const*)
+0000007a t do_RTL()
+0000007c t Log_Read_Data()
 0000007c t send_gps_status(mavlink_channel_t)
-0000007e t test_rawgps(unsigned char, Menu::arg const*)
 00000080 T __vector_25
 00000080 T __vector_36
 00000080 T __vector_54
@@ -523,22 +526,19 @@
 00000090 t report_tuning()
 00000092 t test_tuning(unsigned char, Menu::arg const*)
 00000092 T GCS_MAVLINK::queued_param_send()
-00000096 t print_wp(Location*, unsigned char)
 0000009a t planner_gcs(unsigned char, Menu::arg const*)
 0000009a t init_compass()
 0000009a t Log_Read_Motors()
-0000009b B gcs0
-0000009b B gcs3
+0000009c B gcs0
+0000009c B gcs3
 0000009e t setup_mode(unsigned char, Menu::arg const*)
 0000009e t Log_Read_Mode()
-0000009e t Log_Write_Cmd(unsigned char, Location*)
+000000a0 r test_menu_commands
 000000a4 T __vector_26
 000000a4 T __vector_37
 000000a4 T __vector_55
 000000a8 t test_sonar(unsigned char, Menu::arg const*)
-000000aa t Log_Read_Nav_Tuning()
 000000ae t report_frame()
-000000b0 t test_relay(unsigned char, Menu::arg const*)
 000000b2 t erase_logs(unsigned char, Menu::arg const*)
 000000b6 t get_log_boundaries(unsigned char, int&, int&)
 000000b7 B compass
@@ -548,82 +548,79 @@
 000000c2 t send_radio_out(mavlink_channel_t)
 000000c2 t Log_Read_Attitude()
 000000c4 t get_distance(Location*, Location*)
-000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c
 000000c6 t send_radio_in(mavlink_channel_t)
-000000c6 t Log_Read_Performance()
-000000c6 t test_tri(unsigned char, Menu::arg const*)
 000000c7 B dcm
 000000ca t init_barometer()
+000000cc t Log_Read_Nav_Tuning()
+000000cc t Log_Read_Control_Tuning()
 000000d0 t read_radio()
 000000d0 t get_bearing(Location*, Location*)
 000000d0 t print_switch(unsigned char, unsigned char, bool)
-000000d4 t Log_Read(int, int)
+000000d4 t print_wp(Location*, unsigned char)
 000000d8 t test_radio(unsigned char, Menu::arg const*)
 000000d8 t read_barometer()
-000000dc t test_adc(unsigned char, Menu::arg const*)
+000000dc t Log_Read(int, int)
 000000de t setup_motors(unsigned char, Menu::arg const*)
 000000e0 r setup_menu_commands
-000000e4 t test_radio_pwm(unsigned char, Menu::arg const*)
 000000e4 t Log_Read_Optflow()
 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)
 000000e8 t Log_Read_Current()
-000000ea t Log_Read_Control_Tuning()
 000000ee t report_batt_monitor()
+000000ee t Log_Read_Performance()
 000000f6 t Log_Read_Cmd()
-00000100 r test_menu_commands
+000000fa t calc_loiter_pitch_roll()
 0000010a t mavlink_delay(unsigned long)
 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 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)
-0000011c t get_command_with_index(int)
-0000012c t calc_loiter_pitch_roll()
+00000118 t arm_motors()
+0000011c t get_cmd_with_index(int)
 00000130 t report_compass()
-00000138 t get_stabilize_roll(long)
-00000138 t get_stabilize_pitch(long)
-00000148 t Log_Read_GPS()
+00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
+0000013a t test_baro(unsigned char, Menu::arg const*)
+0000014c t get_stabilize_roll(long)
+0000014c t get_stabilize_pitch(long)
 0000014e t send_servo_out(mavlink_channel_t)
-00000156 t update_commands()
+00000152 t init_home()
+00000156 t Log_Read_GPS()
 0000015c t update_trig()
+0000015c t test_wp(unsigned char, Menu::arg const*)
 00000160 t send_location(mavlink_channel_t)
-00000160 t send_nav_controller_output(mavlink_channel_t)
-00000162 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
+00000164 t set_cmd_with_index(Location, int)
 00000166 t select_logs(unsigned char, Menu::arg const*)
 00000166 t send_vfr_hud(mavlink_channel_t)
-00000168 T GCS_MAVLINK::update()
-0000016c t test_imu(unsigned char, Menu::arg const*)
+00000166 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
 0000016e t send_attitude(mavlink_channel_t)
 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int)
+00000178 t verify_nav_wp()
 000001a2 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int)
 000001a8 t print_radio_values()
-000001be t arm_motors()
-000001cc t start_new_log()
-000001e4 t verify_nav_wp()
+000001b2 t start_new_log()
+000001b8 t send_nav_controller_output(mavlink_channel_t)
+000001ce T GCS_MAVLINK::update()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
-000001ea t init_home()
-00000216 t set_mode(unsigned char)
+00000210 t test_imu(unsigned char, Menu::arg const*)
 0000021a t send_raw_imu1(mavlink_channel_t)
-0000021c t test_wp(unsigned char, Menu::arg const*)
 00000228 t setup_radio(unsigned char, Menu::arg const*)
-0000022a t send_gps_raw(mavlink_channel_t)
-00000242 t calc_loiter(int, int)
+00000246 t calc_loiter(int, int)
 00000268 t send_raw_imu3(mavlink_channel_t)
-00000330 t tuning()
+0000026a t send_gps_raw(mavlink_channel_t)
+000002cc t set_mode(unsigned char)
+00000362 t tuning()
 00000382 t print_log_menu()
-0000039a T update_throttle_mode()
+00000396 T update_roll_pitch_mode()
 000003a0 t read_battery()
-00000410 T update_yaw_mode()
-0000046e T update_roll_pitch_mode()
-0000063a t init_ardupilot()
-0000071a t update_nav_wp()
-000007c8 t __static_initialization_and_destruction_0(int, int)
-00000842 b g
-00000870 t process_next_command()
-00000884 W Parameters::Parameters()
-000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-00001836 T loop
+0000041c T update_yaw_mode()
+00000444 T update_throttle_mode()
+00000706 t init_ardupilot()
+000007d8 t __static_initialization_and_destruction_0(int, int)
+00000878 t update_nav_wp()
+000008f6 W Parameters::Parameters()
+000008fa b g
+000009be t update_commands(bool)
+000014f0 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+000019d6 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log
index b2a63cd62c..930bed821c 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.build.log
@@ -3,40 +3,38 @@
 In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
-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:399: warning: overflow in implicit constant conversion
-autogenerated: At global scope:
-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:121: 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/Attitude.pde:132: warning: 'void reset_hold_I()' defined but not used
+autogenerated:33: warning: 'void init_z_damper()' declared 'static' but never defined
+autogenerated:114: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:106: warning: 'void handle_no_commands()' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:433: 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:183: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:184: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:185: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:180: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:187: warning: 'int heli_get_angle_boost(int)' declared 'static' but never defined
 autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined
-autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/navigation.pde:98: warning: 'void calc_loiter2(int, int)' defined but not used
+autogenerated:236: warning: 'int32_t get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:238: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:240: warning: 'int32_t get_alt_distance(Location*, Location*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/radio.pde:132: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:251: warning: 'int32_t 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:262: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:263: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:270: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:271: warning: 'int read_num_from_serial()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:292: 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/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+autogenerated:265: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:266: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:273: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:274: warning: 'int read_num_from_serial()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/system.pde:486: warning: 'void set_failsafe(boolean)' defined but not used
+autogenerated:289: warning: 'void init_optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:265: warning: 'rc_override' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:266: warning: 'rc_override_active' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:267: warning: 'rc_override_fs_timer' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:488: warning: 'pmTest1' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1768: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/test.pde:34: warning: 'int8_t test_rawgps(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
@@ -77,7 +75,7 @@ In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_Optic
 %% libraries/AP_RangeFinder/RangeFinder.o
 %% libraries/AP_Relay/AP_Relay.o
 %% libraries/DataFlash/DataFlash.o
-In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35:
+In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:36:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
 %% libraries/FastSerial/BetterStream.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt
index 7df5b02f4f..f9580e0e2c 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.size.txt
@@ -3,16 +3,17 @@
 00000001 b home_is_set
 00000001 b motor_armed
 00000001 b motor_light
+00000001 b CH7_wp_index
 00000001 b control_mode
 00000001 b gps_watchdog
 00000001 b simple_timer
 00000001 d yaw_tracking
 00000001 b land_complete
 00000001 b throttle_mode
-00000001 b command_may_ID
+00000001 b mavlink_active
+00000001 b prev_nav_index
 00000001 b wp_verify_byte
 00000001 d altitude_sensor
-00000001 b command_must_ID
 00000001 b command_yaw_dir
 00000001 b new_radio_frame
 00000001 b roll_pitch_mode
@@ -24,13 +25,14 @@
 00000001 b old_control_mode
 00000001 b slow_loopCounter
 00000001 b takeoff_complete
-00000001 b command_may_index
+00000001 b command_nav_index
 00000001 b oldSwitchPosition
-00000001 b command_must_index
+00000001 b command_cond_index
 00000001 d ground_start_count
 00000001 b medium_loopCounter
 00000001 b command_yaw_relative
 00000001 d jump
+00000001 b nav_ok
 00000001 b event_id
 00000001 b led_mode
 00000001 b yaw_mode
@@ -45,20 +47,25 @@
 00000002 T userhook_init()
 00000002 b climb_rate
 00000002 b loiter_sum
+00000002 b sonar_rate
+00000002 b angle_boost
 00000002 b event_delay
 00000002 b event_value
 00000002 b event_repeat
 00000002 b loiter_total
+00000002 b manual_boost
 00000002 b nav_throttle
+00000002 b old_baro_alt
 00000002 b x_rate_error
 00000002 b y_rate_error
-00000002 b altitude_rate
 00000002 b gps_fix_count
+00000002 b old_sonar_alt
 00000002 b velocity_land
 00000002 b x_actual_speed
 00000002 b y_actual_speed
 00000002 b loiter_time_max
 00000002 b command_yaw_time
+00000002 b crosstrack_error
 00000002 b event_undo_value
 00000002 b command_yaw_speed
 00000002 b auto_level_counter
@@ -69,6 +76,7 @@
 00000002 b g_gps
 00000002 b airspeed
 00000002 b baro_alt
+00000002 b baro_rate
 00000002 b sonar_alt
 00000002 b arm_motors()::arming_counter
 00000002 r setup_frame(unsigned char, Menu::arg const*)::__c
@@ -82,6 +90,9 @@
 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c
 00000003 r print_enabled(unsigned char)::__c
 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c
+00000003 r report_batt_monitor()::__c
+00000003 r report_batt_monitor()::__c
+00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 d cos_roll_x
 00000004 b land_start
 00000004 b long_error
@@ -93,15 +104,14 @@
 00000004 b sin_pitch_y
 00000004 b wp_distance
 00000004 b abs_pressure
+00000004 b circle_angle
 00000004 b current_amps
-00000004 b old_altitude
 00000004 b original_alt
 00000004 b simple_cos_x
 00000004 b simple_sin_y
 00000004 b current_total
 00000004 b nav_loopTimer
 00000004 d scaleLongDown
-00000004 t test_failsafe(unsigned char, Menu::arg const*)
 00000004 b altitude_error
 00000004 b fast_loopTimer
 00000004 b perf_mon_timer
@@ -152,7 +162,6 @@
 00000004 r setup_compass(unsigned char, Menu::arg const*)::__c
 00000004 r print_log_menu()::__c
 00000004 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
-00000004 r test_adc(unsigned char, Menu::arg const*)::__c
 00000004 V Parameters::Parameters()::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
@@ -171,7 +180,6 @@
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
-00000005 r test_adc(unsigned char, Menu::arg const*)::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
@@ -193,6 +201,7 @@
 00000006 r Log_Read_Mode()::__c
 00000006 r print_log_menu()::__c
 00000006 r print_log_menu()::__c
+00000006 r report_batt_monitor()::__c
 00000006 V Parameters::Parameters()::__c
 00000007 b setup_menu
 00000007 b planner_menu
@@ -205,6 +214,8 @@
 00000007 r report_radio()::__c
 00000007 r report_sonar()::__c
 00000007 r print_enabled(unsigned char)::__c
+00000007 r Log_Read_Nav_Tuning()::__c
+00000007 r Log_Read_Control_Tuning()::__c
 00000007 r test_wp(unsigned char, Menu::arg const*)::__c
 00000007 V Parameters::Parameters()::__c
 00000007 V Parameters::Parameters()::__c
@@ -214,6 +225,7 @@
 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 r print_done()::__c
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -221,8 +233,6 @@
 00000008 r report_tuning()::__c
 00000008 r init_ardupilot()::__c
 00000008 r print_log_menu()::__c
-00000008 r report_batt_monitor()::__c
-00000008 r report_batt_monitor()::__c
 00000008 r test_wp(unsigned char, Menu::arg const*)::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
@@ -230,7 +240,6 @@
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
-00000008 V Parameters::Parameters()::__c
 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000009 r print_switch(unsigned char, unsigned char, bool)::__c
 00000009 r print_log_menu()::__c
@@ -260,7 +269,6 @@
 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 0000000a T piezo_on()
 0000000a T piezo_off()
-0000000a r test_relay(unsigned char, Menu::arg const*)::__c
 0000000a r report_frame()::__c
 0000000a r start_new_log()::__c
 0000000a r print_log_menu()::__c
@@ -271,9 +279,10 @@
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
 0000000a T setup
-0000000b r test_relay(unsigned char, Menu::arg const*)::__c
-0000000b r report_batt_monitor()::__c
+0000000b r setup_esc(unsigned char, Menu::arg const*)::__c
 0000000b V Parameters::Parameters()::__c
 0000000c t process_logs(unsigned char, Menu::arg const*)
 0000000c b omega
@@ -282,11 +291,13 @@
 0000000c V vtable for IMU
 0000000c r print_switch(unsigned char, unsigned char, bool)::__c
 0000000c r setup_factory(unsigned char, Menu::arg const*)::__c
-0000000c r test_baro(unsigned char, Menu::arg const*)::__c
+0000000c r report_version()::__c
+0000000c r report_batt_monitor()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000d r select_logs(unsigned char, Menu::arg const*)::__c
+0000000d r setup_frame(unsigned char, Menu::arg const*)::__c
 0000000d r test_tuning(unsigned char, Menu::arg const*)::__c
 0000000d r test_battery(unsigned char, Menu::arg const*)::__c
 0000000d r startup_ground()::__c
@@ -298,6 +309,8 @@
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
 0000000d B sonar_mode_filter
 0000000e t global destructors keyed to Serial
 0000000e t global constructors keyed to Serial
@@ -313,6 +326,7 @@
 0000000e r setup_mode(unsigned char, Menu::arg const*)::__c
 0000000e r test_sonar(unsigned char, Menu::arg const*)::__c
 0000000e r select_logs(unsigned char, Menu::arg const*)::__c
+0000000e r setup_compass(unsigned char, Menu::arg const*)::__c
 0000000e r print_log_menu()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
@@ -321,6 +335,7 @@
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
+0000000e r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 0000000e r report_batt_monitor()::__c
 0000000e r report_flight_modes()::__c
 0000000e r dump_log(unsigned char, Menu::arg const*)::__c
@@ -332,19 +347,18 @@
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
-0000000e V Parameters::Parameters()::__c
 0000000f b current_loc
-0000000f b next_command
+0000000f b command_nav_queue
+0000000f b command_cond_queue
 0000000f b home
 0000000f b next_WP
 0000000f b prev_WP
 0000000f b guided_WP
 0000000f b target_WP
+0000000f r setup_sonar(unsigned char, Menu::arg const*)::__c
+0000000f r Log_Read_Data()::__c
 0000000f r print_log_menu()::__c
 0000000f r print_log_menu()::__c
-0000000f r report_version()::__c
-0000000f r report_batt_monitor()::__c
-0000000f r test_imu(unsigned char, Menu::arg const*)::__c
 0000000f V AP_IMU_Oilpan::AP_IMU_Oilpan(AP_ADC*, unsigned int)::__c
 00000010 r planner_menu_commands
 00000010 b motor_out
@@ -354,7 +368,6 @@
 00000010 r report_compass()::__c
 00000011 r erase_logs(unsigned char, Menu::arg const*)::__c
 00000011 r zero_eeprom()::__c
-00000011 r update_commands()::__c
 00000012 B Serial
 00000012 B Serial1
 00000012 B Serial3
@@ -368,30 +381,22 @@
 00000012 W AP_VarT<float>::~AP_VarT()
 00000012 W AP_VarT<int>::~AP_VarT()
 00000012 W AP_VarT<signed char>::serialize(void*, unsigned int) const
-00000012 r print_done()::__c
 00000012 r select_logs(unsigned char, Menu::arg const*)::__c
-00000012 r setup_frame(unsigned char, Menu::arg const*)::__c
 00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
-00000013 r setup_compass(unsigned char, Menu::arg const*)::__c
-00000013 r change_command(unsigned char)::__c
-00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 00000014 W AP_VarT<signed char>::unserialize(void*, unsigned int)
 00000014 W AP_VarT<signed char>::cast_to_float() const
 00000014 W AP_VarT<int>::cast_to_float() const
-00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c
-00000014 r test_tri(unsigned char, Menu::arg const*)::__c
 00000015 r init_ardupilot()::__c
-00000015 r Log_Read_Motors()::__c
 00000015 r print_hit_enter()::__c
 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
 00000016 T piezo_beep()
+00000016 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000016 B sonar
 00000018 t setup_tune(unsigned char, Menu::arg const*)
 00000018 t setup_accel(unsigned char, Menu::arg const*)
 00000018 r __menu_name__main_menu
 00000018 W AP_VarT<int>::serialize(void*, unsigned int) const
 0000001a r print_log_menu()::__c
-0000001a r Log_Read_Nav_Tuning()::__c
 0000001c W AP_VarA<float, (unsigned char)6>::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Matrix3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Vector3<float> >::unserialize(void*, unsigned int)
@@ -399,18 +404,21 @@
 0000001c W AP_VarA<float, (unsigned char)6>::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Matrix3<float> >::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Vector3<float> >::serialize(void*, unsigned int) const
+0000001c r print_gyro_offsets()::__c
+0000001c r print_accel_offsets()::__c
+0000001d r report_compass()::__c
+0000001d r Log_Read_Motors()::__c
 0000001d r Log_Read_Attitude()::__c
+0000001d r Log_Read_Performance()::__c
 0000001e r Log_Read_Optflow()::__c
 00000020 t gcs_send_message(ap_message)
 00000020 r test_current(unsigned char, Menu::arg const*)::__c
 00000020 t byte_swap_4
+00000021 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000021 r print_log_menu()::__c
-00000021 r report_compass()::__c
 00000021 r Log_Read_Current()::__c
-00000021 r Log_Read_Performance()::__c
 00000022 t clear_leds()
 00000022 t print_blanks(int)
-00000022 t reset_hold_I()
 00000022 t startup_ground()
 00000022 W AP_Float16::~AP_Float16()
 00000022 W AP_VarA<float, (unsigned char)6>::~AP_VarA()
@@ -419,52 +427,49 @@
 00000022 W AP_VarT<signed char>::~AP_VarT()
 00000022 W AP_VarT<float>::~AP_VarT()
 00000022 W AP_VarT<int>::~AP_VarT()
-00000023 r setup_mode(unsigned char, Menu::arg const*)::__c
-00000023 r print_gyro_offsets()::__c
+00000022 r test_imu(unsigned char, Menu::arg const*)::__c
+00000023 r test_baro(unsigned char, Menu::arg const*)::__c
 00000024 r init_ardupilot()::__c
-00000024 r print_accel_offsets()::__c
 00000026 t print_done()
+00000026 t Log_Write_Data(signed char, float)
 00000026 t print_hit_enter()
 00000026 t Log_Read_Startup()
-00000026 r Log_Read_Control_Tuning()::__c
 00000028 t test_battery(unsigned char, Menu::arg const*)
 00000028 t main_menu_help(unsigned char, Menu::arg const*)
-00000028 t help_log(unsigned char, Menu::arg const*)
 00000028 W AP_VarT<float>::unserialize(void*, unsigned int)
 00000028 W AP_VarT<float>::serialize(void*, unsigned int) const
 00000028 r Log_Read_Cmd()::__c
-00000029 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
+0000002a t _mav_put_int8_t_array
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
 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)
 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char)
+0000002e r init_ardupilot()::__c
 0000002f r test_radio(unsigned char, Menu::arg const*)::__c
-0000002f r init_ardupilot()::__c
+0000002f r setup_factory(unsigned char, Menu::arg const*)::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 r setup_radio(unsigned char, Menu::arg const*)::__c
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
 00000032 r Log_Read_GPS()::__c
 00000033 b pending_status
 00000034 W AP_Float16::serialize(void*, unsigned int) const
-00000034 t _mav_put_int8_t_array
 00000034 t mavlink_msg_statustext_send
-00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c
 00000036 t report_radio()
-00000037 r print_wp(Location*, unsigned char)::__c
-00000038 t init_throttle_cruise()
+00000036 r print_wp(Location*, unsigned char)::__c
 00000038 t send_current_waypoint(mavlink_channel_t)
-00000038 r setup_radio(unsigned char, Menu::arg const*)::__c
-00000038 r setup_factory(unsigned char, Menu::arg const*)::__c
 0000003a t report_gps()
 0000003a t report_imu()
-0000003c W RC_Channel::~RC_Channel()
+0000003c B barometer
 0000003d B g_gps_driver
 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*)
 0000003e W AP_VarT<signed char>::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char)
+00000040 r log_menu_commands
+00000040 t init_throttle_cruise()
 00000040 t read_AHRS()
 00000040 W AP_Float16::unserialize(void*, unsigned int)
 00000040 B adc
@@ -472,14 +477,13 @@
 00000042 t report_sonar()
 00000044 t setup_show(unsigned char, Menu::arg const*)
 00000044 W AP_VarT<int>::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char)
-00000048 t change_command(unsigned char)
+00000044 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
+00000046 W RC_Channel::~RC_Channel()
 00000048 t update_motor_leds()
-00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
 0000004a t send_meminfo(mavlink_channel_t)
 0000004a W AP_VarT<int>::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char)
 0000004c t update_auto_yaw()
 00000050 b mavlink_queue
-00000050 r log_menu_commands
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
 00000050 B imu
@@ -489,13 +493,13 @@
 00000056 t readSwitch()
 00000056 t dancing_light()
 00000056 T GCS_MAVLINK::queued_waypoint_send()
-00000057 r help_log(unsigned char, Menu::arg const*)::__c
+0000005a t Log_Write_Data(signed char, long)
 0000005a W AP_VarT<float>::AP_VarT(float, unsigned int, prog_char_t const*, unsigned char)
 0000005c t get_num_logs()
 0000005c t setup_esc(unsigned char, Menu::arg const*)
+0000005e t change_command(unsigned char)
 0000005e t update_GPS_light()
 0000005e T GCS_MAVLINK::_count_parameters()
-00000064 B barometer
 00000064 t mavlink_msg_param_value_send
 00000068 t zero_eeprom()
 00000068 t find_last_log_page(int)
@@ -504,13 +508,12 @@
 0000006c t setup_sonar(unsigned char, Menu::arg const*)
 0000006e T output_min()
 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*)
-00000078 t do_RTL()
 0000007a t setup_factory(unsigned char, Menu::arg const*)
 0000007a t read_control_switch()
 0000007a t report_flight_modes()
+0000007a t do_RTL()
+0000007c t Log_Read_Data()
 0000007c t send_gps_status(mavlink_channel_t)
-0000007c t test_baro(unsigned char, Menu::arg const*)
-0000007e t test_rawgps(unsigned char, Menu::arg const*)
 00000080 T __vector_25
 00000080 T __vector_36
 00000080 T __vector_54
@@ -523,107 +526,101 @@
 00000092 t test_tuning(unsigned char, Menu::arg const*)
 00000092 t report_tuning()
 00000092 T GCS_MAVLINK::queued_param_send()
-00000096 t print_wp(Location*, unsigned char)
 0000009a t planner_gcs(unsigned char, Menu::arg const*)
 0000009a t init_compass()
-0000009a t Log_Read_Motors()
-0000009b B gcs0
-0000009b B gcs3
+0000009c B gcs0
+0000009c B gcs3
 0000009e t setup_mode(unsigned char, Menu::arg const*)
-0000009e t Log_Write_Cmd(unsigned char, Location*)
 000000a0 t Log_Read_Mode()
+000000a0 r test_menu_commands
 000000a4 T __vector_26
 000000a4 T __vector_37
 000000a4 T __vector_55
 000000aa t test_sonar(unsigned char, Menu::arg const*)
-000000aa t Log_Read_Nav_Tuning()
 000000ae t report_frame()
-000000b0 t test_relay(unsigned char, Menu::arg const*)
 000000b2 t erase_logs(unsigned char, Menu::arg const*)
 000000b6 t get_log_boundaries(unsigned char, int&, int&)
 000000b7 B compass
 000000be t update_events()
 000000c2 t setup_compass(unsigned char, Menu::arg const*)
 000000c2 t send_radio_out(mavlink_channel_t)
+000000c2 t Log_Read_Motors()
 000000c2 t Log_Read_Attitude()
 000000c4 t get_distance(Location*, Location*)
-000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c
 000000c6 t test_eedump(unsigned char, Menu::arg const*)
 000000c6 t send_radio_in(mavlink_channel_t)
-000000c6 t Log_Read_Performance()
-000000c6 t test_tri(unsigned char, Menu::arg const*)
 000000c7 B dcm
 000000ca t init_barometer()
 000000d0 t read_radio()
 000000d0 t get_bearing(Location*, Location*)
+000000d0 t Log_Read_Nav_Tuning()
+000000d0 t Log_Read_Control_Tuning()
 000000d2 t print_switch(unsigned char, unsigned char, bool)
-000000d4 t Log_Read(int, int)
+000000d4 t print_wp(Location*, unsigned char)
 000000d8 t test_radio(unsigned char, Menu::arg const*)
 000000d8 t read_barometer()
-000000de t test_adc(unsigned char, Menu::arg const*)
+000000dc t Log_Read(int, int)
 000000e0 r setup_menu_commands
-000000e4 t test_radio_pwm(unsigned char, Menu::arg const*)
 000000e4 t Log_Read_Optflow()
 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)
 000000e8 t Log_Read_Current()
-000000ea t Log_Read_Control_Tuning()
 000000ee t report_batt_monitor()
+000000ee t Log_Read_Performance()
 000000f6 t Log_Read_Cmd()
-00000100 r test_menu_commands
+000000fa t calc_loiter_pitch_roll()
 0000010a t mavlink_delay(unsigned long)
 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 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)
-0000011c t get_command_with_index(int)
-0000012c t calc_loiter_pitch_roll()
+00000118 t arm_motors()
+0000011c t get_cmd_with_index(int)
 00000130 t report_compass()
-00000138 t get_stabilize_roll(long)
-00000138 t get_stabilize_pitch(long)
-00000148 t Log_Read_GPS()
+00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
+0000013a t test_baro(unsigned char, Menu::arg const*)
+0000014c t get_stabilize_roll(long)
+0000014c t get_stabilize_pitch(long)
 0000014e t send_servo_out(mavlink_channel_t)
-00000156 t update_commands()
+00000152 t init_home()
+00000156 t Log_Read_GPS()
 0000015c t update_trig()
 0000015c t setup_motors(unsigned char, Menu::arg const*)
 00000160 t send_location(mavlink_channel_t)
-00000160 t send_nav_controller_output(mavlink_channel_t)
-00000162 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
+00000160 t test_wp(unsigned char, Menu::arg const*)
+00000164 t set_cmd_with_index(Location, int)
 00000166 t select_logs(unsigned char, Menu::arg const*)
 00000166 t send_vfr_hud(mavlink_channel_t)
-00000168 T GCS_MAVLINK::update()
-0000016c t test_imu(unsigned char, Menu::arg const*)
+00000166 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
 0000016e t send_attitude(mavlink_channel_t)
 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int)
+0000017a t verify_nav_wp()
 000001a2 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int)
 000001a8 t print_radio_values()
-000001be t arm_motors()
-000001cc t start_new_log()
+000001b2 t start_new_log()
+000001b8 t send_nav_controller_output(mavlink_channel_t)
+000001ce T GCS_MAVLINK::update()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
-000001e6 t verify_nav_wp()
-000001ea t init_home()
-00000216 t set_mode(unsigned char)
+00000210 t test_imu(unsigned char, Menu::arg const*)
 0000021a t send_raw_imu1(mavlink_channel_t)
-00000220 t test_wp(unsigned char, Menu::arg const*)
 00000228 t setup_radio(unsigned char, Menu::arg const*)
-0000022a t send_gps_raw(mavlink_channel_t)
-00000242 t calc_loiter(int, int)
+00000246 t calc_loiter(int, int)
 00000268 t send_raw_imu3(mavlink_channel_t)
-00000330 t tuning()
+0000026a t send_gps_raw(mavlink_channel_t)
+000002cc t set_mode(unsigned char)
+00000362 t tuning()
 00000384 t print_log_menu()
-0000039a T update_throttle_mode()
+00000396 T update_roll_pitch_mode()
 000003a0 t read_battery()
-00000410 T update_yaw_mode()
-0000046e T update_roll_pitch_mode()
-00000638 t init_ardupilot()
-0000071a t update_nav_wp()
-000007c8 t __static_initialization_and_destruction_0(int, int)
-00000842 b g
-00000870 t process_next_command()
-00000884 W Parameters::Parameters()
-000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-0000190e T loop
+0000041c T update_yaw_mode()
+00000444 T update_throttle_mode()
+00000704 t init_ardupilot()
+000007d8 t __static_initialization_and_destruction_0(int, int)
+00000878 t update_nav_wp()
+000008f6 W Parameters::Parameters()
+000008fa b g
+000009be t update_commands(bool)
+000014f0 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+00001ce4 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log
index b2a63cd62c..930bed821c 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.build.log
@@ -3,40 +3,38 @@
 In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:55:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
-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:399: warning: overflow in implicit constant conversion
-autogenerated: At global scope:
-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:121: 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/Attitude.pde:132: warning: 'void reset_hold_I()' defined but not used
+autogenerated:33: warning: 'void init_z_damper()' declared 'static' but never defined
+autogenerated:114: warning: 'void Log_Write_Optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:106: warning: 'void handle_no_commands()' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/commands_logic.pde:433: 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:183: warning: 'void heli_init_swash()' declared 'static' but never defined
-autogenerated:184: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
-autogenerated:185: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:180: warning: 'void heli_init_swash()' declared 'static' but never defined
+autogenerated:181: warning: 'void heli_move_servos_to_mid()' declared 'static' but never defined
+autogenerated:182: warning: 'void heli_move_swash(int, int, int, int)' declared 'static' but never defined
+autogenerated:187: warning: 'int heli_get_angle_boost(int)' declared 'static' but never defined
 autogenerated:213: warning: 'void debug_motors()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/navigation.pde:183: warning: 'int get_loiter_angle()' defined but not used
-autogenerated:234: warning: 'long int get_crosstrack_correction()' declared 'static' but never defined
-autogenerated:235: warning: 'long int cross_track_test()' declared 'static' but never defined
-autogenerated:236: warning: 'void reset_crosstrack()' declared 'static' but never defined
-autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/radio.pde:130: warning: 'void throttle_failsafe(uint16_t)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/navigation.pde:98: warning: 'void calc_loiter2(int, int)' defined but not used
+autogenerated:236: warning: 'int32_t get_crosstrack_correction()' declared 'static' but never defined
+autogenerated:238: warning: 'void reset_crosstrack()' declared 'static' but never defined
+autogenerated:240: warning: 'int32_t get_alt_distance(Location*, Location*)' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/radio.pde:132: warning: 'void throttle_failsafe(uint16_t)' defined but not used
 /root/apm/ardupilot-mega/ArduCopter/sensors.pde:6: warning: 'void ReadSCP1000()' defined but not used
-autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined
+autogenerated:251: warning: 'int32_t 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:262: warning: 'void report_heli()' declared 'static' but never defined
-autogenerated:263: warning: 'void report_gyro()' declared 'static' but never defined
-autogenerated:270: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
-autogenerated:271: warning: 'int read_num_from_serial()' declared 'static' but never defined
-/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used
-autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined
-autogenerated:292: 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/GCS_Mavlink.pde:1706: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+autogenerated:265: warning: 'void report_heli()' declared 'static' but never defined
+autogenerated:266: warning: 'void report_gyro()' declared 'static' but never defined
+autogenerated:273: warning: 'RC_Channel* heli_get_servo(int)' declared 'static' but never defined
+autogenerated:274: warning: 'int read_num_from_serial()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/system.pde:486: warning: 'void set_failsafe(boolean)' defined but not used
+autogenerated:289: warning: 'void init_optflow()' declared 'static' but never defined
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:265: warning: 'rc_override' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:266: warning: 'rc_override_active' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:267: warning: 'rc_override_fs_timer' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:488: warning: 'pmTest1' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/GCS_Mavlink.pde:1768: warning: 'void gcs_send_text_fmt(const prog_char_t*, ...)' defined but not used
+/root/apm/ardupilot-mega/ArduCopter/test.pde:34: warning: 'int8_t test_rawgps(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
@@ -77,7 +75,7 @@ In file included from /root/apm/ardupilot-mega/libraries/AP_OpticalFlow/AP_Optic
 %% libraries/AP_RangeFinder/RangeFinder.o
 %% libraries/AP_Relay/AP_Relay.o
 %% libraries/DataFlash/DataFlash.o
-In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:35:
+In file included from /root/apm/ardupilot-mega/libraries/DataFlash/DataFlash.cpp:36:
 /usr/local/share/arduino/libraries/SPI/SPI.h:25:1: warning: "SPI_CLOCK_DIV64" redefined
 /usr/local/share/arduino/libraries/SPI/SPI.h:20:1: warning: this is the location of the previous definition
 %% libraries/FastSerial/BetterStream.o
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt
index 7895b8e72b..553b7223a9 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt
+++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.size.txt
@@ -3,16 +3,17 @@
 00000001 b home_is_set
 00000001 b motor_armed
 00000001 b motor_light
+00000001 b CH7_wp_index
 00000001 b control_mode
 00000001 b gps_watchdog
 00000001 b simple_timer
 00000001 d yaw_tracking
 00000001 b land_complete
 00000001 b throttle_mode
-00000001 b command_may_ID
+00000001 b mavlink_active
+00000001 b prev_nav_index
 00000001 b wp_verify_byte
 00000001 d altitude_sensor
-00000001 b command_must_ID
 00000001 b command_yaw_dir
 00000001 b new_radio_frame
 00000001 b roll_pitch_mode
@@ -24,13 +25,14 @@
 00000001 b old_control_mode
 00000001 b slow_loopCounter
 00000001 b takeoff_complete
-00000001 b command_may_index
+00000001 b command_nav_index
 00000001 b oldSwitchPosition
-00000001 b command_must_index
+00000001 b command_cond_index
 00000001 d ground_start_count
 00000001 b medium_loopCounter
 00000001 b command_yaw_relative
 00000001 d jump
+00000001 b nav_ok
 00000001 b event_id
 00000001 b led_mode
 00000001 b yaw_mode
@@ -45,20 +47,25 @@
 00000002 T userhook_init()
 00000002 b climb_rate
 00000002 b loiter_sum
+00000002 b sonar_rate
+00000002 b angle_boost
 00000002 b event_delay
 00000002 b event_value
 00000002 b event_repeat
 00000002 b loiter_total
+00000002 b manual_boost
 00000002 b nav_throttle
+00000002 b old_baro_alt
 00000002 b x_rate_error
 00000002 b y_rate_error
-00000002 b altitude_rate
 00000002 b gps_fix_count
+00000002 b old_sonar_alt
 00000002 b velocity_land
 00000002 b x_actual_speed
 00000002 b y_actual_speed
 00000002 b loiter_time_max
 00000002 b command_yaw_time
+00000002 b crosstrack_error
 00000002 b event_undo_value
 00000002 b command_yaw_speed
 00000002 b auto_level_counter
@@ -69,6 +76,7 @@
 00000002 b g_gps
 00000002 b airspeed
 00000002 b baro_alt
+00000002 b baro_rate
 00000002 b sonar_alt
 00000002 b arm_motors()::arming_counter
 00000002 r setup_frame(unsigned char, Menu::arg const*)::__c
@@ -82,6 +90,9 @@
 00000003 r setup_sonar(unsigned char, Menu::arg const*)::__c
 00000003 r print_enabled(unsigned char)::__c
 00000003 r setup_compass(unsigned char, Menu::arg const*)::__c
+00000003 r report_batt_monitor()::__c
+00000003 r report_batt_monitor()::__c
+00000003 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 d cos_roll_x
 00000004 b land_start
 00000004 b long_error
@@ -93,15 +104,14 @@
 00000004 b sin_pitch_y
 00000004 b wp_distance
 00000004 b abs_pressure
+00000004 b circle_angle
 00000004 b current_amps
-00000004 b old_altitude
 00000004 b original_alt
 00000004 b simple_cos_x
 00000004 b simple_sin_y
 00000004 b current_total
 00000004 b nav_loopTimer
 00000004 d scaleLongDown
-00000004 t test_failsafe(unsigned char, Menu::arg const*)
 00000004 b altitude_error
 00000004 b fast_loopTimer
 00000004 b perf_mon_timer
@@ -152,7 +162,6 @@
 00000004 r setup_compass(unsigned char, Menu::arg const*)::__c
 00000004 r print_log_menu()::__c
 00000004 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
-00000004 r test_adc(unsigned char, Menu::arg const*)::__c
 00000004 V Parameters::Parameters()::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
 00000004 V RC_Channel::RC_Channel(unsigned int, prog_char_t const*)::__c
@@ -171,7 +180,6 @@
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
 00000005 r print_log_menu()::__c
-00000005 r test_adc(unsigned char, Menu::arg const*)::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
 00000005 V Parameters::Parameters()::__c
@@ -193,6 +201,7 @@
 00000006 r Log_Read_Mode()::__c
 00000006 r print_log_menu()::__c
 00000006 r print_log_menu()::__c
+00000006 r report_batt_monitor()::__c
 00000006 V Parameters::Parameters()::__c
 00000007 b setup_menu
 00000007 b planner_menu
@@ -205,6 +214,8 @@
 00000007 r report_radio()::__c
 00000007 r report_sonar()::__c
 00000007 r print_enabled(unsigned char)::__c
+00000007 r Log_Read_Nav_Tuning()::__c
+00000007 r Log_Read_Control_Tuning()::__c
 00000007 r test_wp(unsigned char, Menu::arg const*)::__c
 00000007 V Parameters::Parameters()::__c
 00000007 V Parameters::Parameters()::__c
@@ -214,6 +225,7 @@
 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 r print_done()::__c
 00000008 r select_logs(unsigned char, Menu::arg const*)::__c
 00000008 r report_frame()::__c
 00000008 r report_frame()::__c
@@ -221,8 +233,6 @@
 00000008 r report_tuning()::__c
 00000008 r init_ardupilot()::__c
 00000008 r print_log_menu()::__c
-00000008 r report_batt_monitor()::__c
-00000008 r report_batt_monitor()::__c
 00000008 r test_wp(unsigned char, Menu::arg const*)::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
@@ -230,7 +240,6 @@
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
 00000008 V Parameters::Parameters()::__c
-00000008 V Parameters::Parameters()::__c
 00000008 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 00000009 r print_switch(unsigned char, unsigned char, bool)::__c
 00000009 r print_log_menu()::__c
@@ -260,7 +269,6 @@
 00000009 r GCS_MAVLINK::GCS_MAVLINK(unsigned int)::__c
 0000000a T piezo_on()
 0000000a T piezo_off()
-0000000a r test_relay(unsigned char, Menu::arg const*)::__c
 0000000a r report_frame()::__c
 0000000a r start_new_log()::__c
 0000000a r print_log_menu()::__c
@@ -271,9 +279,10 @@
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
 0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
+0000000a V Parameters::Parameters()::__c
 0000000a T setup
-0000000b r test_relay(unsigned char, Menu::arg const*)::__c
-0000000b r report_batt_monitor()::__c
+0000000b r setup_esc(unsigned char, Menu::arg const*)::__c
 0000000b V Parameters::Parameters()::__c
 0000000c t process_logs(unsigned char, Menu::arg const*)
 0000000c b omega
@@ -282,11 +291,13 @@
 0000000c V vtable for IMU
 0000000c r print_switch(unsigned char, unsigned char, bool)::__c
 0000000c r setup_factory(unsigned char, Menu::arg const*)::__c
-0000000c r test_baro(unsigned char, Menu::arg const*)::__c
+0000000c r report_version()::__c
+0000000c r report_batt_monitor()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000c V Parameters::Parameters()::__c
 0000000d r select_logs(unsigned char, Menu::arg const*)::__c
+0000000d r setup_frame(unsigned char, Menu::arg const*)::__c
 0000000d r test_tuning(unsigned char, Menu::arg const*)::__c
 0000000d r test_battery(unsigned char, Menu::arg const*)::__c
 0000000d r startup_ground()::__c
@@ -298,6 +309,8 @@
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
 0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
+0000000d V Parameters::Parameters()::__c
 0000000d B sonar_mode_filter
 0000000e t global destructors keyed to Serial
 0000000e t global constructors keyed to Serial
@@ -313,6 +326,7 @@
 0000000e r setup_mode(unsigned char, Menu::arg const*)::__c
 0000000e r test_sonar(unsigned char, Menu::arg const*)::__c
 0000000e r select_logs(unsigned char, Menu::arg const*)::__c
+0000000e r setup_compass(unsigned char, Menu::arg const*)::__c
 0000000e r print_log_menu()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
@@ -321,6 +335,7 @@
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
 0000000e r print_radio_values()::__c
+0000000e r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 0000000e r report_batt_monitor()::__c
 0000000e r report_flight_modes()::__c
 0000000e r dump_log(unsigned char, Menu::arg const*)::__c
@@ -332,19 +347,18 @@
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
 0000000e V Parameters::Parameters()::__c
-0000000e V Parameters::Parameters()::__c
 0000000f b current_loc
-0000000f b next_command
+0000000f b command_nav_queue
+0000000f b command_cond_queue
 0000000f b home
 0000000f b next_WP
 0000000f b prev_WP
 0000000f b guided_WP
 0000000f b target_WP
+0000000f r setup_sonar(unsigned char, Menu::arg const*)::__c
+0000000f r Log_Read_Data()::__c
 0000000f r print_log_menu()::__c
 0000000f r print_log_menu()::__c
-0000000f r report_version()::__c
-0000000f r report_batt_monitor()::__c
-0000000f r test_imu(unsigned char, Menu::arg const*)::__c
 0000000f V AP_IMU_Oilpan::AP_IMU_Oilpan(AP_ADC*, unsigned int)::__c
 00000010 r planner_menu_commands
 00000010 b motor_out
@@ -354,7 +368,6 @@
 00000010 r report_compass()::__c
 00000011 r erase_logs(unsigned char, Menu::arg const*)::__c
 00000011 r zero_eeprom()::__c
-00000011 r update_commands()::__c
 00000012 B Serial
 00000012 B Serial1
 00000012 B Serial3
@@ -368,30 +381,22 @@
 00000012 W AP_VarT<float>::~AP_VarT()
 00000012 W AP_VarT<int>::~AP_VarT()
 00000012 W AP_VarT<signed char>::serialize(void*, unsigned int) const
-00000012 r print_done()::__c
 00000012 r select_logs(unsigned char, Menu::arg const*)::__c
-00000012 r setup_frame(unsigned char, Menu::arg const*)::__c
 00000012 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
-00000013 r setup_compass(unsigned char, Menu::arg const*)::__c
-00000013 r change_command(unsigned char)::__c
-00000013 r setup_batt_monitor(unsigned char, Menu::arg const*)::__c
 00000014 W AP_VarT<signed char>::unserialize(void*, unsigned int)
 00000014 W AP_VarT<signed char>::cast_to_float() const
 00000014 W AP_VarT<int>::cast_to_float() const
-00000014 r setup_sonar(unsigned char, Menu::arg const*)::__c
-00000014 r test_tri(unsigned char, Menu::arg const*)::__c
 00000015 r init_ardupilot()::__c
-00000015 r Log_Read_Motors()::__c
 00000015 r print_hit_enter()::__c
 00000015 r GCS_MAVLINK::handleMessage(__mavlink_message*)::__c
 00000016 T piezo_beep()
+00000016 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000016 B sonar
 00000018 t setup_tune(unsigned char, Menu::arg const*)
 00000018 t setup_accel(unsigned char, Menu::arg const*)
 00000018 r __menu_name__main_menu
 00000018 W AP_VarT<int>::serialize(void*, unsigned int) const
 0000001a r print_log_menu()::__c
-0000001a r Log_Read_Nav_Tuning()::__c
 0000001c W AP_VarA<float, (unsigned char)6>::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Matrix3<float> >::unserialize(void*, unsigned int)
 0000001c W AP_VarS<Vector3<float> >::unserialize(void*, unsigned int)
@@ -399,18 +404,21 @@
 0000001c W AP_VarA<float, (unsigned char)6>::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Matrix3<float> >::serialize(void*, unsigned int) const
 0000001c W AP_VarS<Vector3<float> >::serialize(void*, unsigned int) const
+0000001c r print_gyro_offsets()::__c
+0000001c r print_accel_offsets()::__c
+0000001d r report_compass()::__c
+0000001d r Log_Read_Motors()::__c
 0000001d r Log_Read_Attitude()::__c
+0000001d r Log_Read_Performance()::__c
 0000001e r Log_Read_Optflow()::__c
 00000020 t gcs_send_message(ap_message)
 00000020 r test_current(unsigned char, Menu::arg const*)::__c
 00000020 t byte_swap_4
+00000021 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000021 r print_log_menu()::__c
-00000021 r report_compass()::__c
 00000021 r Log_Read_Current()::__c
-00000021 r Log_Read_Performance()::__c
 00000022 t clear_leds()
 00000022 t print_blanks(int)
-00000022 t reset_hold_I()
 00000022 t startup_ground()
 00000022 W AP_Float16::~AP_Float16()
 00000022 W AP_VarA<float, (unsigned char)6>::~AP_VarA()
@@ -419,52 +427,49 @@
 00000022 W AP_VarT<signed char>::~AP_VarT()
 00000022 W AP_VarT<float>::~AP_VarT()
 00000022 W AP_VarT<int>::~AP_VarT()
-00000023 r setup_mode(unsigned char, Menu::arg const*)::__c
-00000023 r print_gyro_offsets()::__c
+00000022 r test_imu(unsigned char, Menu::arg const*)::__c
+00000023 r test_baro(unsigned char, Menu::arg const*)::__c
 00000024 r init_ardupilot()::__c
-00000024 r print_accel_offsets()::__c
 00000026 t print_done()
+00000026 t Log_Write_Data(signed char, float)
 00000026 t print_hit_enter()
 00000026 t Log_Read_Startup()
-00000026 r Log_Read_Control_Tuning()::__c
 00000028 t test_battery(unsigned char, Menu::arg const*)
 00000028 t main_menu_help(unsigned char, Menu::arg const*)
-00000028 t help_log(unsigned char, Menu::arg const*)
 00000028 W AP_VarT<float>::unserialize(void*, unsigned int)
 00000028 W AP_VarT<float>::serialize(void*, unsigned int) const
 00000028 r Log_Read_Cmd()::__c
-00000029 r setup_mode(unsigned char, Menu::arg const*)::__c
 00000029 r test_gps(unsigned char, Menu::arg const*)::__c
 0000002a t setup_declination(unsigned char, Menu::arg const*)
+0000002a t _mav_put_int8_t_array
 0000002b r planner_mode(unsigned char, Menu::arg const*)::__c
 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)
 0000002e W AP_Var_group::AP_Var_group(unsigned int, prog_char_t const*, unsigned char)
+0000002e r init_ardupilot()::__c
 0000002f r test_radio(unsigned char, Menu::arg const*)::__c
-0000002f r init_ardupilot()::__c
+0000002f r setup_factory(unsigned char, Menu::arg const*)::__c
 00000030 t planner_mode(unsigned char, Menu::arg const*)
 00000030 t send_heartbeat(mavlink_channel_t)
+00000030 r setup_radio(unsigned char, Menu::arg const*)::__c
 00000032 T GCS_MAVLINK::init(FastSerial*)
 00000032 W APM_PI::~APM_PI()
 00000032 r Log_Read_GPS()::__c
 00000033 b pending_status
 00000034 W AP_Float16::serialize(void*, unsigned int) const
-00000034 t _mav_put_int8_t_array
 00000034 t mavlink_msg_statustext_send
-00000035 r test_radio_pwm(unsigned char, Menu::arg const*)::__c
 00000036 t report_radio()
-00000037 r print_wp(Location*, unsigned char)::__c
-00000038 t init_throttle_cruise()
+00000036 r print_wp(Location*, unsigned char)::__c
 00000038 t send_current_waypoint(mavlink_channel_t)
-00000038 r setup_radio(unsigned char, Menu::arg const*)::__c
-00000038 r setup_factory(unsigned char, Menu::arg const*)::__c
 0000003a t report_gps()
 0000003a t report_imu()
-0000003c W RC_Channel::~RC_Channel()
+0000003c B barometer
 0000003d B g_gps_driver
 0000003e T GCS_MAVLINK::send_text(gcs_severity, prog_char_t const*)
 0000003e W AP_VarT<signed char>::AP_VarT(signed char, unsigned int, prog_char_t const*, unsigned char)
+00000040 r log_menu_commands
+00000040 t init_throttle_cruise()
 00000040 t read_AHRS()
 00000040 W AP_Float16::unserialize(void*, unsigned int)
 00000040 B adc
@@ -472,15 +477,14 @@
 00000042 t report_sonar()
 00000044 t setup_show(unsigned char, Menu::arg const*)
 00000044 W AP_VarT<int>::AP_VarT(int, unsigned int, prog_char_t const*, unsigned char)
-00000048 t change_command(unsigned char)
+00000044 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
+00000046 W RC_Channel::~RC_Channel()
 00000048 t update_motor_leds()
-00000049 r setup_flightmodes(unsigned char, Menu::arg const*)::__c
 0000004a t send_meminfo(mavlink_channel_t)
 0000004a W AP_VarT<int>::AP_VarT(AP_Var_group*, unsigned int, int, prog_char_t const*, unsigned char)
 0000004c t update_auto_yaw()
 00000050 b mavlink_queue
 00000050 t report_version()
-00000050 r log_menu_commands
 00000050 r main_menu_commands
 00000050 T GCS_MAVLINK::_find_parameter(unsigned int)
 00000050 B imu
@@ -489,13 +493,13 @@
 00000056 t readSwitch()
 00000056 t dancing_light()
 00000056 T GCS_MAVLINK::queued_waypoint_send()
-00000057 r help_log(unsigned char, Menu::arg const*)::__c
+0000005a t Log_Write_Data(signed char, long)
 0000005a W AP_VarT<float>::AP_VarT(float, unsigned int, prog_char_t const*, unsigned char)
 0000005c t get_num_logs()
 0000005c t setup_esc(unsigned char, Menu::arg const*)
+0000005e t change_command(unsigned char)
 0000005e t update_GPS_light()
 0000005e T GCS_MAVLINK::_count_parameters()
-00000064 B barometer
 00000064 t mavlink_msg_param_value_send
 00000068 t zero_eeprom()
 00000068 t find_last_log_page(int)
@@ -504,13 +508,12 @@
 0000006c t setup_sonar(unsigned char, Menu::arg const*)
 0000006e T output_min()
 00000078 t setup_batt_monitor(unsigned char, Menu::arg const*)
-00000078 t do_RTL()
 0000007a t setup_factory(unsigned char, Menu::arg const*)
 0000007a t read_control_switch()
 0000007a t report_flight_modes()
-0000007a t test_baro(unsigned char, Menu::arg const*)
+0000007a t do_RTL()
+0000007c t Log_Read_Data()
 0000007c t send_gps_status(mavlink_channel_t)
-0000007e t test_rawgps(unsigned char, Menu::arg const*)
 00000080 T __vector_25
 00000080 T __vector_36
 00000080 T __vector_54
@@ -523,22 +526,18 @@
 00000090 t report_tuning()
 00000092 t test_tuning(unsigned char, Menu::arg const*)
 00000092 T GCS_MAVLINK::queued_param_send()
-00000096 t print_wp(Location*, unsigned char)
 0000009a t planner_gcs(unsigned char, Menu::arg const*)
 0000009a t init_compass()
-0000009a t Log_Read_Motors()
-0000009b B gcs0
-0000009b B gcs3
+0000009c B gcs0
+0000009c B gcs3
 0000009e t setup_mode(unsigned char, Menu::arg const*)
 0000009e t Log_Read_Mode()
-0000009e t Log_Write_Cmd(unsigned char, Location*)
+000000a0 r test_menu_commands
 000000a4 T __vector_26
 000000a4 T __vector_37
 000000a4 T __vector_55
 000000a8 t test_sonar(unsigned char, Menu::arg const*)
-000000aa t Log_Read_Nav_Tuning()
 000000ae t report_frame()
-000000b0 t test_relay(unsigned char, Menu::arg const*)
 000000b2 t erase_logs(unsigned char, Menu::arg const*)
 000000b6 t get_log_boundaries(unsigned char, int&, int&)
 000000b7 B compass
@@ -546,84 +545,82 @@
 000000c2 t test_eedump(unsigned char, Menu::arg const*)
 000000c2 t setup_compass(unsigned char, Menu::arg const*)
 000000c2 t send_radio_out(mavlink_channel_t)
+000000c2 t Log_Read_Motors()
 000000c2 t Log_Read_Attitude()
 000000c4 t get_distance(Location*, Location*)
-000000c4 r setup_esc(unsigned char, Menu::arg const*)::__c
 000000c6 t send_radio_in(mavlink_channel_t)
-000000c6 t Log_Read_Performance()
-000000c6 t test_tri(unsigned char, Menu::arg const*)
 000000c7 B dcm
 000000ca t init_barometer()
+000000cc t Log_Read_Nav_Tuning()
+000000cc t Log_Read_Control_Tuning()
 000000d0 t read_radio()
 000000d0 t get_bearing(Location*, Location*)
 000000d0 t print_switch(unsigned char, unsigned char, bool)
-000000d4 t Log_Read(int, int)
+000000d4 t print_wp(Location*, unsigned char)
 000000d8 t test_radio(unsigned char, Menu::arg const*)
 000000d8 t read_barometer()
-000000dc t test_adc(unsigned char, Menu::arg const*)
+000000dc t Log_Read(int, int)
 000000e0 r setup_menu_commands
-000000e4 t test_radio_pwm(unsigned char, Menu::arg const*)
 000000e4 t Log_Read_Optflow()
 000000e4 W APM_PI::APM_PI(unsigned int, prog_char_t const*, float const&, float const&, int const&)
 000000e8 t Log_Read_Current()
-000000ea t Log_Read_Control_Tuning()
 000000ee t report_batt_monitor()
+000000ee t Log_Read_Performance()
 000000f6 t Log_Read_Cmd()
-00000100 r test_menu_commands
+000000fa t calc_loiter_pitch_roll()
 0000010a t mavlink_delay(unsigned long)
 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 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)
-0000011c t get_command_with_index(int)
-0000012c t calc_loiter_pitch_roll()
+00000118 t arm_motors()
+0000011c t get_cmd_with_index(int)
 00000130 t report_compass()
-00000138 t get_stabilize_roll(long)
-00000138 t get_stabilize_pitch(long)
-00000148 t Log_Read_GPS()
+00000136 W RC_Channel::RC_Channel(unsigned int, prog_char_t const*)
+0000013a t test_baro(unsigned char, Menu::arg const*)
+0000014c t get_stabilize_roll(long)
+0000014c t get_stabilize_pitch(long)
 0000014e t send_servo_out(mavlink_channel_t)
-00000156 t update_commands()
+00000152 t init_home()
+00000156 t Log_Read_GPS()
 0000015c t update_trig()
 0000015c t setup_motors(unsigned char, Menu::arg const*)
+0000015c t test_wp(unsigned char, Menu::arg const*)
 00000160 t send_location(mavlink_channel_t)
-00000160 t send_nav_controller_output(mavlink_channel_t)
-00000162 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
+00000164 t set_cmd_with_index(Location, int)
 00000166 t select_logs(unsigned char, Menu::arg const*)
 00000166 t send_vfr_hud(mavlink_channel_t)
-00000168 T GCS_MAVLINK::update()
-0000016c t test_imu(unsigned char, Menu::arg const*)
+00000166 T GCS_MAVLINK::data_stream_send(unsigned int, unsigned int)
 0000016e t send_attitude(mavlink_channel_t)
 00000174 t mavlink_send_message(mavlink_channel_t, ap_message, unsigned int)
+00000178 t verify_nav_wp()
 000001a2 t mavlink_try_send_message(mavlink_channel_t, ap_message, unsigned int)
 000001a8 t print_radio_values()
-000001be t arm_motors()
-000001cc t start_new_log()
-000001e4 t verify_nav_wp()
+000001b2 t start_new_log()
+000001b8 t send_nav_controller_output(mavlink_channel_t)
+000001ce T GCS_MAVLINK::update()
 000001e4 t setup_flightmodes(unsigned char, Menu::arg const*)
-000001ea t init_home()
-00000216 t set_mode(unsigned char)
+00000210 t test_imu(unsigned char, Menu::arg const*)
 0000021a t send_raw_imu1(mavlink_channel_t)
-0000021c t test_wp(unsigned char, Menu::arg const*)
 00000228 t setup_radio(unsigned char, Menu::arg const*)
-0000022a t send_gps_raw(mavlink_channel_t)
-00000242 t calc_loiter(int, int)
+00000246 t calc_loiter(int, int)
 00000268 t send_raw_imu3(mavlink_channel_t)
-00000330 t tuning()
+0000026a t send_gps_raw(mavlink_channel_t)
+000002cc t set_mode(unsigned char)
+00000362 t tuning()
 00000382 t print_log_menu()
-0000039a T update_throttle_mode()
+00000396 T update_roll_pitch_mode()
 000003a0 t read_battery()
-00000410 T update_yaw_mode()
-0000046e T update_roll_pitch_mode()
-00000636 t init_ardupilot()
-0000071a t update_nav_wp()
-000007c8 t __static_initialization_and_destruction_0(int, int)
-00000842 b g
-00000870 t process_next_command()
-00000884 W Parameters::Parameters()
-000012ec T GCS_MAVLINK::handleMessage(__mavlink_message*)
-0000190e T loop
+0000041c T update_yaw_mode()
+00000444 T update_throttle_mode()
+00000702 t init_ardupilot()
+000007d8 t __static_initialization_and_destruction_0(int, int)
+00000878 t update_nav_wp()
+000008f6 W Parameters::Parameters()
+000008fa b g
+000009be t update_commands(bool)
+000014f0 T GCS_MAVLINK::handleMessage(__mavlink_message*)
+00001ce4 T loop
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml
index f15428b643..3f4ea6a658 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml
+++ b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml
@@ -57,7 +57,7 @@
 #define FRAME_ORIENTATION X_FRAME
 
 </desc>
-    <format_version>112</format_version>
+    <format_version>113</format_version>
   </Firmware>
   <Firmware>
     <url>http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.hex</url>
@@ -68,7 +68,7 @@
 #define FRAME_ORIENTATION X_FRAME
 
 </desc>
-    <format_version>112</format_version>
+    <format_version>113</format_version>
   </Firmware>
   <Firmware>
     <url>http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.hex</url>
@@ -79,7 +79,7 @@
 #define FRAME_ORIENTATION X_FRAME
 
 </desc>
-    <format_version>112</format_version>
+    <format_version>113</format_version>
   </Firmware>
   <Firmware>
     <url>http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.hex</url>
@@ -90,7 +90,7 @@
 #define FRAME_ORIENTATION X_FRAME
 
 </desc>
-    <format_version>112</format_version>
+    <format_version>113</format_version>
   </Firmware>
   <Firmware>
     <url>http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.hex</url>
@@ -152,7 +152,7 @@
 
 
 </desc>
-    <format_version>112</format_version>
+    <format_version>113</format_version>
   </Firmware>
   <Firmware>
     <url>http://ardupilot-mega.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.hex</url>
diff --git a/Tools/ArdupilotMegaPlanner/Firmware/git.log b/Tools/ArdupilotMegaPlanner/Firmware/git.log
index 1615f378de..bba134ae9c 100644
--- a/Tools/ArdupilotMegaPlanner/Firmware/git.log
+++ b/Tools/ArdupilotMegaPlanner/Firmware/git.log
@@ -1,20 +1,49 @@
 From https://code.google.com/p/ardupilot-mega
-   6aeabd0..833acbd  master     -> origin/master
-Updating 6aeabd0..833acbd
+   6d78bad..6d8e6a8  master     -> origin/master
+Updating 6d78bad..6d8e6a8
 Fast-forward
- ArduCopter/ArduCopter.pde    |   11 ++++-
- ArduCopter/Attitude.pde      |    1 -
- ArduCopter/Log.pde           |   28 ++++++------
- ArduCopter/Parameters.h      |   50 +++++++++--------------
- ArduCopter/config.h          |   20 +--------
- ArduCopter/defines.h         |    2 +
- ArduCopter/navigation.pde    |   17 ++++++--
- ArduCopter/radio.pde         |    9 +----
- ArduCopter/setup.pde         |   52 ++++++++++++------------
- ArduCopter/system.pde        |   22 ++++++----
- ArduCopter/test.pde          |   63 +++++++++++++++--------------
- ArduPlane/ArduPlane.pde      |    2 +-
- ArduPlane/Log.pde            |   91 +++++++++++++++++++++---------------------
- Tools/autotest/arducopter.py |   10 ++--
- Tools/autotest/common.py     |    8 +++-
- 15 files changed, 193 insertions(+), 193 deletions(-)
+ ArduCopter/ArduCopter.pde                          |   59 +-
+ ArduCopter/GCS_Mavlink copy.txt                    | 2091 ++++++++++++++++++++
+ ArduCopter/GCS_Mavlink.pde                         |   16 +-
+ ArduCopter/Log.pde                                 |   27 +
+ ArduCopter/Parameters.h                            |    2 +-
+ ArduCopter/commands.pde                            |    7 +-
+ ArduCopter/commands_logic.pde                      |    8 +-
+ ArduCopter/control_modes.pde                       |    2 +-
+ ArduCopter/defines.h                               |   20 +-
+ ArduCopter/heli.pde                                |  115 +-
+ ArduCopter/system.pde                              |    4 +
+ ArduCopter/test.pde                                |   16 +-
+ ArduPlane/Log.pde                                  |    7 +
+ .../Projects/arduino-usbserial/Arduino-usbserial.c |    2 +-
+ Tools/ArduPPM/Libraries/PPM_Encoder.h              |   28 +-
+ Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj    |    2 +-
+ Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs       |   21 +-
+ Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs    |    7 +-
+ Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs  |    9 +-
+ .../GCSViews/FlightPlanner.Designer.cs             |    1 +
+ .../ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs |   64 +-
+ .../GCSViews/FlightPlanner.resx                    |    4 +-
+ Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs  |    2 +-
+ Tools/ArdupilotMegaPlanner/MAVLink.cs              |   15 +-
+ Tools/ArdupilotMegaPlanner/MainV2.Designer.cs      |   10 +-
+ Tools/ArdupilotMegaPlanner/MainV2.cs               |    8 +
+ Tools/ArdupilotMegaPlanner/Program.cs              |    2 +-
+ .../Properties/AssemblyInfo.cs                     |    2 +-
+ Tools/ArdupilotMegaPlanner/Setup/Setup.Designer.cs |    1 +
+ Tools/ArdupilotMegaPlanner/Setup/Setup.cs          |    8 +-
+ .../bin/Release/ArdupilotMegaPlanner.application   |    2 +-
+ .../bin/Release/ArdupilotMegaPlanner.exe           |  Bin 2212864 -> 2214400 bytes
+ .../bin/Release/BSE.Windows.Forms.dll              |  Bin 141824 -> 141824 bytes
+ .../bin/Release/GCSViews/FlightPlanner.resx        |    4 +-
+ .../bin/Release/GMap.NET.Core.dll                  |  Bin 189952 -> 183808 bytes
+ .../bin/Release/GMap.NET.WindowsForms.dll          |  Bin 77824 -> 77824 bytes
+ Tools/ArdupilotMegaPlanner/bin/Release/Updater.exe |  Bin 8192 -> 8192 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
+ Tools/autotest/autotest.py                         |    3 +-
+ Tools/autotest/mission2.txt                        |    2 +-
+ libraries/Desktop/support/DataFlash.cpp            |   20 +-
+ 43 files changed, 2445 insertions(+), 146 deletions(-)
+ create mode 100644 ArduCopter/GCS_Mavlink copy.txt