rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

Deal with the case where path start and endpoints are the same to avoid NAN.

Browse Source
This commit is contained in:
James Cotton 2012-04-10 02:49:19 -05:00
parent 1aec5b8deb
commit dfa18eaef1
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
flight/Libraries/paths.c
View File

@ -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; dot = path_north * diff_north + path_east * diff_east;
dist_path2 = path_north * path_north + path_east * path_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 // Compute the normal to the path
normal[0] = -path_east / sqrtf(dist_path2); normal[0] = -path_east / sqrtf(dist_path2);
normal[1] = path_north / sqrtf(dist_path2); normal[1] = path_north / sqrtf(dist_path2);