aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: fix spline overshoot

Browse Source
This commit is contained in:
Randy Mackay 2015-05-13 22:36:27 +09:00
parent c86a396ca0
commit 7056484ef0
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -802,7 +802,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
} }
// code below ensures we don't get too much overshoot when the next segment is short // code below ensures we don't get too much overshoot when the next segment is short
float vel_len = (_spline_origin_vel + _spline_destination_vel).length(); float vel_len = _spline_origin_vel.length() + _spline_destination_vel.length();
float pos_len = (destination - origin).length() * 4.0f; float pos_len = (destination - origin).length() * 4.0f;
if (vel_len > pos_len) { if (vel_len > pos_len) {
// if total start+stop velocity is more than twice position difference // if total start+stop velocity is more than twice position difference
@ -854,12 +854,12 @@ void AC_WPNav::set_spline_dest_and_vel(const Vector3f& dest_pos, const Vector3f&
_flags.fast_waypoint = _spline_destination_vel.length() > 0.0f; _flags.fast_waypoint = _spline_destination_vel.length() > 0.0f;
float vel_len = (_spline_origin_vel + _spline_destination_vel).length(); float vel_len = _spline_origin_vel.length() + _spline_destination_vel.length();
float pos_len = (_destination - _origin).length() * 4.0f; float pos_len = (_destination - _origin).length() * 4.0f;
if (vel_len > pos_len) { if (vel_len > pos_len) {
// if total start+stop velocity is more than twice position difference // if total start+stop velocity is more than twice position difference
// use a scaled down start and stop velocityscale the start and stop velocities down // use a scaled down start and stop velocity
float vel_scaling = pos_len / vel_len; float vel_scaling = pos_len / vel_len;
// update spline calculator // update spline calculator
update_spline_solution(_origin, _destination, _spline_origin_vel * vel_scaling, _spline_destination_vel * vel_scaling); update_spline_solution(_origin, _destination, _spline_origin_vel * vel_scaling, _spline_destination_vel * vel_scaling);