Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: protect against divide by zero

Browse Source 
This could occur if the spline origin and destination were the same location
In these cases we mark the vehicle as having reached the destination avoid all calculations
This commit is contained in:
Leonard Hall 2017-11-23 21:14:52 +09:00 committed by Randy Mackay
parent 8845b6d5b5
commit e49d29f8cd
1 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -1093,7 +1093,15 @@ bool AC_WPNav::advance_spline_target_along_track(float dt)
// update target position and velocity from spline calculator // update target position and velocity from spline calculator
calc_spline_pos_vel(_spline_time, target_pos, target_vel); calc_spline_pos_vel(_spline_time, target_pos, target_vel);
_pos_delta_unit = target_vel/target_vel.length(); // if target velocity is zero the origin and destination must be the same
// so flag reached destination (and protect against divide by zero)
float target_vel_length = target_vel.length();
if (is_zero(target_vel_length)) {
_flags.reached_destination = true;
return true;
}
_pos_delta_unit = target_vel / target_vel_length;
calculate_wp_leash_length(); calculate_wp_leash_length();
// get current location // get current location
@ -1149,10 +1157,7 @@ bool AC_WPNav::advance_spline_target_along_track(float dt)
_spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit); _spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit);
// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator // scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
float target_vel_length = target_vel.length(); _spline_time_scale = _spline_vel_scaler / target_vel_length;
if (!is_zero(target_vel_length)) {
_spline_time_scale = _spline_vel_scaler/target_vel_length;
}
// update target position // update target position
target_pos.z += terr_offset; target_pos.z += terr_offset;