diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 6831631f4a..526201a55e 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -953,36 +953,36 @@ private:
 
     Mode *flightmode;
 #if FRAME_CONFIG == HELI_FRAME
-    ModeAcro_Heli mode_acro{*this};
+    ModeAcro_Heli mode_acro;
 #else
-    ModeAcro mode_acro{*this};
+    ModeAcro mode_acro;
 #endif
-    ModeAltHold mode_althold{*this};
-    ModeAuto mode_auto{*this, mission, circle_nav};
+    ModeAltHold mode_althold;
+    ModeAuto mode_auto;
 #if AUTOTUNE_ENABLED == ENABLED
-    ModeAutoTune mode_autotune{*this};
+    ModeAutoTune mode_autotune;
 #endif
-    ModeBrake mode_brake{*this};
-    ModeCircle mode_circle{*this, circle_nav};
-    ModeDrift mode_drift{*this};
-    ModeFlip mode_flip{*this};
-    ModeGuided mode_guided{*this};
-    ModeLand mode_land{*this};
-    ModeLoiter mode_loiter{*this};
-    ModePosHold mode_poshold{*this};
-    ModeRTL mode_rtl{*this};
+    ModeBrake mode_brake;
+    ModeCircle mode_circle;
+    ModeDrift mode_drift;
+    ModeFlip mode_flip;
+    ModeGuided mode_guided;
+    ModeLand mode_land;
+    ModeLoiter mode_loiter;
+    ModePosHold mode_poshold;
+    ModeRTL mode_rtl;
 #if FRAME_CONFIG == HELI_FRAME
-    ModeStabilize_Heli mode_stabilize{*this};
+    ModeStabilize_Heli mode_stabilize;
 #else
-    ModeStabilize mode_stabilize{*this};
+    ModeStabilize mode_stabilize;
 #endif
-    ModeSport mode_sport{*this};
-    ModeAvoidADSB mode_avoid_adsb{*this};
-    ModeThrow mode_throw{*this};
-    ModeGuidedNoGPS mode_guided_nogps{*this};
-    ModeSmartRTL mode_smartrtl{*this};
+    ModeSport mode_sport;
+    ModeAvoidADSB mode_avoid_adsb;
+    ModeThrow mode_throw;
+    ModeGuidedNoGPS mode_guided_nogps;
+    ModeSmartRTL mode_smartrtl;
 #if OPTFLOW == ENABLED
-    ModeFlowHold mode_flowhold{*this};
+    ModeFlowHold mode_flowhold;
 #endif
 
     // mode.cpp
diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp
index 507471ed83..37a48dfda6 100644
--- a/ArduCopter/Log.cpp
+++ b/ArduCopter/Log.cpp
@@ -36,7 +36,7 @@ void Copter::ModeAutoTune::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step,
         new_gain_sp : new_gain_sp,
         new_ddt     : new_ddt
     };
-    _copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
+    copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
 struct PACKED log_AutoTuneDetails {
@@ -55,7 +55,7 @@ void Copter::ModeAutoTune::Log_Write_AutoTuneDetails(float angle_cd, float rate_
         angle_cd    : angle_cd,
         rate_cds    : rate_cds
     };
-    _copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
+    copter.DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 #endif
 
diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
index a75aaaed16..cd1cf2597b 100644
--- a/ArduCopter/Parameters.cpp
+++ b/ArduCopter/Parameters.cpp
@@ -970,6 +970,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 1000),
 
+#if OPTFLOW == ENABLED
+    // @Group: FHLD
+    // @Path: mode_flowhold.cpp
+    AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, Copter::ModeFlowHold),
+#endif
+    
     AP_GROUPEND
 };
 
@@ -987,6 +993,9 @@ ParametersG2::ParametersG2(void)
 #endif
     ,smart_rtl(copter.ahrs)
     ,temp_calibration(copter.barometer, copter.ins)
+#if OPTFLOW == ENABLED
+    ,mode_flowhold_ptr(&copter.mode_flowhold)
+#endif
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index 18f974e419..0f2bcab87a 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -568,6 +568,11 @@ public:
 #if TOY_MODE_ENABLED == ENABLED
     ToyMode toy_mode;
 #endif
+
+#if OPTFLOW == ENABLED
+    // we need a pointer to the mode for the G2 table
+    void *mode_flowhold_ptr;
+#endif
 };
 
 extern const AP_Param::Info        var_info[];
diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp
index efe0e94a32..56d0a824f7 100644
--- a/ArduCopter/commands_logic.cpp
+++ b/ArduCopter/commands_logic.cpp
@@ -4,8 +4,8 @@
 bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
 {
     // To-Do: logging when new commands start/end
-    if (_copter.should_log(MASK_LOG_CMD)) {
-        _copter.DataFlash.Log_Write_Mission_Cmd(mission, cmd);
+    if (copter.should_log(MASK_LOG_CMD)) {
+        copter.DataFlash.Log_Write_Mission_Cmd(copter.mission, cmd);
     }
 
     switch(cmd.id) {
@@ -86,20 +86,20 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_DO_SET_SERVO:
-        _copter.ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
+        copter.ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
         break;
         
     case MAV_CMD_DO_SET_RELAY:
-        _copter.ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
+        copter.ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
         break;
         
     case MAV_CMD_DO_REPEAT_SERVO:
-        _copter.ServoRelayEvents.do_repeat_servo(cmd.content.repeat_servo.channel, cmd.content.repeat_servo.pwm,
+        copter.ServoRelayEvents.do_repeat_servo(cmd.content.repeat_servo.channel, cmd.content.repeat_servo.pwm,
                                          cmd.content.repeat_servo.repeat_count, cmd.content.repeat_servo.cycle_time * 1000.0f);
         break;
         
     case MAV_CMD_DO_REPEAT_RELAY:
-        _copter.ServoRelayEvents.do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count,
+        copter.ServoRelayEvents.do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count,
                                          cmd.content.repeat_relay.cycle_time * 1000.0f);
         break;
 
@@ -138,7 +138,7 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
-        _copter.camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
+        copter.camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
         break;
 #endif
 
@@ -181,7 +181,7 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
 //      we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
 bool Copter::ModeAuto::verify_command_callback(const AP_Mission::Mission_Command& cmd)
 {
-    if (_copter.flightmode == &_copter.mode_auto) {
+    if (copter.flightmode == &copter.mode_auto) {
         bool cmd_complete = verify_command(cmd);
 
         // send message to GCS
@@ -297,7 +297,7 @@ void Copter::ModeAuto::exit_mission()
         }
     }else{
         // if we've landed it's safe to disarm
-        _copter.init_disarm_motors();
+        copter.init_disarm_motors();
     }
 }
 
@@ -327,7 +327,7 @@ void Copter::ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd)
 void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
     Location_Class target_loc(cmd.content.location);
-    const Location_Class &current_loc = _copter.current_loc;
+    const Location_Class &current_loc = copter.current_loc;
 
     // use current lat, lon if zero
     if (target_loc.lat == 0 && target_loc.lng == 0) {
@@ -356,7 +356,7 @@ void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 
     // if no delay set the waypoint as "fast"
     if (loiter_time_max == 0 ) {
-        wp_nav->set_fast_waypoint(true);
+        copter.wp_nav->set_fast_waypoint(true);
     }
 }
 
@@ -366,7 +366,7 @@ Location_Class Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mis
 {
     // convert to location class
     Location_Class target_loc(cmd.content.location);
-    const Location_Class &current_loc = _copter.current_loc;
+    const Location_Class &current_loc = copter.current_loc;
 
     // decide if we will use terrain following
     int32_t curr_terr_alt_cm, target_terr_alt_cm;
@@ -430,13 +430,13 @@ void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cm
 {
     // convert back to location
     Location_Class target_loc(cmd.content.location);
-    const Location_Class &current_loc = _copter.current_loc;
+    const Location_Class &current_loc = copter.current_loc;
 
     // use current location if not provided
     if (target_loc.lat == 0 && target_loc.lng == 0) {
         // To-Do: make this simpler
         Vector3f temp_pos;
-        wp_nav->get_wp_stopping_point_xy(temp_pos);
+        copter.wp_nav->get_wp_stopping_point_xy(temp_pos);
         Location_Class temp_loc(temp_pos);
         target_loc.lat = temp_loc.lat;
         target_loc.lng = temp_loc.lng;
@@ -463,7 +463,7 @@ void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cm
 void Copter::ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
 {
     Location_Class circle_center(cmd.content.location);
-    const Location_Class &current_loc = _copter.current_loc;
+    const Location_Class &current_loc = copter.current_loc;
 
     // default lat/lon to current position if not provided
     // To-Do: use stopping point or position_controller's target instead of current location to avoid jerk?
@@ -481,7 +481,7 @@ void Copter::ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
         } else {
             // default to current altitude above origin
             circle_center.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
-            _copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
+            copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
         }
     }
 
@@ -508,7 +508,7 @@ void Copter::ModeAuto::do_loiter_time(const AP_Mission::Mission_Command& cmd)
 void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
 {
     Location_Class target_loc(cmd.content.location);
-    const Location_Class &current_loc = _copter.current_loc;
+    const Location_Class &current_loc = copter.current_loc;
 
     // use current lat, lon if zero
     if (target_loc.lat == 0 && target_loc.lng == 0) {
@@ -539,9 +539,9 @@ void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
 
     // if previous command was a wp_nav command with no delay set stopped_at_start to false
     // To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself?
-    uint16_t prev_cmd_idx = mission.get_prev_nav_cmd_index();
+    uint16_t prev_cmd_idx = copter.mission.get_prev_nav_cmd_index();
     if (prev_cmd_idx != AP_MISSION_CMD_INDEX_NONE) {
-        if (mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
+        if (copter.mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
             if ((temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) && temp_cmd.p1 == 0) {
                 stopped_at_start = false;
             }
@@ -550,7 +550,7 @@ void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
 
     // if there is no delay at the end of this segment get next nav command
     Location_Class next_loc;
-    if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
+    if (cmd.p1 == 0 && copter.mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
         next_loc = temp_cmd.content.location;
         // default lat, lon to first waypoint's lat, lon
         if (next_loc.lat == 0 && next_loc.lng == 0) {
@@ -585,7 +585,7 @@ void Copter::ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& c
 {
     if (cmd.p1 > 0) {
         // initialise guided limits
-        _copter.mode_guided.limit_init_time_and_pos();
+        copter.mode_guided.limit_init_time_and_pos();
 
         // set spline navigation target
         nav_guided_start();
@@ -614,15 +614,15 @@ void Copter::ModeAuto::do_parachute(const AP_Mission::Mission_Command& cmd)
 {
     switch (cmd.p1) {
         case PARACHUTE_DISABLE:
-            _copter.parachute.enabled(false);
+            copter.parachute.enabled(false);
             Log_Write_Event(DATA_PARACHUTE_DISABLED);
             break;
         case PARACHUTE_ENABLE:
-            _copter.parachute.enabled(true);
+            copter.parachute.enabled(true);
             Log_Write_Event(DATA_PARACHUTE_ENABLED);
             break;
         case PARACHUTE_RELEASE:
-            _copter.parachute_release();
+            copter.parachute_release();
             break;
         default:
             // do nothing
@@ -656,7 +656,7 @@ void Copter::ModeAuto::do_gripper(const AP_Mission::Mission_Command& cmd)
 // do_guided_limits - pass guided limits to guided controller
 void Copter::ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd)
 {
-    _copter.mode_guided.limit_set(
+    copter.mode_guided.limit_set(
         cmd.p1 * 1000, // convert seconds to ms
         cmd.content.guided_limits.alt_min * 100.0f,    // convert meters to cm
         cmd.content.guided_limits.alt_max * 100.0f,    // convert meters to cm
@@ -695,7 +695,7 @@ void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
 bool Copter::ModeAuto::verify_takeoff()
 {
     // have we reached our target altitude?
-    return wp_nav->reached_wp_destination();
+    return copter.wp_nav->reached_wp_destination();
 }
 
 // verify_land - returns true if landing has been completed
@@ -706,9 +706,9 @@ bool Copter::ModeAuto::verify_land()
     switch (land_state) {
         case LandStateType_FlyToLocation:
             // check if we've reached the location
-            if (wp_nav->reached_wp_destination()) {
+            if (copter.wp_nav->reached_wp_destination()) {
                 // get destination so we can use it for loiter target
-                Vector3f dest = wp_nav->get_wp_destination();
+                Vector3f dest = copter.wp_nav->get_wp_destination();
 
                 // initialise landing controller
                 land_start(dest);
@@ -778,7 +778,7 @@ bool Copter::ModeAuto::verify_payload_place()
 
     switch (nav_payload_place.state) {
     case PayloadPlaceStateType_FlyToLocation:
-        if (!wp_nav->reached_wp_destination()) {
+        if (!copter.wp_nav->reached_wp_destination()) {
             return false;
         }
         // we're there; set loiter target
@@ -879,7 +879,7 @@ bool Copter::ModeAuto::verify_payload_place()
         }
         FALLTHROUGH;
     case PayloadPlaceStateType_Ascending:
-        if (!wp_nav->reached_wp_destination()) {
+        if (!copter.wp_nav->reached_wp_destination()) {
             return false;
         }
         nav_payload_place.state = PayloadPlaceStateType_Done;
@@ -900,7 +900,7 @@ bool Copter::ModeAuto::verify_payload_place()
 bool Copter::ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
     // check if we have reached the waypoint
-    if( !wp_nav->reached_wp_destination() ) {
+    if( !copter.wp_nav->reached_wp_destination() ) {
         return false;
     }
 
@@ -930,7 +930,7 @@ bool Copter::ModeAuto::verify_loiter_unlimited()
 bool Copter::ModeAuto::verify_loiter_time()
 {
     // return immediately if we haven't reached our destination
-    if (!wp_nav->reached_wp_destination()) {
+    if (!copter.wp_nav->reached_wp_destination()) {
         return false;
     }
 
@@ -948,9 +948,9 @@ bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
 {
     // check if we've reached the edge
     if (mode() == Auto_CircleMoveToEdge) {
-        if (wp_nav->reached_wp_destination()) {
-            const Vector3f curr_pos = _copter.inertial_nav.get_position();
-            Vector3f circle_center = _copter.pv_location_to_vector(cmd.content.location);
+        if (copter.wp_nav->reached_wp_destination()) {
+            const Vector3f curr_pos = copter.inertial_nav.get_position();
+            Vector3f circle_center = copter.pv_location_to_vector(cmd.content.location);
 
             // set target altitude if not provided
             if (is_zero(circle_center.z)) {
@@ -970,7 +970,7 @@ bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
     }
 
     // check if we have completed circling
-    return fabsf(circle_nav->get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
+    return fabsf(copter.circle_nav->get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
 }
 
 // verify_RTL - handles any state changes required to implement RTL
@@ -978,14 +978,14 @@ bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
 // returns true with RTL has completed successfully
 bool Copter::ModeAuto::verify_RTL()
 {
-    return (_copter.mode_rtl.state_complete() && (_copter.mode_rtl.state() == RTL_FinalDescent || _copter.mode_rtl.state() == RTL_Land));
+    return (copter.mode_rtl.state_complete() && (copter.mode_rtl.state() == RTL_FinalDescent || copter.mode_rtl.state() == RTL_Land));
 }
 
 // verify_spline_wp - check if we have reached the next way point using spline
 bool Copter::ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
 {
     // check if we have reached the waypoint
-    if( !wp_nav->reached_wp_destination() ) {
+    if( !copter.wp_nav->reached_wp_destination() ) {
         return false;
     }
 
@@ -1013,7 +1013,7 @@ bool Copter::ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Comman
     }
 
     // check time and position limits
-    return _copter.mode_guided.limit_check();
+    return copter.mode_guided.limit_check();
 }
 #endif  // NAV_GUIDED
 
@@ -1084,7 +1084,7 @@ bool Copter::ModeAuto::verify_yaw()
     }
 
     // check if we are within 2 degrees of the target heading
-    if (labs(wrap_180_cd(ahrs.yaw_sensor-_copter.yaw_look_at_heading)) <= 200) {
+    if (labs(wrap_180_cd(ahrs.yaw_sensor-copter.yaw_look_at_heading)) <= 200) {
         return true;
     }else{
         return false;
@@ -1099,7 +1099,7 @@ bool Copter::ModeAuto::verify_yaw()
 bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
 {
     // only process guided waypoint if we are in guided mode
-    if (_copter.control_mode != GUIDED && !(_copter.control_mode == AUTO && mode() == Auto_NavGuided)) {
+    if (copter.control_mode != GUIDED && !(copter.control_mode == AUTO && mode() == Auto_NavGuided)) {
         return false;
     }
 
@@ -1110,7 +1110,7 @@ bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
         {
             // set wp_nav's destination
             Location_Class dest(cmd.content.location);
-            return _copter.mode_guided.set_destination(dest);
+            return copter.mode_guided.set_destination(dest);
         }
 
         case MAV_CMD_CONDITION_YAW:
@@ -1128,16 +1128,16 @@ bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
 void Copter::ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.content.speed.target_ms > 0) {
-        wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f);
+        copter.wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f);
     }
 }
 
 void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
 {
     if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
-        _copter.set_home_to_current_location(false);
+        copter.set_home_to_current_location(false);
     } else {
-        _copter.set_home(cmd.content.location, false);
+        copter.set_home(cmd.content.location, false);
     }
 }
 
@@ -1147,7 +1147,7 @@ void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
 //	TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
 void Copter::ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
 {
-    _copter.set_auto_yaw_roi(cmd.content.location);
+    copter.set_auto_yaw_roi(cmd.content.location);
 }
 
 #if CAMERA == ENABLED
@@ -1155,7 +1155,7 @@ void Copter::ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
 // do_digicam_configure Send Digicam Configure message with the camera library
 void Copter::ModeAuto::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
 {
-    _copter.camera.configure(
+    copter.camera.configure(
         cmd.content.digicam_configure.shooting_mode,
         cmd.content.digicam_configure.shutter_speed,
         cmd.content.digicam_configure.aperture,
@@ -1168,7 +1168,7 @@ void Copter::ModeAuto::do_digicam_configure(const AP_Mission::Mission_Command& c
 // do_digicam_control Send Digicam Control message with the camera library
 void Copter::ModeAuto::do_digicam_control(const AP_Mission::Mission_Command& cmd)
 {
-    _copter.camera.control(cmd.content.digicam_control.session,
+    copter.camera.control(cmd.content.digicam_control.session,
                            cmd.content.digicam_control.zoom_pos,
                            cmd.content.digicam_control.zoom_step,
                            cmd.content.digicam_control.focus_lock,
@@ -1182,9 +1182,9 @@ void Copter::ModeAuto::do_digicam_control(const AP_Mission::Mission_Command& cmd
 void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
 {
 #if MOUNT == ENABLED
-    if(!_copter.camera_mount.has_pan_control()) {
-        _copter.set_auto_yaw_look_at_heading(cmd.content.mount_control.yaw,0.0f,0,0);
+    if(!copter.camera_mount.has_pan_control()) {
+        copter.set_auto_yaw_look_at_heading(cmd.content.mount_control.yaw,0.0f,0,0);
     }
-    _copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
+    copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
 #endif
 }
diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
index fc46016a8f..80afd7c5d2 100644
--- a/ArduCopter/mode.cpp
+++ b/ArduCopter/mode.cpp
@@ -5,6 +5,33 @@
  * flight modes is in control_acro.cpp, control_stabilize.cpp, etc
  */
 
+/*
+  constructor for Mode object
+ */
+Copter::Mode::Mode(void) :
+    g(copter.g),
+    g2(copter.g2),
+    wp_nav(copter.wp_nav),
+    pos_control(copter.pos_control),
+    inertial_nav(copter.inertial_nav),
+    ahrs(copter.ahrs),
+    attitude_control(copter.attitude_control),
+    motors(copter.motors),
+    channel_roll(copter.channel_roll),
+    channel_pitch(copter.channel_pitch),
+    channel_throttle(copter.channel_throttle),
+    channel_yaw(copter.channel_yaw),
+    G_Dt(copter.G_Dt),
+    ap(copter.ap),
+    takeoff_state(copter.takeoff_state),
+    ekfGndSpdLimit(copter.ekfGndSpdLimit),
+    ekfNavVelGainScaler(copter.ekfNavVelGainScaler),
+#if FRAME_CONFIG == HELI_FRAME
+    heli_flags(copter.heli_flags),
+#endif
+    auto_yaw_mode(copter.auto_yaw_mode)
+{ };
+
 // return the static controller object corresponding to supplied mode
 Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode)
 {
@@ -91,7 +118,7 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode)
 
 #if OPTFLOW == ENABLED
         case FLOWHOLD:
-            ret = &mode_flowhold;
+            ret = (Copter::Mode *)g2.mode_flowhold_ptr;
             break;
 #endif
             
@@ -274,10 +301,127 @@ void Copter::Mode::zero_throttle_and_relax_ac()
 #if FRAME_CONFIG == HELI_FRAME
     // Helicopters always stabilize roll/pitch/yaw
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
-    attitude_control->set_throttle_out(0.0f, false, g.throttle_filt);
+    attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt);
 #else
     motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
     // multicopters do not stabilize roll/pitch/yaw when disarmed
-    attitude_control->set_throttle_out_unstabilized(0.0f, true, g.throttle_filt);
+    attitude_control->set_throttle_out_unstabilized(0.0f, true, copter.g.throttle_filt);
 #endif
 }
+
+
+// pass-through functions to reduce code churn on conversion;
+// these are candidates for moving into the Mode base
+// class.
+void Copter::Mode::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
+{
+    copter.get_pilot_desired_lean_angles(roll_in, pitch_in, roll_out, pitch_out, angle_max);
+}
+
+float Copter::Mode::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt)
+{
+    return copter.get_surface_tracking_climb_rate(target_rate, current_alt_target, dt);
+}
+
+float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
+{
+    return copter.get_pilot_desired_yaw_rate(stick_angle);
+}
+
+float Copter::Mode::get_pilot_desired_climb_rate(float throttle_control)
+{
+    return copter.get_pilot_desired_climb_rate(throttle_control);
+}
+
+float Copter::Mode::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
+{
+    return copter.get_pilot_desired_throttle(throttle_control, thr_mid);
+}
+
+float Copter::Mode::get_non_takeoff_throttle()
+{
+    return copter.get_non_takeoff_throttle();
+}
+
+void Copter::Mode::update_simple_mode(void) {
+    copter.update_simple_mode();
+}
+
+float Copter::Mode::get_smoothing_gain() {
+    return copter.get_smoothing_gain();
+}
+
+bool Copter::Mode::set_mode(control_mode_t mode, mode_reason_t reason)
+{
+    return copter.set_mode(mode, reason);
+}
+
+void Copter::Mode::set_land_complete(bool b)
+{
+    return copter.set_land_complete(b);
+}
+
+GCS_Copter &Copter::Mode::gcs()
+{
+    return copter.gcs();
+}
+
+void Copter::Mode::Log_Write_Event(uint8_t id)
+{
+    return copter.Log_Write_Event(id);
+}
+
+void Copter::Mode::set_throttle_takeoff()
+{
+    return copter.set_throttle_takeoff();
+}
+
+void Copter::Mode::set_auto_yaw_mode(uint8_t yaw_mode)
+{
+    return copter.set_auto_yaw_mode(yaw_mode);
+}
+
+void Copter::Mode::set_auto_yaw_rate(float turn_rate_cds)
+{
+    return copter.set_auto_yaw_rate(turn_rate_cds);
+}
+
+void Copter::Mode::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
+{
+    return copter.set_auto_yaw_look_at_heading(angle_deg, turn_rate_dps, direction, relative_angle);
+}
+
+void Copter::Mode::takeoff_timer_start(float alt_cm)
+{
+    return copter.takeoff_timer_start(alt_cm);
+}
+
+void Copter::Mode::takeoff_stop()
+{
+    return copter.takeoff_stop();
+}
+
+void Copter::Mode::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate)
+{
+    return copter.takeoff_get_climb_rates(pilot_climb_rate, takeoff_climb_rate);
+}
+
+float Copter::Mode::get_auto_heading()
+{
+    return copter.get_auto_heading();
+}
+
+float Copter::Mode::get_auto_yaw_rate_cds()
+{
+    return copter.get_auto_yaw_rate_cds();
+}
+
+float Copter::Mode::get_avoidance_adjusted_climbrate(float target_rate)
+{
+    return copter.get_avoidance_adjusted_climbrate(target_rate);
+}
+
+uint16_t Copter::Mode::get_pilot_speed_dn()
+{
+    return copter.get_pilot_speed_dn();
+}
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 6165be01c5..3e4ae835c8 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -7,33 +7,9 @@ class Mode {
     friend class AP_Arming_Copter;
     friend class ToyMode;
 
-public:
-
-    Mode(Copter &copter) :
-        _copter(copter),
-        g(copter.g),
-        g2(copter.g2),
-        wp_nav(_copter.wp_nav),
-        pos_control(_copter.pos_control),
-        inertial_nav(_copter.inertial_nav),
-        ahrs(_copter.ahrs),
-        attitude_control(_copter.attitude_control),
-        motors(_copter.motors),
-        channel_roll(_copter.channel_roll),
-        channel_pitch(_copter.channel_pitch),
-        channel_throttle(_copter.channel_throttle),
-        channel_yaw(_copter.channel_yaw),
-        G_Dt(_copter.G_Dt),
-        ap(_copter.ap),
-        takeoff_state(_copter.takeoff_state),
-        ekfGndSpdLimit(_copter.ekfGndSpdLimit),
-        ekfNavVelGainScaler(_copter.ekfNavVelGainScaler),
-#if FRAME_CONFIG == HELI_FRAME
-        heli_flags(_copter.heli_flags),
-#endif
-        auto_yaw_mode(_copter.auto_yaw_mode)
-        { };
-
+    // constructor
+    Mode(void);
+    
 protected:
 
     virtual bool init(bool ignore_checks) = 0;
@@ -57,8 +33,6 @@ protected:
     virtual uint32_t wp_distance() const { return 0; }
     virtual int32_t wp_bearing() const { return 0; }
 
-    Copter &_copter;
-
     // convenience references to avoid code churn in conversion:
     Parameters &g;
     ParametersG2 &g2;
@@ -96,75 +70,29 @@ protected:
     // pass-through functions to reduce code churn on conversion;
     // these are candidates for moving into the Mode base
     // class.
-    void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max) {
-        _copter.get_pilot_desired_lean_angles(roll_in, pitch_in, roll_out, pitch_out, angle_max);
-    }
-    float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) {
-        return _copter.get_surface_tracking_climb_rate(target_rate, current_alt_target, dt);
-    }
-    float get_pilot_desired_yaw_rate(int16_t stick_angle) {
-        return _copter.get_pilot_desired_yaw_rate(stick_angle);
-    }
-    float get_pilot_desired_climb_rate(float throttle_control) {
-        return _copter.get_pilot_desired_climb_rate(throttle_control);
-    }
-    float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f) {
-        return _copter.get_pilot_desired_throttle(throttle_control, thr_mid);
-    }
-    float get_non_takeoff_throttle() {
-        return _copter.get_non_takeoff_throttle();
-    }
-    void update_simple_mode(void) {
-        _copter.update_simple_mode();
-    }
-    float get_smoothing_gain() {
-        return _copter.get_smoothing_gain();
-    }
-    bool set_mode(control_mode_t mode, mode_reason_t reason) {
-        return _copter.set_mode(mode, reason);
-    }
-    void set_land_complete(bool b) {
-        return _copter.set_land_complete(b);
-    }
-    GCS_Copter &gcs() {
-        return _copter.gcs();
-    }
-    void Log_Write_Event(uint8_t id) {
-        return _copter.Log_Write_Event(id);
-    }
-    void set_throttle_takeoff() {
-        return _copter.set_throttle_takeoff();
-    }
-    void set_auto_yaw_mode(uint8_t yaw_mode) {
-        return _copter.set_auto_yaw_mode(yaw_mode);
-    }
-    void set_auto_yaw_rate(float turn_rate_cds) {
-        return _copter.set_auto_yaw_rate(turn_rate_cds);
-    }
-    void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle) {
-        return _copter.set_auto_yaw_look_at_heading(angle_deg, turn_rate_dps, direction, relative_angle);
-    }
-    void takeoff_timer_start(float alt_cm) {
-        return _copter.takeoff_timer_start(alt_cm);
-    }
-    void takeoff_stop() {
-        return _copter.takeoff_stop();
-    }
-    void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate) {
-        return _copter.takeoff_get_climb_rates(pilot_climb_rate, takeoff_climb_rate);
-    }
-    float get_auto_heading() {
-        return _copter.get_auto_heading();
-    }
-    float get_auto_yaw_rate_cds() {
-        return _copter.get_auto_yaw_rate_cds();
-    }
-    float get_avoidance_adjusted_climbrate(float target_rate) {
-        return _copter.get_avoidance_adjusted_climbrate(target_rate);
-    }
-    uint16_t get_pilot_speed_dn() {
-        return _copter.get_pilot_speed_dn();
-    }
+    void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
+    float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
+    float get_pilot_desired_yaw_rate(int16_t stick_angle);
+    float get_pilot_desired_climb_rate(float throttle_control);
+    float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f);
+    float get_non_takeoff_throttle(void);
+    void update_simple_mode(void);
+    float get_smoothing_gain(void);
+    bool set_mode(control_mode_t mode, mode_reason_t reason);
+    void set_land_complete(bool b);
+    GCS_Copter &gcs();
+    void Log_Write_Event(uint8_t id);
+    void set_throttle_takeoff(void);
+    void set_auto_yaw_mode(uint8_t yaw_mode);
+    void set_auto_yaw_rate(float turn_rate_cds);
+    void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle);
+    void takeoff_timer_start(float alt_cm);
+    void takeoff_stop(void);
+    void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate);
+    float get_auto_heading(void);
+    float get_auto_yaw_rate_cds(void);
+    float get_avoidance_adjusted_climbrate(float target_rate);
+    uint16_t get_pilot_speed_dn(void);
 
     // end pass-through functions
 
@@ -175,10 +103,9 @@ protected:
 class ModeAcro : public Mode {
 
 public:
-
-    ModeAcro(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
+    
     virtual bool init(bool ignore_checks) override;
     virtual void run() override;
 
@@ -202,10 +129,8 @@ private:
 class ModeAcro_Heli : public ModeAcro {
 
 public:
-
-    ModeAcro_Heli(Copter &copter) :
-        Copter::ModeAcro(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -219,10 +144,8 @@ private:
 class ModeAltHold : public Mode {
 
 public:
-
-    ModeAltHold(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -245,12 +168,8 @@ private:
 class ModeAuto : public Mode {
 
 public:
-
-    ModeAuto(Copter &copter, AP_Mission &_mission, AC_Circle *& _circle_nav) :
-        Copter::Mode(copter),
-        mission(_mission),
-        circle_nav(_circle_nav)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     virtual bool init(bool ignore_checks) override;
     virtual void run() override;
@@ -293,13 +212,9 @@ protected:
     const char *name() const override { return "AUTO"; }
     const char *name4() const override { return "AUTO"; }
 
-    uint32_t wp_distance() const override {
-        return wp_nav->get_wp_distance_to_destination();
-    }
-    int32_t wp_bearing() const override {
-        return wp_nav->get_wp_bearing_to_destination();
-    }
-    void run_autopilot() override { mission.update(); }
+    uint32_t wp_distance() const override;
+    int32_t wp_bearing() const override;
+    void run_autopilot() override;
 
 private:
 
@@ -323,9 +238,6 @@ private:
 
     AutoMode _mode = Auto_TakeOff;   // controls which auto controller is run
 
-    AP_Mission &mission;
-    AC_Circle *&circle_nav;
-
     Location_Class terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const;
 
     void do_takeoff(const AP_Mission::Mission_Command& cmd);
@@ -407,15 +319,12 @@ private:
 
 };
 
-
 #if AUTOTUNE_ENABLED == ENABLED
 class ModeAutoTune : public Mode {
 
 public:
-
-    ModeAutoTune(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -564,10 +473,8 @@ private:
 class ModeBrake : public Mode {
 
 public:
-
-    ModeBrake(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -595,11 +502,8 @@ private:
 class ModeCircle : public Mode {
 
 public:
-
-    ModeCircle(Copter &copter, AC_Circle *& _circle_nav) :
-        Copter::Mode(copter),
-        circle_nav(_circle_nav)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -614,29 +518,21 @@ protected:
     const char *name() const override { return "CIRCLE"; }
     const char *name4() const override { return "CIRC"; }
 
-    uint32_t wp_distance() const override {
-        return wp_nav->get_loiter_distance_to_target();
-    }
-    int32_t wp_bearing() const override {
-        return wp_nav->get_loiter_bearing_to_target();
-    }
+    uint32_t wp_distance() const override;
+    int32_t wp_bearing() const override;
 
 private:
 
     // Circle
     bool pilot_yaw_override = false; // true if pilot is overriding yaw
-    AC_Circle *&circle_nav;
-
 };
 
 
 class ModeDrift : public Mode {
 
 public:
-
-    ModeDrift(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     virtual bool init(bool ignore_checks) override;
     virtual void run() override;
@@ -661,10 +557,8 @@ private:
 class ModeFlip : public Mode {
 
 public:
-
-    ModeFlip(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     virtual bool init(bool ignore_checks) override;
     virtual void run() override;
@@ -690,9 +584,8 @@ private:
 class ModeGuided : public Mode {
 
 public:
-
-    ModeGuided(Copter &copter) :
-        Copter::Mode(copter)        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -749,9 +642,8 @@ private:
 class ModeGuidedNoGPS : public ModeGuided {
 
 public:
-
-    ModeGuidedNoGPS(Copter &copter) :
-        Copter::ModeGuided(copter)        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -774,10 +666,8 @@ private:
 class ModeLand : public Mode {
 
 public:
-
-    ModeLand(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -809,10 +699,8 @@ private:
 class ModeLoiter : public Mode {
 
 public:
-
-    ModeLoiter(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -831,12 +719,8 @@ protected:
     const char *name() const override { return "LOITER"; }
     const char *name4() const override { return "LOIT"; }
 
-    uint32_t wp_distance() const override {
-        return wp_nav->get_loiter_distance_to_target();
-    }
-    int32_t wp_bearing() const override {
-        return wp_nav->get_loiter_bearing_to_target();
-    }
+    uint32_t wp_distance() const override;
+    int32_t wp_bearing() const override;
 
 #if PRECISION_LANDING == ENABLED
     bool do_precision_loiter();
@@ -855,10 +739,8 @@ private:
 class ModePosHold : public Mode {
 
 public:
-
-    ModePosHold(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -889,10 +771,8 @@ private:
 class ModeRTL : public Mode {
 
 public:
-
-    ModeRTL(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override {
@@ -919,12 +799,8 @@ protected:
     const char *name() const override { return "RTL"; }
     const char *name4() const override { return "RTL "; }
 
-    uint32_t wp_distance() const override {
-        return wp_nav->get_wp_distance_to_destination();
-    }
-    int32_t wp_bearing() const override {
-        return wp_nav->get_wp_bearing_to_destination();
-    }
+    uint32_t wp_distance() const override;
+    int32_t wp_bearing() const override;
 
     void descent_start();
     void descent_run();
@@ -965,10 +841,8 @@ private:
 class ModeSmartRTL : public ModeRTL {
 
 public:
-
-    ModeSmartRTL(Copter &copter) :
-        ModeSmartRTL::ModeRTL(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -986,12 +860,8 @@ protected:
     const char *name() const override { return "SMARTRTL"; }
     const char *name4() const override { return "SRTL"; }
 
-    uint32_t wp_distance() const override {
-        return wp_nav->get_wp_distance_to_destination();
-    }
-    int32_t wp_bearing() const override {
-        return wp_nav->get_wp_bearing_to_destination();
-    }
+    uint32_t wp_distance() const override;
+    int32_t wp_bearing() const override;
 
 private:
 
@@ -1007,10 +877,8 @@ private:
 class ModeSport : public Mode {
 
 public:
-
-    ModeSport(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     virtual bool init(bool ignore_checks) override;
     virtual void run() override;
@@ -1033,10 +901,8 @@ private:
 class ModeStabilize : public Mode {
 
 public:
-
-    ModeStabilize(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     virtual bool init(bool ignore_checks) override;
     virtual void run() override;
@@ -1059,10 +925,8 @@ private:
 class ModeStabilize_Heli : public ModeStabilize {
 
 public:
-
-    ModeStabilize_Heli(Copter &copter) :
-        Copter::ModeStabilize(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -1078,10 +942,8 @@ private:
 class ModeThrow : public Mode {
 
 public:
-
-    ModeThrow(Copter &copter) :
-        Copter::Mode(copter)
-        { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -1117,9 +979,8 @@ private:
 class ModeAvoidADSB : public ModeGuided {
 
 public:
-
-    ModeAvoidADSB(Copter &copter) :
-        Copter::ModeGuided(copter) { }
+    // inherit constructor
+    using Copter::Mode::Mode;
 
     bool init(bool ignore_checks) override;
     void run() override;
@@ -1141,6 +1002,7 @@ private:
 };
 
 
+#if OPTFLOW == ENABLED
 /*
   class to support FLOWHOLD mode, which is a position hold mode using
   optical flow directly, avoiding the need for a rangefinder
@@ -1148,10 +1010,8 @@ private:
 
 class ModeFlowHold : public Mode {
 public:
-    ModeFlowHold(Copter &copter) :
-        Copter::Mode(copter) {
-        AP_Param::setup_object_defaults(this, var_info);            
-    }
+    // need a constructor for parameters
+    ModeFlowHold(void);
     
     bool init(bool ignore_checks) override;
     void run(void) override;
@@ -1223,3 +1083,5 @@ private:
     // last time there was significant stick input
     uint32_t last_stick_input_ms;
 };
+#endif // OPTFLOW
+
diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp
index 256d84dc09..35b4cc0211 100644
--- a/ArduCopter/mode_acro.cpp
+++ b/ArduCopter/mode_acro.cpp
@@ -9,8 +9,8 @@
 bool Copter::ModeAcro::init(bool ignore_checks)
 {
    // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
-   if (motors->armed() && ap.land_complete && !_copter.flightmode->has_manual_throttle() &&
-           (get_pilot_desired_throttle(channel_throttle->get_control_in(), _copter.g2.acro_thr_mid) > _copter.get_non_takeoff_throttle())) {
+   if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() &&
+           (get_pilot_desired_throttle(channel_throttle->get_control_in(), copter.g2.acro_thr_mid) > copter.get_non_takeoff_throttle())) {
        return false;
    }
    // set target altitude to zero for reporting
@@ -45,7 +45,7 @@ void Copter::ModeAcro::run()
     attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
 
     // output pilot's throttle without angle boost
-    attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
+    attitude_control->set_throttle_out(pilot_throttle_scaled, false, copter.g.throttle_filt);
 }
 
 
@@ -56,7 +56,7 @@ void Copter::ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pi
     float rate_limit;
     Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
 
-    AP_Vehicle::MultiCopter &aparm = _copter.aparm;
+    AP_Vehicle::MultiCopter &aparm = copter.aparm;
 
     // apply circular limit to pitch and roll inputs
     float total_in = norm(pitch_in, roll_in);
diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp
index 5d56709c9d..cc115a8b98 100644
--- a/ArduCopter/mode_acro_heli.cpp
+++ b/ArduCopter/mode_acro_heli.cpp
@@ -14,7 +14,7 @@ bool Copter::ModeAcro_Heli::init(bool ignore_checks)
     motors->set_acro_tail(true);
     
     // set stab collective false to use full collective pitch range
-    _copter.input_manager.set_use_stab_col(false);
+    copter.input_manager.set_use_stab_col(false);
 
     // always successfully enter acro
     return true;
@@ -34,16 +34,16 @@ void Copter::ModeAcro_Heli::run()
     // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
     
     if(!motors->armed()) {
-        _copter.heli_flags.init_targets_on_arming=true;
+        copter.heli_flags.init_targets_on_arming=true;
         attitude_control->set_attitude_target_to_current_attitude();
         attitude_control->reset_rate_controller_I_terms();
     }
     
-    if(motors->armed() && _copter.heli_flags.init_targets_on_arming) {
+    if(motors->armed() && copter.heli_flags.init_targets_on_arming) {
         attitude_control->set_attitude_target_to_current_attitude();
         attitude_control->reset_rate_controller_I_terms();
         if (motors->get_interlock()) {
-            _copter.heli_flags.init_targets_on_arming=false;
+            copter.heli_flags.init_targets_on_arming=false;
         }
     }   
 
@@ -91,7 +91,7 @@ void Copter::ModeAcro_Heli::run()
     }
 
     // get pilot's desired throttle
-    pilot_throttle_scaled = _copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
+    pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
 
     // output pilot's throttle without angle boost
     attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp
index 1eac191cc8..0aabc7b69d 100644
--- a/ArduCopter/mode_althold.cpp
+++ b/ArduCopter/mode_althold.cpp
@@ -142,14 +142,14 @@ void Copter::ModeAltHold::run()
 
 #if AC_AVOID_ENABLED == ENABLED
         // apply avoidance
-        _copter.avoid.adjust_roll_pitch(target_roll, target_pitch, _copter.aparm.angle_max);
+        copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max);
 #endif
 
         // call attitude controller
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
 
         // adjust climb rate using rangefinder
-        if (_copter.rangefinder_alt_ok()) {
+        if (copter.rangefinder_alt_ok()) {
             // if rangefinder is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
         }
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index 3af5a772ed..95bb25f5e1 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -17,21 +17,38 @@
  *  Code in this file implements the navigation commands
  */
 
+// update mission
+void Copter::ModeAuto::run_autopilot()
+{
+    copter.mission.update();
+}
+
+uint32_t Copter::ModeAuto::wp_distance() const
+{
+    return wp_nav->get_wp_distance_to_destination();
+}
+
+int32_t Copter::ModeAuto::wp_bearing() const
+{
+    return wp_nav->get_wp_bearing_to_destination();
+}
+
+
 // auto_init - initialise auto controller
 bool Copter::ModeAuto::init(bool ignore_checks)
 {
-    if ((_copter.position_ok() && mission.num_commands() > 1) || ignore_checks) {
+    if ((copter.position_ok() && copter.mission.num_commands() > 1) || ignore_checks) {
         _mode = Auto_Loiter;
 
         // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips)
-        if (motors->armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) {
+        if (motors->armed() && ap.land_complete && !copter.mission.starts_with_takeoff_cmd()) {
             gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
             return false;
         }
 
         // stop ROI from carrying over from previous runs of the mission
         // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
-        if (_copter.auto_yaw_mode == AUTO_YAW_ROI) {
+        if (copter.auto_yaw_mode == AUTO_YAW_ROI) {
             set_auto_yaw_mode(AUTO_YAW_HOLD);
         }
 
@@ -39,10 +56,10 @@ bool Copter::ModeAuto::init(bool ignore_checks)
         wp_nav->wp_and_spline_init();
 
         // clear guided limits
-        _copter.mode_guided.limit_clear();
+        copter.mode_guided.limit_clear();
 
         // start/resume the mission (based on MIS_RESTART parameter)
-        mission.start_or_resume();
+        copter.mission.start_or_resume();
         return true;
     }else{
         return false;
@@ -107,21 +124,21 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
     Location_Class dest(dest_loc);
 
     // set horizontal target
-    dest.lat = _copter.current_loc.lat;
-    dest.lng = _copter.current_loc.lng;
+    dest.lat = copter.current_loc.lat;
+    dest.lng = copter.current_loc.lng;
 
     // get altitude target
     int32_t alt_target;
     if (!dest.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target)) {
         // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
-        _copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
+        copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
         // fall back to altitude above current altitude
-        alt_target = _copter.current_loc.alt + dest.alt;
+        alt_target = copter.current_loc.alt + dest.alt;
     }
 
     // sanity check target
-    if (alt_target < _copter.current_loc.alt) {
-        dest.set_alt_cm(_copter.current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
+    if (alt_target < copter.current_loc.alt) {
+        dest.set_alt_cm(copter.current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
     }
     // Note: if taking off from below home this could cause a climb to an unexpectedly high altitude
     if (alt_target < 100) {
@@ -131,7 +148,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
     // set waypoint controller target
     if (!wp_nav->set_wp_destination(dest)) {
         // failure to set destination can only be because of missing terrain data
-        _copter.failsafe_terrain_on_event();
+        copter.failsafe_terrain_on_event();
         return;
     }
 
@@ -142,7 +159,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
     set_throttle_takeoff();
 
     // get initial alt for WP_NAVALT_MIN
-    _copter.auto_takeoff_set_start_alt();
+    copter.auto_takeoff_set_start_alt();
 }
 
 // auto_takeoff_run - takeoff in auto mode
@@ -161,7 +178,7 @@ void Copter::ModeAuto::takeoff_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
     }
@@ -182,13 +199,13 @@ void Copter::ModeAuto::takeoff_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    _copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
+    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control->update_z_controller();
 
     // call attitude controller
-    _copter.auto_takeoff_attitude_run(target_yaw_rate);
+    copter.auto_takeoff_attitude_run(target_yaw_rate);
 }
 
 // auto_wp_start - initialises waypoint controller to implement flying to a particular destination
@@ -201,8 +218,8 @@ void Copter::ModeAuto::wp_start(const Vector3f& destination)
 
     // initialise yaw
     // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (_copter.auto_yaw_mode != AUTO_YAW_ROI) {
-        set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
+    if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
+        set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
     }
 }
 
@@ -214,14 +231,14 @@ void Copter::ModeAuto::wp_start(const Location_Class& dest_loc)
     // send target to waypoint controller
     if (!wp_nav->set_wp_destination(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
-        _copter.failsafe_terrain_on_event();
+        copter.failsafe_terrain_on_event();
         return;
     }
 
     // initialise yaw
     // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (_copter.auto_yaw_mode != AUTO_YAW_ROI) {
-        set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
+    if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
+        set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
     }
 }
 
@@ -241,7 +258,7 @@ void Copter::ModeAuto::wp_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -253,13 +270,13 @@ void Copter::ModeAuto::wp_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    _copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
+    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
     }else{
@@ -279,14 +296,14 @@ void Copter::ModeAuto::spline_start(const Location_Class& destination, bool stop
     // initialise wpnav
     if (!wp_nav->set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
         // failure to set destination can only be because of missing terrain data
-        _copter.failsafe_terrain_on_event();
+        copter.failsafe_terrain_on_event();
         return;
     }
 
     // initialise yaw
     // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (_copter.auto_yaw_mode != AUTO_YAW_ROI) {
-        set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
+    if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
+        set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
     }
 }
 
@@ -306,7 +323,7 @@ void Copter::ModeAuto::spline_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // get pilot's desired yaw rat
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -324,7 +341,7 @@ void Copter::ModeAuto::spline_run()
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
     }else{
@@ -377,8 +394,8 @@ void Copter::ModeAuto::land_run()
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
     
-    _copter.land_run_horizontal_control();
-    _copter.land_run_vertical_control();
+    copter.land_run_horizontal_control();
+    copter.land_run_vertical_control();
 }
 
 bool Copter::ModeAuto::landing_gear_should_be_deployed() const
@@ -387,7 +404,7 @@ bool Copter::ModeAuto::landing_gear_should_be_deployed() const
     case Auto_Land:
         return true;
     case Auto_RTL:
-        return _copter.mode_rtl.landing_gear_should_be_deployed();
+        return copter.mode_rtl.landing_gear_should_be_deployed();
     default:
         return false;
     }
@@ -400,7 +417,7 @@ void Copter::ModeAuto::rtl_start()
     _mode = Auto_RTL;
 
     // call regular rtl flight mode initialisation and ask it to ignore checks
-    _copter.mode_rtl.init(true);
+    copter.mode_rtl.init(true);
 }
 
 // auto_rtl_run - rtl in AUTO flight mode
@@ -408,7 +425,7 @@ void Copter::ModeAuto::rtl_start()
 void Copter::ModeAuto::rtl_run()
 {
     // call regular rtl flight mode run function
-    _copter.mode_rtl.run(false);
+    copter.mode_rtl.run(false);
 }
 
 // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
@@ -420,18 +437,18 @@ void Copter::ModeAuto::circle_movetoedge_start(const Location_Class &circle_cent
     if (!circle_center.get_vector_from_origin_NEU(circle_center_neu)) {
         // default to current position and log error
         circle_center_neu = inertial_nav.get_position();
-        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT);
+        copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT);
     }
-    circle_nav->set_center(circle_center_neu);
+    copter.circle_nav->set_center(circle_center_neu);
 
     // set circle radius
     if (!is_zero(radius_m)) {
-        circle_nav->set_radius(radius_m * 100.0f);
+        copter.circle_nav->set_radius(radius_m * 100.0f);
     }
 
     // check our distance from edge of circle
     Vector3f circle_edge_neu;
-    circle_nav->get_closest_point_on_circle(circle_edge_neu);
+    copter.circle_nav->get_closest_point_on_circle(circle_edge_neu);
     float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length();
 
     // if more than 3m then fly to edge
@@ -448,14 +465,14 @@ void Copter::ModeAuto::circle_movetoedge_start(const Location_Class &circle_cent
         // initialise wpnav to move to edge of circle
         if (!wp_nav->set_wp_destination(circle_edge)) {
             // failure to set destination can only be because of missing terrain data
-            _copter.failsafe_terrain_on_event();
+            copter.failsafe_terrain_on_event();
         }
 
         // if we are outside the circle, point at the edge, otherwise hold yaw
         const Vector3f &curr_pos = inertial_nav.get_position();
         float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
-        if (dist_to_center > circle_nav->get_radius() && dist_to_center > 500) {
-            set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
+        if (dist_to_center > copter.circle_nav->get_radius() && dist_to_center > 500) {
+            set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
         } else {
             // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
             set_auto_yaw_mode(AUTO_YAW_HOLD);
@@ -472,7 +489,7 @@ void Copter::ModeAuto::circle_start()
     _mode = Auto_Circle;
 
     // initialise circle controller
-    circle_nav->init(circle_nav->get_center());
+    copter.circle_nav->init(copter.circle_nav->get_center());
 }
 
 // auto_circle_run - circle in AUTO flight mode
@@ -480,13 +497,13 @@ void Copter::ModeAuto::circle_start()
 void Copter::ModeAuto::circle_run()
 {
     // call circle controller
-    circle_nav->update();
+    copter.circle_nav->update();
 
     // call z-axis position controller
     pos_control->update_z_controller();
 
     // roll & pitch from waypoint controller, yaw rate from pilot
-    attitude_control->input_euler_angle_roll_pitch_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), circle_nav->get_yaw(),true, get_smoothing_gain());
+    attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(),true, get_smoothing_gain());
 }
 
 #if NAV_GUIDED == ENABLED
@@ -496,10 +513,10 @@ void Copter::ModeAuto::nav_guided_start()
     _mode = Auto_NavGuided;
 
     // call regular guided flight mode initialisation
-    _copter.mode_guided.init(true);
+    copter.mode_guided.init(true);
 
     // initialise guided start time and position as reference for limit checking
-    _copter.mode_guided.limit_init_time_and_pos();
+    copter.mode_guided.limit_init_time_and_pos();
 }
 
 // auto_nav_guided_run - allows control by external navigation controller
@@ -507,7 +524,7 @@ void Copter::ModeAuto::nav_guided_start()
 void Copter::ModeAuto::nav_guided_run()
 {
     // call regular guided flight mode run function
-    _copter.mode_guided.run();
+    copter.mode_guided.run();
 }
 #endif  // NAV_GUIDED
 
@@ -516,7 +533,7 @@ void Copter::ModeAuto::nav_guided_run()
 bool Copter::ModeAuto::loiter_start()
 {
     // return failure if GPS is bad
-    if (!_copter.position_ok()) {
+    if (!copter.position_ok()) {
         return false;
     }
     _mode = Auto_Loiter;
@@ -546,7 +563,7 @@ void Copter::ModeAuto::loiter_run()
 
     // accept pilot input of yaw
     float target_yaw_rate = 0;
-    if(!_copter.failsafe.radio) {
+    if(!copter.failsafe.radio) {
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
     }
 
@@ -554,7 +571,7 @@ void Copter::ModeAuto::loiter_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint and z-axis position controller
-    _copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
+    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
     pos_control->update_z_controller();
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
@@ -833,7 +850,7 @@ void Copter::ModeAuto::payload_place_run()
 void Copter::ModeAuto::payload_place_run_loiter()
 {
     // loiter...
-    _copter.land_run_horizontal_control();
+    copter.land_run_horizontal_control();
 
     // run loiter controller
     wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
@@ -848,6 +865,6 @@ void Copter::ModeAuto::payload_place_run_loiter()
 
 void Copter::ModeAuto::payload_place_run_descend()
 {
-    _copter.land_run_horizontal_control();
-    _copter.land_run_vertical_control();
+    copter.land_run_horizontal_control();
+    copter.land_run_vertical_control();
 }
diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp
index 986a5beef4..1b2ab0e4c9 100644
--- a/ArduCopter/mode_autotune.cpp
+++ b/ArduCopter/mode_autotune.cpp
@@ -138,7 +138,7 @@ bool Copter::ModeAutoTune::init(bool ignore_checks)
     }
 
     // only do position hold if starting autotune from LOITER or POSHOLD
-    use_poshold = (_copter.control_mode == LOITER || _copter.control_mode == POSHOLD);
+    use_poshold = (copter.control_mode == LOITER || copter.control_mode == POSHOLD);
     have_position = false;
 
     return success;
@@ -165,8 +165,8 @@ void Copter::ModeAutoTune::stop()
 bool Copter::ModeAutoTune::start(bool ignore_checks)
 {
     // only allow flip from Stabilize, AltHold,  PosHold or Loiter modes
-    if (_copter.control_mode != STABILIZE && _copter.control_mode != ALT_HOLD &&
-        _copter.control_mode != LOITER && _copter.control_mode != POSHOLD) {
+    if (copter.control_mode != STABILIZE && copter.control_mode != ALT_HOLD &&
+        copter.control_mode != LOITER && copter.control_mode != POSHOLD) {
         return false;
     }
 
@@ -337,7 +337,7 @@ void Copter::ModeAutoTune::run()
     update_simple_mode();
 
     // get pilot desired lean angles
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
+    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
 
     // get pilot's desired yaw rate
     target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
@@ -601,25 +601,25 @@ void Copter::ModeAutoTune::autotune_attitude_control()
         switch (axis) {
         case ROLL:
             if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
-                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f), _copter.scheduler.get_loop_period_s());
+                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f), copter.scheduler.get_loop_period_s());
             } else {
-                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - start_rate), _copter.scheduler.get_loop_period_s());
+                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().x) * 100.0f - start_rate), copter.scheduler.get_loop_period_s());
             }
             lean_angle = direction_sign * (ahrs.roll_sensor - (int32_t)start_angle);
             break;
         case PITCH:
             if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
-                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f), _copter.scheduler.get_loop_period_s());
+                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f), copter.scheduler.get_loop_period_s());
             } else {
-                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - start_rate), _copter.scheduler.get_loop_period_s());
+                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().y) * 100.0f - start_rate), copter.scheduler.get_loop_period_s());
             }
             lean_angle = direction_sign * (ahrs.pitch_sensor - (int32_t)start_angle);
             break;
         case YAW:
             if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
-                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f), _copter.scheduler.get_loop_period_s());
+                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f), copter.scheduler.get_loop_period_s());
             } else {
-                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - start_rate), _copter.scheduler.get_loop_period_s());
+                rotation_rate = rotation_rate_filt.apply(direction_sign * (ToDeg(ahrs.get_gyro().z) * 100.0f - start_rate), copter.scheduler.get_loop_period_s());
             }
             lean_angle = direction_sign * wrap_180_cd(ahrs.yaw_sensor-(int32_t)start_angle);
             break;
@@ -650,7 +650,7 @@ void Copter::ModeAutoTune::autotune_attitude_control()
 
         // log this iterations lean angle and rotation rate
         Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
-        _copter.DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
+        copter.DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
         break;
 
     case UPDATE_GAINS:
@@ -1456,7 +1456,7 @@ void Copter::ModeAutoTune::get_poshold_attitude(float &roll_cd_out, float &pitch
     }
 
     // do we know where we are?
-    if (!_copter.position_ok()) {
+    if (!copter.position_ok()) {
         return;
     }
 
diff --git a/ArduCopter/mode_avoid_adsb.cpp b/ArduCopter/mode_avoid_adsb.cpp
index 6680d70c70..dd4e80741b 100644
--- a/ArduCopter/mode_avoid_adsb.cpp
+++ b/ArduCopter/mode_avoid_adsb.cpp
@@ -19,7 +19,7 @@ bool Copter::ModeAvoidADSB::init(const bool ignore_checks)
 bool Copter::ModeAvoidADSB::set_velocity(const Vector3f& velocity_neu)
 {
     // check flight mode
-    if (_copter.control_mode != AVOID_ADSB) {
+    if (copter.control_mode != AVOID_ADSB) {
         return false;
     }
 
diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp
index e9c7042ec6..e8a9194d82 100644
--- a/ArduCopter/mode_brake.cpp
+++ b/ArduCopter/mode_brake.cpp
@@ -7,7 +7,7 @@
 // brake_init - initialise brake controller
 bool Copter::ModeBrake::init(bool ignore_checks)
 {
-    if (_copter.position_ok() || ignore_checks) {
+    if (copter.position_ok() || ignore_checks) {
 
         // set desired acceleration to zero
         wp_nav->clear_pilot_desired_acceleration();
@@ -52,7 +52,7 @@ void Copter::ModeBrake::run()
 
     // if landed immediately disarm
     if (ap.land_complete) {
-        _copter.init_disarm_motors();
+        copter.init_disarm_motors();
     }
 
     // set motors to full range
@@ -71,8 +71,8 @@ void Copter::ModeBrake::run()
     pos_control->update_z_controller();
 
     if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) {
-        if (!_copter.set_mode(LOITER, MODE_REASON_BRAKE_TIMEOUT)) {
-            _copter.set_mode(ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT);
+        if (!copter.set_mode(LOITER, MODE_REASON_BRAKE_TIMEOUT)) {
+            copter.set_mode(ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT);
         }
     }
 }
diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp
index e8f09f338f..ab77ce0c71 100644
--- a/ArduCopter/mode_circle.cpp
+++ b/ArduCopter/mode_circle.cpp
@@ -7,7 +7,7 @@
 // circle_init - initialise circle controller flight mode
 bool Copter::ModeCircle::init(bool ignore_checks)
 {
-    if (_copter.position_ok() || ignore_checks) {
+    if (copter.position_ok() || ignore_checks) {
         pilot_yaw_override = false;
 
         // initialize speeds and accelerations
@@ -18,7 +18,7 @@ bool Copter::ModeCircle::init(bool ignore_checks)
         pos_control->set_accel_z(g.pilot_accel_z);
 
         // initialise circle controller including setting the circle center based on vehicle speed
-        circle_nav->init();
+        copter.circle_nav->init();
 
         return true;
     }else{
@@ -48,7 +48,7 @@ void Copter::ModeCircle::run()
     }
 
     // process pilot inputs
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -71,17 +71,21 @@ void Copter::ModeCircle::run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run circle controller
-    circle_nav->update();
+    copter.circle_nav->update();
 
     // call attitude controller
     if (pilot_yaw_override) {
-        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(),
+                                                                      copter.circle_nav->get_pitch(),
+                                                                      target_yaw_rate, get_smoothing_gain());
     }else{
-        attitude_control->input_euler_angle_roll_pitch_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), circle_nav->get_yaw(),true, get_smoothing_gain());
+        attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(),
+                                                           copter.circle_nav->get_pitch(),
+                                                           copter.circle_nav->get_yaw(),true, get_smoothing_gain());
     }
 
     // adjust climb rate using rangefinder
-    if (_copter.rangefinder_alt_ok()) {
+    if (copter.rangefinder_alt_ok()) {
         // if rangefinder is ok, use surface tracking
         target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
     }
@@ -89,3 +93,13 @@ void Copter::ModeCircle::run()
     pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
     pos_control->update_z_controller();
 }
+
+uint32_t Copter::ModeCircle::wp_distance() const
+{
+    return wp_nav->get_loiter_distance_to_target();
+}
+
+int32_t Copter::ModeCircle::wp_bearing() const
+{
+    return wp_nav->get_loiter_bearing_to_target();
+}
diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp
index cdf000736f..6e1eb45198 100644
--- a/ArduCopter/mode_drift.cpp
+++ b/ArduCopter/mode_drift.cpp
@@ -29,7 +29,7 @@
 // drift_init - initialise drift controller
 bool Copter::ModeDrift::init(bool ignore_checks)
 {
-    if (_copter.position_ok() || ignore_checks) {
+    if (copter.position_ok() || ignore_checks) {
         return true;
     }else{
         return false;
@@ -58,7 +58,7 @@ void Copter::ModeDrift::run()
     }
 
     // convert pilot input to lean angles
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
+    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
 
     // get pilot's desired throttle
     pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp
index 3e71904fc9..c680d5b135 100644
--- a/ArduCopter/mode_flip.cpp
+++ b/ArduCopter/mode_flip.cpp
@@ -41,15 +41,15 @@ int8_t    flip_pitch_dir;           // pitch direction (-1 = pitch forward, 1 =
 bool Copter::ModeFlip::init(bool ignore_checks)
 {
     // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
-    if (_copter.control_mode != ACRO &&
-        _copter.control_mode != STABILIZE &&
-        _copter.control_mode != ALT_HOLD &&
-        _copter.control_mode != FLOWHOLD) {
+    if (copter.control_mode != ACRO &&
+        copter.control_mode != STABILIZE &&
+        copter.control_mode != ALT_HOLD &&
+        copter.control_mode != FLOWHOLD) {
         return false;
     }
 
     // if in acro or stabilize ensure throttle is above zero
-    if (ap.throttle_zero && (_copter.control_mode == ACRO || _copter.control_mode == STABILIZE)) {
+    if (ap.throttle_zero && (copter.control_mode == ACRO || copter.control_mode == STABILIZE)) {
         return false;
     }
 
@@ -64,7 +64,7 @@ bool Copter::ModeFlip::init(bool ignore_checks)
     }
 
     // capture original flight mode so that we can return to it after completion
-    flip_orig_control_mode = _copter.control_mode;
+    flip_orig_control_mode = copter.control_mode;
 
     // initialise state
     flip_state = Flip_Start;
@@ -87,7 +87,7 @@ bool Copter::ModeFlip::init(bool ignore_checks)
     Log_Write_Event(DATA_FLIP_START);
 
     // capture current attitude which will be used during the Flip_Recovery stage
-    float angle_max = _copter.aparm.angle_max;
+    float angle_max = copter.aparm.angle_max;
     flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -angle_max, angle_max);
     flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -angle_max, angle_max);
     flip_orig_attitude.z = ahrs.yaw_sensor;
@@ -196,9 +196,9 @@ void Copter::ModeFlip::run()
         // check for successful recovery
         if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
             // restore original flight mode
-            if (!_copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
+            if (!copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
                 // this should never happen but just in case
-                _copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
+                copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
             }
             // log successful completion
             Log_Write_Event(DATA_FLIP_END);
@@ -207,12 +207,12 @@ void Copter::ModeFlip::run()
 
     case Flip_Abandon:
         // restore original flight mode
-        if (!_copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
+        if (!copter.set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
             // this should never happen but just in case
-            _copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
+            copter.set_mode(STABILIZE, MODE_REASON_UNKNOWN);
         }
         // log abandoning flip
-        _copter.Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED);
+        copter.Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED);
         break;
     }
 
diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp
index ced6f95dae..515b451925 100644
--- a/ArduCopter/mode_flowhold.cpp
+++ b/ArduCopter/mode_flowhold.cpp
@@ -46,12 +46,12 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_FILT_HZ", 3, Copter::ModeFlowHold, flow_filter_hz, 5),
 
-    // @Param: _MIN_QUAL
+    // @Param: _QUAL_MIN
     // @DisplayName: Minimum flow quality
     // @Description: Minimum flow quality to use flow position hold
     // @Range: 0 255
     // @User: Standard
-    AP_GROUPINFO("_MIN_QUAL", 4, Copter::ModeFlowHold, flow_min_quality, 10),
+    AP_GROUPINFO("_QUAL_MIN", 4, Copter::ModeFlowHold, flow_min_quality, 10),
 
     // 5 was FLOW_SPEED
     
@@ -66,13 +66,18 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
     AP_GROUPEND
 };
 
+Copter::ModeFlowHold::ModeFlowHold(void) : Mode()
+{
+    AP_Param::setup_object_defaults(this, var_info);            
+}
+
 #define CONTROL_FLOWHOLD_EARTH_FRAME 0
 
 // flowhold_init - initialise flowhold controller
 bool Copter::ModeFlowHold::init(bool ignore_checks)
 {
 #if FRAME_CONFIG == HELI_FRAME
-    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete
+    // do not allow helis to enter Flow Hold if the Rotor Runup is not complete
     if (!ignore_checks && !motors->rotor_runup_complete()){
         return false;
     }
@@ -279,7 +284,7 @@ void Copter::ModeFlowHold::run()
     bf_angles.x = constrain_float(bf_angles.x, -angle_max, angle_max);
     bf_angles.y = constrain_float(bf_angles.y, -angle_max, angle_max);
             
-    // Alt Hold State Machine
+    // Flow Hold State Machine
     switch (flowhold_state) {
 
     case FlowHold_MotorStopped:
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
index 0939486006..cc27ea530a 100644
--- a/ArduCopter/mode_guided.cpp
+++ b/ArduCopter/mode_guided.cpp
@@ -38,9 +38,9 @@ struct Guided_Limit {
 // guided_init - initialise guided controller
 bool Copter::ModeGuided::init(bool ignore_checks)
 {
-    if (_copter.position_ok() || ignore_checks) {
+    if (copter.position_ok() || ignore_checks) {
         // initialise yaw
-        set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
+        set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
         // start in position control mode
         pos_control_start();
         return true;
@@ -56,12 +56,12 @@ bool Copter::ModeGuided::takeoff_start(float final_alt_above_home)
     guided_mode = Guided_TakeOff;
 
     // initialise wpnav destination
-    Location_Class target_loc = _copter.current_loc;
+    Location_Class target_loc = copter.current_loc;
     target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME);
 
     if (!wp_nav->set_wp_destination(target_loc)) {
         // failure to set destination can only be because of missing terrain data
-        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
+        copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
         // failure is propagated to GCS with NAK
         return false;
     }
@@ -73,7 +73,7 @@ bool Copter::ModeGuided::takeoff_start(float final_alt_above_home)
     set_throttle_takeoff();
 
     // get initial alt for WP_NAVALT_MIN
-    _copter.auto_takeoff_set_start_alt();
+    copter.auto_takeoff_set_start_alt();
 
     return true;
 }
@@ -95,7 +95,7 @@ void Copter::ModeGuided::pos_control_start()
     wp_nav->set_wp_destination(stopping_point, false);
 
     // initialise yaw
-    set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
+    set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
 }
 
 // initialise guided mode's velocity controller
@@ -163,7 +163,7 @@ void Copter::ModeGuided::angle_control_start()
 
     // initialise targets
     guided_angle_state.update_time_ms = millis();
-    guided_angle_state.roll_cd = _copter.ahrs.roll_sensor;
+    guided_angle_state.roll_cd = copter.ahrs.roll_sensor;
     guided_angle_state.pitch_cd = ahrs.pitch_sensor;
     guided_angle_state.yaw_cd = ahrs.yaw_sensor;
     guided_angle_state.climb_rate_cms = 0.0f;
@@ -187,8 +187,8 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y
 #if AC_FENCE == ENABLED
     // reject destination if outside the fence
     Location_Class dest_loc(destination);
-    if (!_copter.fence.check_destination_within_fence(dest_loc)) {
-        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
+    if (!copter.fence.check_destination_within_fence(dest_loc)) {
+        copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
         // failure is propagated to GCS with NAK
         return false;
     }
@@ -201,7 +201,7 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y
     wp_nav->set_wp_destination(destination, false);
 
     // log target
-    _copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
+    copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
     return true;
 }
 
@@ -218,8 +218,8 @@ bool Copter::ModeGuided::set_destination(const Location_Class& dest_loc, bool us
 #if AC_FENCE == ENABLED
     // reject destination outside the fence.
     // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
-    if (!_copter.fence.check_destination_within_fence(dest_loc)) {
-        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
+    if (!copter.fence.check_destination_within_fence(dest_loc)) {
+        copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
         // failure is propagated to GCS with NAK
         return false;
     }
@@ -227,7 +227,7 @@ bool Copter::ModeGuided::set_destination(const Location_Class& dest_loc, bool us
 
     if (!wp_nav->set_wp_destination(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
-        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
+        copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
         // failure is propagated to GCS with NAK
         return false;
     }
@@ -236,7 +236,7 @@ bool Copter::ModeGuided::set_destination(const Location_Class& dest_loc, bool us
     set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
 
     // log target
-    _copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
+    copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
     return true;
 }
 
@@ -256,7 +256,7 @@ void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, fl
     vel_update_time_ms = millis();
 
     // log target
-    _copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
+    copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
 }
 
 // set guided mode posvel target
@@ -270,8 +270,8 @@ bool Copter::ModeGuided::set_destination_posvel(const Vector3f& destination, con
 #if AC_FENCE == ENABLED
     // reject destination if outside the fence
     Location_Class dest_loc(destination);
-    if (!_copter.fence.check_destination_within_fence(dest_loc)) {
-        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
+    if (!copter.fence.check_destination_within_fence(dest_loc)) {
+        copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
         // failure is propagated to GCS with NAK
         return false;
     }
@@ -284,10 +284,10 @@ bool Copter::ModeGuided::set_destination_posvel(const Vector3f& destination, con
     guided_pos_target_cm = destination;
     guided_vel_target_cms = velocity;
 
-    _copter.pos_control->set_pos_target(guided_pos_target_cm);
+    copter.pos_control->set_pos_target(guided_pos_target_cm);
 
     // log target
-    _copter.Log_Write_GuidedTarget(guided_mode, destination, velocity);
+    copter.Log_Write_GuidedTarget(guided_mode, destination, velocity);
     return true;
 }
 
@@ -312,11 +312,11 @@ void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bo
 
     // interpret positive climb rate as triggering take-off
     if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
-        _copter.set_auto_armed(true);
+        copter.set_auto_armed(true);
     }
 
     // log target
-    _copter.Log_Write_GuidedTarget(guided_mode,
+    copter.Log_Write_GuidedTarget(guided_mode,
                            Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
                            Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms));
 }
@@ -371,7 +371,7 @@ void Copter::ModeGuided::takeoff_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
     }
@@ -392,13 +392,13 @@ void Copter::ModeGuided::takeoff_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    _copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
+    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
     // call z-axis position controller (wpnav should have already updated it's alt target)
-    _copter.pos_control->update_z_controller();
+    copter.pos_control->update_z_controller();
 
     // call attitude controller
-    _copter.auto_takeoff_attitude_run(target_yaw_rate);
+    copter.auto_takeoff_attitude_run(target_yaw_rate);
 }
 
 // guided_pos_control_run - runs the guided position controller
@@ -413,7 +413,7 @@ void Copter::ModeGuided::pos_control_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -425,13 +425,13 @@ void Copter::ModeGuided::pos_control_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    _copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
+    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
     } else if (auto_yaw_mode == AUTO_YAW_RATE) {
@@ -457,7 +457,7 @@ void Copter::ModeGuided::vel_control_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -485,7 +485,7 @@ void Copter::ModeGuided::vel_control_run()
     pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
 
     // call attitude controller
-    if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
     } else if (auto_yaw_mode == AUTO_YAW_RATE) {
@@ -513,7 +513,7 @@ void Copter::ModeGuided::posvel_control_run()
     // process pilot's yaw input
     float target_yaw_rate = 0;
 
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -557,7 +557,7 @@ void Copter::ModeGuided::posvel_control_run()
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
     } else if (auto_yaw_mode == AUTO_YAW_RATE) {
@@ -587,7 +587,7 @@ void Copter::ModeGuided::angle_control_run()
     float roll_in = guided_angle_state.roll_cd;
     float pitch_in = guided_angle_state.pitch_cd;
     float total_in = norm(roll_in, pitch_in);
-    float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), _copter.aparm.angle_max);
+    float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), copter.aparm.angle_max);
     if (total_in > angle_max) {
         float ratio = angle_max / total_in;
         roll_in *= ratio;
@@ -653,7 +653,7 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const
 
 #if AC_AVOID_ENABLED
     // limit the velocity to prevent fence violations
-    _copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt);
+    copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt);
     // get avoidance adjusted climb rate
     curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z);    
 #endif
diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp
index df013d031a..d2d63e4a1a 100644
--- a/ArduCopter/mode_land.cpp
+++ b/ArduCopter/mode_land.cpp
@@ -10,7 +10,7 @@ static bool land_pause;
 bool Copter::ModeLand::init(bool ignore_checks)
 {
     // check if we have GPS and decide which LAND we're going to do
-    land_with_gps = _copter.position_ok();
+    land_with_gps = copter.position_ok();
     if (land_with_gps) {
         // set target to stopping point
         Vector3f stopping_point;
@@ -61,7 +61,7 @@ void Copter::ModeLand::gps_run()
 
         // disarm when the landing detector says we've landed
         if (ap.land_complete) {
-            _copter.init_disarm_motors();
+            copter.init_disarm_motors();
         }
         return;
     }
@@ -74,8 +74,8 @@ void Copter::ModeLand::gps_run()
         land_pause = false;
     }
     
-    _copter.land_run_horizontal_control();
-    _copter.land_run_vertical_control(land_pause);
+    copter.land_run_horizontal_control();
+    copter.land_run_vertical_control(land_pause);
 }
 
 // land_nogps_run - runs the land controller
@@ -87,11 +87,11 @@ void Copter::ModeLand::nogps_run()
     float target_yaw_rate = 0;
 
     // process pilot inputs
-    if (!_copter.failsafe.radio) {
-        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && _copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
+    if (!copter.failsafe.radio) {
+        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
             Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
             // exit land if throttle is high
-            _copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
+            copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
         }
 
         if (g.land_repositioning) {
@@ -99,7 +99,7 @@ void Copter::ModeLand::nogps_run()
             update_simple_mode();
 
             // get pilot desired lean angles
-            get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
+            get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
         }
 
         // get pilot's desired yaw rate
@@ -120,7 +120,7 @@ void Copter::ModeLand::nogps_run()
 
         // disarm when the landing detector says we've landed
         if (ap.land_complete) {
-            _copter.init_disarm_motors();
+            copter.init_disarm_motors();
         }
         return;
     }
@@ -136,7 +136,7 @@ void Copter::ModeLand::nogps_run()
         land_pause = false;
     }
 
-    _copter.land_run_vertical_control(land_pause);
+    copter.land_run_vertical_control(land_pause);
 }
 
 /*
@@ -145,12 +145,12 @@ void Copter::ModeLand::nogps_run()
 int32_t Copter::ModeLand::get_alt_above_ground(void)
 {
     int32_t alt_above_ground;
-    if (_copter.rangefinder_alt_ok()) {
-        alt_above_ground = _copter.rangefinder_state.alt_cm_filt.get();
+    if (copter.rangefinder_alt_ok()) {
+        alt_above_ground = copter.rangefinder_state.alt_cm_filt.get();
     } else {
         bool navigating = pos_control->is_active_xy();
-        if (!navigating || !_copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
-            alt_above_ground = _copter.current_loc.alt;
+        if (!navigating || !copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
+            alt_above_ground = copter.current_loc.alt;
         }
     }
     return alt_above_ground;
diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp
index 425cbf3a3a..b31b09a0cd 100644
--- a/ArduCopter/mode_loiter.cpp
+++ b/ArduCopter/mode_loiter.cpp
@@ -7,7 +7,7 @@
 // loiter_init - initialise loiter controller
 bool Copter::ModeLoiter::init(bool ignore_checks)
 {
-    if (_copter.position_ok() || ignore_checks) {
+    if (copter.position_ok() || ignore_checks) {
 
         // set target to current position
         wp_nav->init_loiter_target();
@@ -41,7 +41,7 @@ bool Copter::ModeLoiter::do_precision_loiter()
     if (wp_nav->get_pilot_desired_acceleration().length() > 50.0f) {
         return false;
     }
-    if (!_copter.precland.target_acquired()) {
+    if (!copter.precland.target_acquired()) {
         return false; // we don't have a good vector
     }
     return true;
@@ -51,11 +51,11 @@ void Copter::ModeLoiter::precision_loiter_xy()
 {
     wp_nav->clear_pilot_desired_acceleration();
     Vector2f target_pos, target_vel_rel;
-    if (!_copter.precland.get_target_position_cm(target_pos)) {
+    if (!copter.precland.get_target_position_cm(target_pos)) {
         target_pos.x = inertial_nav.get_position().x;
         target_pos.y = inertial_nav.get_position().y;
     }
-    if (!_copter.precland.get_target_velocity_relative_cms(target_vel_rel)) {
+    if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) {
         target_vel_rel.x = -inertial_nav.get_velocity().x;
         target_vel_rel.y = -inertial_nav.get_velocity().y;
     }
@@ -78,7 +78,7 @@ void Copter::ModeLoiter::run()
     pos_control->set_accel_z(g.pilot_accel_z);
 
     // process pilot inputs unless we are in radio failsafe
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // apply SIMPLE mode transform to pilot inputs
         update_simple_mode();
 
@@ -196,7 +196,7 @@ void Copter::ModeLoiter::run()
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
 
         // adjust climb rate using rangefinder
-        if (_copter.rangefinder_alt_ok()) {
+        if (copter.rangefinder_alt_ok()) {
             // if rangefinder is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
         }
@@ -210,3 +210,13 @@ void Copter::ModeLoiter::run()
         break;
     }
 }
+
+uint32_t Copter::ModeLoiter::wp_distance() const
+{
+    return wp_nav->get_loiter_distance_to_target();
+}
+
+int32_t Copter::ModeLoiter::wp_bearing() const
+{
+    return wp_nav->get_loiter_bearing_to_target();
+}
diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp
index cff98118d5..c494ec84d4 100644
--- a/ArduCopter/mode_poshold.cpp
+++ b/ArduCopter/mode_poshold.cpp
@@ -74,7 +74,7 @@ static struct {
 bool Copter::ModePosHold::init(bool ignore_checks)
 {
     // fail to initialise PosHold mode if no GPS lock
-    if (!_copter.position_ok() && !ignore_checks) {
+    if (!copter.position_ok() && !ignore_checks) {
         return false;
     }
     
@@ -147,7 +147,7 @@ void Copter::ModePosHold::run()
     }
 
     // process pilot inputs
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // apply SIMPLE mode transform to pilot inputs
         update_simple_mode();
 
@@ -201,7 +201,7 @@ void Copter::ModePosHold::run()
         return;
     }else{
         // convert pilot input to lean angles
-        get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
+        get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
 
         // convert inertial nav earth-frame velocities to body-frame
         // To-Do: move this to AP_Math (or perhaps we already have a function to do this)
@@ -517,7 +517,7 @@ void Copter::ModePosHold::run()
         }
         
         // constrain target pitch/roll angles
-        float angle_max = _copter.aparm.angle_max;
+        float angle_max = copter.aparm.angle_max;
         poshold.roll = constrain_int16(poshold.roll, -angle_max, angle_max);
         poshold.pitch = constrain_int16(poshold.pitch, -angle_max, angle_max);
 
@@ -525,7 +525,7 @@ void Copter::ModePosHold::run()
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate, get_smoothing_gain());
 
         // adjust climb rate using rangefinder
-        if (_copter.rangefinder_alt_ok()) {
+        if (copter.rangefinder_alt_ok()) {
             // if rangefinder is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
         }
diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp
index 8979019dff..5d871100f0 100644
--- a/ArduCopter/mode_rtl.cpp
+++ b/ArduCopter/mode_rtl.cpp
@@ -10,10 +10,10 @@
 // rtl_init - initialise rtl controller
 bool Copter::ModeRTL::init(bool ignore_checks)
 {
-    if (_copter.position_ok() || ignore_checks) {
+    if (copter.position_ok() || ignore_checks) {
         // initialise waypoint and spline controller
         wp_nav->wp_and_spline_init();
-        build_path(!_copter.failsafe.terrain);
+        build_path(!copter.failsafe.terrain);
         climb_start();
         return true;
     }else{
@@ -25,7 +25,7 @@ bool Copter::ModeRTL::init(bool ignore_checks)
 void Copter::ModeRTL::restart_without_terrain()
 {
     // log an error
-    _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL);
+    copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL);
     if (rtl_path.terrain_used) {
         build_path(false);
         climb_start();
@@ -47,7 +47,7 @@ void Copter::ModeRTL::run(bool disarm_on_land)
             loiterathome_start();
             break;
         case RTL_LoiterAtHome:
-            if (rtl_path.land || _copter.failsafe.radio) {
+            if (rtl_path.land || copter.failsafe.radio) {
                 land_start();
             }else{
                 descent_start();
@@ -101,8 +101,8 @@ void Copter::ModeRTL::climb_start()
     // set the destination
     if (!wp_nav->set_wp_destination(rtl_path.climb_target)) {
         // this should not happen because rtl_build_path will have checked terrain data was available
-        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
-        _copter.set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE);
+        copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
+        copter.set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE);
         return;
     }
     wp_nav->set_fast_waypoint(true);
@@ -123,7 +123,7 @@ void Copter::ModeRTL::return_start()
     }
 
     // initialise yaw to point home (maybe)
-    set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(true));
+    set_auto_yaw_mode(copter.get_default_auto_yaw_mode(true));
 }
 
 // rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
@@ -139,7 +139,7 @@ void Copter::ModeRTL::climb_return_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -151,13 +151,13 @@ void Copter::ModeRTL::climb_return_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    _copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
+    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
     }else{
@@ -177,7 +177,7 @@ void Copter::ModeRTL::loiterathome_start()
     _loiter_start_time = millis();
 
     // yaw back to initial take-off heading yaw unless pilot has already overridden yaw
-    if(_copter.get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
+    if(copter.get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
         set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW);
     } else {
         set_auto_yaw_mode(AUTO_YAW_HOLD);
@@ -197,7 +197,7 @@ void Copter::ModeRTL::loiterathome_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!_copter.failsafe.radio) {
+    if (!copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -209,13 +209,13 @@ void Copter::ModeRTL::loiterathome_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    _copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
+    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
 
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control->update_z_controller();
 
     // call attitude controller
-    if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
+    if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
     }else{
@@ -225,9 +225,9 @@ void Copter::ModeRTL::loiterathome_run()
 
     // check if we've completed this stage of RTL
     if ((millis() - _loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
-        if (_copter.auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
+        if (copter.auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
             // check if heading is within 2 degrees of heading when vehicle was armed
-            if (labs(wrap_180_cd(ahrs.yaw_sensor-_copter.initial_armed_bearing)) <= 200) {
+            if (labs(wrap_180_cd(ahrs.yaw_sensor-copter.initial_armed_bearing)) <= 200) {
                 _state_complete = true;
             }
         } else {
@@ -269,12 +269,12 @@ void Copter::ModeRTL::descent_run()
     }
 
     // process pilot's input
-    if (!_copter.failsafe.radio) {
-        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && _copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
+    if (!copter.failsafe.radio) {
+        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
             Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
             // exit land if throttle is high
-            if (!_copter.set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
-                _copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
+            if (!copter.set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
+                copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
             }
         }
 
@@ -308,7 +308,7 @@ void Copter::ModeRTL::descent_run()
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
 
     // check if we've reached within 20cm of final altitude
-    _state_complete = labs(rtl_path.descent_target.alt - _copter.current_loc.alt) < 20;
+    _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;
 }
 
 // rtl_loiterathome_start - initialise controllers to loiter over home
@@ -355,7 +355,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
 
         // disarm when the landing detector says we've landed
         if (ap.land_complete && disarm_on_land) {
-            _copter.init_disarm_motors();
+            copter.init_disarm_motors();
         }
 
         // check if we've completed this stage of RTL
@@ -366,8 +366,8 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
-    _copter.land_run_horizontal_control();
-    _copter.land_run_vertical_control();
+    copter.land_run_horizontal_control();
+    copter.land_run_vertical_control();
 
     // check if we've completed this stage of RTL
     _state_complete = ap.land_complete;
@@ -402,24 +402,24 @@ void Copter::ModeRTL::compute_return_target(bool terrain_following_allowed)
 {
     // set return target to nearest rally point or home position (Note: alt is absolute)
 #if AC_RALLY == ENABLED
-    rtl_path.return_target = _copter.rally.calc_best_rally_or_home_location(_copter.current_loc, ahrs.get_home().alt);
+    rtl_path.return_target = copter.rally.calc_best_rally_or_home_location(copter.current_loc, ahrs.get_home().alt);
 #else
     rtl_path.return_target = ahrs.get_home();
 #endif
 
     // curr_alt is current altitude above home or above terrain depending upon use_terrain
-    int32_t curr_alt = _copter.current_loc.alt;
+    int32_t curr_alt = copter.current_loc.alt;
 
     // decide if we should use terrain altitudes
-    rtl_path.terrain_used = _copter.terrain_use() && terrain_following_allowed;
+    rtl_path.terrain_used = copter.terrain_use() && terrain_following_allowed;
     if (rtl_path.terrain_used) {
         // attempt to retrieve terrain alt for current location, stopping point and origin
         int32_t origin_terr_alt, return_target_terr_alt;
         if (!rtl_path.origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) ||
             !rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) ||
-            !_copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) {
+            !copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) {
             rtl_path.terrain_used = false;
-            _copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
+            copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
         }
     }
 
@@ -457,10 +457,10 @@ void Copter::ModeRTL::compute_return_target(bool terrain_following_allowed)
     //       if terrain altitudes are being used, the code below which reduces the return_target's altitude can lead to
     //       the vehicle not climbing at all as RTL begins.  This can be overly conservative and it might be better
     //       to apply the fence alt limit independently on the origin_point and return_target
-    if ((_copter.fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
+    if ((copter.fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
         // get return target as alt-above-home so it can be compared to fence's alt
         if (rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt)) {
-            float fence_alt = _copter.fence.get_safe_alt_max()*100.0f;
+            float fence_alt = copter.fence.get_safe_alt_max()*100.0f;
             if (target_alt > fence_alt) {
                 // reduce target alt to the fence alt
                 rtl_path.return_target.alt -= (target_alt - fence_alt);
@@ -472,3 +472,13 @@ void Copter::ModeRTL::compute_return_target(bool terrain_following_allowed)
     // ensure we do not descend
     rtl_path.return_target.alt = MAX(rtl_path.return_target.alt, curr_alt);
 }
+
+uint32_t Copter::ModeRTL::wp_distance() const
+{
+    return wp_nav->get_wp_distance_to_destination();
+}
+
+int32_t Copter::ModeRTL::wp_bearing() const
+{
+    return wp_nav->get_wp_bearing_to_destination();
+}
diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp
index f3397363eb..582d7ec3c0 100644
--- a/ArduCopter/mode_smart_rtl.cpp
+++ b/ArduCopter/mode_smart_rtl.cpp
@@ -137,3 +137,13 @@ void Copter::ModeSmartRTL::save_position()
 
     copter.g2.smart_rtl.update(copter.position_ok(), should_save_position);
 }
+
+uint32_t Copter::ModeSmartRTL::wp_distance() const
+{
+    return wp_nav->get_wp_distance_to_destination();
+}
+
+int32_t Copter::ModeSmartRTL::wp_bearing() const
+{
+    return wp_nav->get_wp_bearing_to_destination();
+}
diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp
index bd031bbf5b..90b0a250df 100644
--- a/ArduCopter/mode_sport.cpp
+++ b/ArduCopter/mode_sport.cpp
@@ -51,7 +51,7 @@ void Copter::ModeSport::run()
     int32_t pitch_angle = wrap_180_cd(att_target.y);
     target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
 
-    AP_Vehicle::MultiCopter &aparm = _copter.aparm;
+    AP_Vehicle::MultiCopter &aparm = copter.aparm;
     if (roll_angle > aparm.angle_max){
         target_roll_rate -=  g.acro_rp_p*(roll_angle-aparm.angle_max);
     }else if (roll_angle < -aparm.angle_max) {
@@ -150,7 +150,7 @@ void Copter::ModeSport::run()
         attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
 
         // adjust climb rate using rangefinder
-        if (_copter.rangefinder_alt_ok()) {
+        if (copter.rangefinder_alt_ok()) {
             // if rangefinder is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
         }
diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp
index ed91b4b606..ca1f6447b0 100644
--- a/ArduCopter/mode_stabilize.cpp
+++ b/ArduCopter/mode_stabilize.cpp
@@ -8,7 +8,7 @@
 bool Copter::ModeStabilize::init(bool ignore_checks)
 {
     // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
-    if (motors->armed() && ap.land_complete && !_copter.flightmode->has_manual_throttle() &&
+    if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() &&
             (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
         return false;
     }
@@ -40,7 +40,7 @@ void Copter::ModeStabilize::run()
     // apply SIMPLE mode transform to pilot inputs
     update_simple_mode();
 
-    AP_Vehicle::MultiCopter &aparm = _copter.aparm;
+    AP_Vehicle::MultiCopter &aparm = copter.aparm;
 
     // convert pilot input to lean angles
     // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp
index 03afc1f731..7a70c3aa90 100644
--- a/ArduCopter/mode_stabilize_heli.cpp
+++ b/ArduCopter/mode_stabilize_heli.cpp
@@ -13,7 +13,7 @@ bool Copter::ModeStabilize_Heli::init(bool ignore_checks)
     pos_control->set_alt_target(0);
 
     // set stab collective true to use stabilize scaled collective pitch range
-    _copter.input_manager.set_use_stab_col(true);
+    copter.input_manager.set_use_stab_col(true);
 
     return true;
 }
@@ -33,16 +33,16 @@ void Copter::ModeStabilize_Heli::run()
     // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
     
     if(!motors->armed()) {
-        _copter.heli_flags.init_targets_on_arming = true;
+        copter.heli_flags.init_targets_on_arming = true;
         attitude_control->set_yaw_target_to_current_heading();
         attitude_control->reset_rate_controller_I_terms();
     }
     
-    if(motors->armed() && _copter.heli_flags.init_targets_on_arming) {
+    if(motors->armed() && copter.heli_flags.init_targets_on_arming) {
         attitude_control->set_yaw_target_to_current_heading();
         attitude_control->reset_rate_controller_I_terms();
         if (motors->get_interlock()) {
-            _copter.heli_flags.init_targets_on_arming=false;
+            copter.heli_flags.init_targets_on_arming=false;
         }
     }
 
@@ -56,13 +56,13 @@ void Copter::ModeStabilize_Heli::run()
 
     // convert pilot input to lean angles
     // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
+    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
 
     // get pilot's desired yaw rate
     target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
 
     // get pilot's desired throttle
-    pilot_throttle_scaled = _copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
+    pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
 
     // call attitude controller
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp
index 0abe5ec16b..9d13845e0d 100644
--- a/ArduCopter/mode_throw.cpp
+++ b/ArduCopter/mode_throw.cpp
@@ -71,7 +71,7 @@ void Copter::ModeThrow::run()
         pos_control->set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f));
 
         // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
-        _copter.set_auto_armed(true);
+        copter.set_auto_armed(true);
 
     } else if (stage == Throw_HgtStabilise && throw_height_good()) {
         gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
@@ -81,7 +81,7 @@ void Copter::ModeThrow::run()
         wp_nav->init_loiter_target();
 
         // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
-        _copter.set_auto_armed(true);
+        copter.set_auto_armed(true);
     } else if (stage == Throw_PosHold && throw_position_good()) {
         if (!nextmode_attempted) {
             switch (g2.throw_nextmode) {
@@ -186,13 +186,13 @@ void Copter::ModeThrow::run()
         last_log_ms = now;
         float velocity = inertial_nav.get_velocity().length();
         float velocity_z = inertial_nav.get_velocity().z;
-        float accel = _copter.ins.get_accel().length();
+        float accel = copter.ins.get_accel().length();
         float ef_accel_z = ahrs.get_accel_ef().z;
         bool throw_detect = (stage > Throw_Detecting) || throw_detected();
         bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good();
         bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good();
         bool pos_ok = (stage > Throw_PosHold) || throw_position_good();
-        _copter.Log_Write_Throw(stage,
+        copter.Log_Write_Throw(stage,
                                 velocity,
                                 velocity_z,
                                 accel,
@@ -227,7 +227,7 @@ bool Copter::ModeThrow::throw_detected()
     bool free_falling = ahrs.get_accel_ef().z > -0.25 * GRAVITY_MSS;
 
     // Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
-    bool no_throw_action = _copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;
+    bool no_throw_action = copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;
 
     // High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release
     bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action;