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

AC_WPNav: spline sets origin vel to zero when no prev segment

Browse Source 
Issue caught by Michael Oborne
This commit is contained in:
Randy Mackay 2014-03-30 15:16:03 +09:00
parent 7dfde39e19
commit d27ca53a9d
1 changed files with 17 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -543,14 +543,13 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
// spline-fast - vehicle is moving, previous segment is splined, vehicle will fly through waypoint but previous segment should have it flying in the correct direction (i.e. exactly parallel to position difference vector from previous segment's origin to this segment's destination) // spline-fast - vehicle is moving, previous segment is splined, vehicle will fly through waypoint but previous segment should have it flying in the correct direction (i.e. exactly parallel to position difference vector from previous segment's origin to this segment's destination)
// calculate spline velocity at origin // calculate spline velocity at origin
if (stopped_at_start) { if (stopped_at_start || !prev_segment_exists) {
// if vehicle is stopped at the origin, set origin velocity to 0.1 * distance vector from origin to destination // if vehicle is stopped at the origin, set origin velocity to 0.1 * distance vector from origin to destination
_spline_origin_vel = (destination - origin) * 0.1f; _spline_origin_vel = (destination - origin) * 0.1f;
_spline_time = 0.0f; _spline_time = 0.0f;
_spline_vel_scaler = 0.0f; _spline_vel_scaler = 0.0f;
}else{ }else{
// look at previous segment to determine velocity at origin // look at previous segment to determine velocity at origin
if (prev_segment_exists) {
if (_flags.segment_type == SEGMENT_STRAIGHT) { if (_flags.segment_type == SEGMENT_STRAIGHT) {
// previous segment is straight, vehicle is moving so vehicle should fly straight through the origin // previous segment is straight, vehicle is moving so vehicle should fly straight through the origin
// before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination // before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination
@ -571,7 +570,6 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
_spline_vel_scaler = 0.0f; _spline_vel_scaler = 0.0f;
} }
} }
}
// calculate spline velocity at destination // calculate spline velocity at destination
switch (seg_end_type) { switch (seg_end_type) {