Plane: calculate next_turn_angle to prevent early WP completion

this should help for ball drop and camera trigger with straight line
runs

ArduPlane/ArduPlane.pde

@@ -533,13 +533,17 @@ static struct {
 
     // initial pitch. Used to detect if nose is rising in a tail dragger
     int16_t initial_pitch_cd;
+
+    // turn angle for next leg of mission
+    float next_turn_angle;
 } auto_state = {
     takeoff_complete : true,
     land_complete : false,
     takeoff_altitude_cm : 0,
     takeoff_pitch_cd : 0,
     highest_airspeed : 0,
-    initial_pitch_cd : 0
+    initial_pitch_cd : 0,
+    next_turn_angle  : 90.0f
 };
 
 // true if we are in an auto-throttle mode, which means

ArduPlane/commands_logic.pde

@@ -413,7 +413,7 @@ static bool verify_nav_wp()
         return false;
     }
     
-    if (wp_distance <= nav_controller->turn_distance(g.waypoint_radius)) {
+    if (wp_distance <= nav_controller->turn_distance(g.waypoint_radius, auto_state.next_turn_angle)) {
         gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
                           (unsigned)mission.get_current_nav_cmd().index,
                           (unsigned)get_distance(current_loc, next_WP_loc));

ArduPlane/navigation.pde

@@ -225,6 +225,9 @@ static void update_fbwb_speed_height(void)
     calc_nav_pitch();
 }
 
+/*
+  setup for a gradual glide slope to the next waypoint, if appropriate
+ */
 static void setup_glide_slope(void)
 {
     // establish the distance we are travelling to the next waypoint,
@@ -266,8 +269,30 @@ static void setup_glide_slope(void)
         offset_altitude_cm = 0;        
         break;
     }
+
+
+    // calculate turn angle for next leg
+    setup_turn_angle();
 }
 
+/*
+  calculate the turn angle for the next leg of the mission
+ */
+static void setup_turn_angle(void)
+{
+    int32_t next_ground_course_cd = mission.get_next_ground_course_cd(-1);
+    if (next_ground_course_cd == -1) {
+        // the mission library can't determine a turn angle, assume 90 degrees
+        auto_state.next_turn_angle = 90.0f;
+    } else {
+        // get the heading of the current leg
+        int32_t ground_course_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
+
+        // work out the angle we need to turn through
+        auto_state.next_turn_angle = wrap_180_cd(next_ground_course_cd - ground_course_cd) * 0.01f;
+    }
+}    
+
 /*
   return relative altitude in meters (relative to home)
  */