From bdc1c41e62e42a10f32e8d63468e2d6e4a5daa7a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 16 Aug 2012 15:38:25 -0700 Subject: [PATCH] ACM: made target bearing the lead filtered location Had some major nav trouble from crosstrack if I used the laggy position. --- ArduCopter/navigation.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 01ed8e0621..cca3781b8e 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -12,7 +12,7 @@ static void navigate() // target_bearing is where we should be heading // -------------------------------------------- - target_bearing = get_bearing_cd(¤t_loc, &next_WP); + target_bearing = get_bearing_cd(&filtered_loc, &next_WP); home_to_copter_bearing = get_bearing_cd(&home, ¤t_loc); } @@ -185,7 +185,7 @@ static void calc_nav_rate(int16_t max_speed) int16_t cross_speed = crosstrack_error * -g.crosstrack_gain; // scale down crosstrack_error in cm // XXX replace above with crosstrack gain. - cross_speed = constrain(cross_speed, -200, 200); + cross_speed = constrain(cross_speed, -150, 150); // rotate by 90 to deal with trig functions temp = (9000l - target_bearing) * RADX100;