From 55e7f9b1d6c058dd36719f5f7c0c6f8cc3cb3518 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 5 Jun 2014 09:35:09 +1000
Subject: [PATCH] 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      |  6 +++++-
 ArduPlane/commands_logic.pde |  2 +-
 ArduPlane/navigation.pde     | 25 +++++++++++++++++++++++++
 3 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index b1e431179c..05cf9c28d1 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/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
diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index b5ac2316a3..8e6dadab8f 100644
--- a/ArduPlane/commands_logic.pde
+++ b/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));
diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde
index 4bac5c8483..f8b80f1861 100644
--- a/ArduPlane/navigation.pde
+++ b/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)
  */