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

AC_WPNav: reduce spline to straight twitch

Browse Source
This commit is contained in:
Randy Mackay 2017-04-25 21:18:56 +09:00
parent f8d6b5fc9c
commit e96332d346
1 changed files with 7 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -581,7 +581,10 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
// calculate how far along the track we are
track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;
// calculate the point closest to the vehicle on the segment from origin to destination
Vector3f track_covered_pos = _pos_delta_unit * track_covered;
// calculate the distance vector from the vehicle to the closest point on the segment from origin to destination
track_error = curr_delta - track_covered_pos;
// calculate the horizontal error
@ -590,22 +593,11 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// get position control leash lengths
float leash_xy = _pos_control.get_leash_xy();
float leash_z;
if (track_error.z >= 0) {
leash_z = _pos_control.get_leash_up_z();
}else{
leash_z = _pos_control.get_leash_down_z();
}
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy);
if (track_leash_slack < 0) {
track_desired_max = track_covered;
}else{
track_desired_max = track_covered + track_leash_slack;
}
float track_leash_length_abs = fabsf(_track_leash_length);
float track_error_max_abs = MAX(fabsf(track_error_z), fabsf(track_error_xy));
track_leash_slack = (track_leash_length_abs > track_error_max_abs) ? safe_sqrt(sq(_track_leash_length) - sq(track_error_max_abs)) : 0;
track_desired_max = track_covered + track_leash_slack;
// check if target is already beyond the leash
if (_track_desired > track_desired_max) {