From 62ed256fca5b32d38d64752ed890ddd2f336757f Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Nov 2011 10:32:58 -0800 Subject: [PATCH] made the crosstrack use the previous WP instead of the current loc to get a better line in AP. --- ArduCopter/commands.pde | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 691218493d..6ef53a72ae 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -146,7 +146,17 @@ static void set_next_WP(struct Location *wp) // copy the current WP into the OldWP slot // --------------------------------------- - prev_WP = next_WP; + if (next_WP.lat == 0 || command_nav_index <= 1){ + prev_WP = current_loc; + }else{ + if (get_distance(¤t_loc, &next_WP) < 10) + prev_WP = next_WP; + else + prev_WP = current_loc; + } + + //Serial.printf("set_next_WP #%d, ", command_nav_index); + //print_wp(&prev_WP, command_nav_index -1); // Load the next_WP slot // --------------------- @@ -165,7 +175,7 @@ static void set_next_WP(struct Location *wp) // ----------------------------------- wp_totalDistance = get_distance(¤t_loc, &next_WP); wp_distance = wp_totalDistance; - target_bearing = get_bearing(¤t_loc, &next_WP); + target_bearing = get_bearing(&prev_WP, &next_WP); // to check if we have missed the WP // ---------------------------------