From dfa18eaef17cf5afd392d62fb10249202598d6b1 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 10 Apr 2012 02:49:19 -0500 Subject: [PATCH] Deal with the case where path start and endpoints are the same to avoid NAN. --- flight/Libraries/paths.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/flight/Libraries/paths.c b/flight/Libraries/paths.c index 2fa035867..9162a0cbb 100644 --- a/flight/Libraries/paths.c +++ b/flight/Libraries/paths.c @@ -52,6 +52,14 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st dot = path_north * diff_north + path_east * diff_east; dist_path2 = path_north * path_north + path_east * path_east; + if(dist_path2 < 1e-3) { + status->fractional_progress = 1; + status->error = 0; + status->correction_direction[0] = status->correction_direction[1] = 0; + status->path_direction[0] = status->path_direction[1] = 0; + return; + } + // Compute the normal to the path normal[0] = -path_east / sqrtf(dist_path2); normal[1] = path_north / sqrtf(dist_path2);