Copter: remove original_wp_bearing

To-Do: extend wpnav yaw control to only repoint nose towards waypoint if
it is more than 5m or 10m away

ArduCopter/ArduCopter.pde

@@ -514,8 +514,6 @@ static float scaleLongDown = 1;
 ////////////////////////////////////////////////////////////////////////////////
 // This is the angle from the copter to the next waypoint in centi-degrees
 static int32_t wp_bearing;
-// The original bearing to the next waypoint.  used to point the nose of the copter at the next waypoint
-static int32_t original_wp_bearing;
 // The location of home in relation to the copter in centi-degrees
 static int32_t home_bearing;
 // distance between plane and home in cm

ArduCopter/commands_logic.pde

@@ -313,10 +313,6 @@ static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
         // set spline navigation target
     	auto_spline_start(local_pos, stopped_at_start, seg_end_type, next_spline_destination);
     }
-
-    // initialise original_wp_bearing which is used to check if we have missed the waypoint
-    wp_bearing = wp_nav.get_wp_bearing_to_destination();
-    original_wp_bearing = wp_bearing;
 }
 
 // do_land - initiate landing procedure
@@ -333,10 +329,6 @@ static void do_land(const AP_Mission::Mission_Command& cmd)
         Vector3f pos = pv_location_to_vector(cmd.content.location);
         pos.z = min(current_loc.alt, RTL_ALT_MAX);
         auto_wp_start(pos);
-
-        // initialise original_wp_bearing which is used to check if we have missed the waypoint
-        wp_bearing = wp_nav.get_wp_bearing_to_destination();
-        original_wp_bearing = wp_bearing;
     }else{
         // set landing state
         land_state = LAND_STATE_DESCENDING;

ArduCopter/control_guided.pde

@@ -38,15 +38,6 @@ static void guided_set_destination(const Vector3f& destination)
         if (!guided_pilot_yaw_override_yaw) {
             // get default yaw mode
             set_auto_yaw_mode(get_default_auto_yaw_mode(false));
-            // point nose at next waypoint if it is more than 10m away
-            if (auto_yaw_mode == AUTO_YAW_LOOK_AT_NEXT_WP) {
-                // get distance to new location
-                wp_distance = wp_nav.get_wp_distance_to_destination();
-                // set original_wp_bearing to point at next waypoint
-                if (wp_distance >= GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM) {
-                    original_wp_bearing = wp_bearing;
-                }
-            }
         }
     }
 }

ArduCopter/control_rtl.pde

@@ -95,10 +95,6 @@ static void rtl_return_start()
     rtl_state = ReturnHome;
     rtl_state_complete = false;
 
-    // initialise original_wp_bearing which is used to point the nose home
-    wp_bearing = wp_nav.get_wp_bearing_to_destination();
-    original_wp_bearing = wp_bearing;
-
     // set target to above home
     Vector3f destination = Vector3f(0,0,get_RTL_alt());
     wp_nav.set_wp_destination(destination);