From 0912bec8f570ab423c7c7646d458f6ef6e60e342 Mon Sep 17 00:00:00 2001 From: lthall Date: Sat, 31 May 2014 22:56:37 +0930 Subject: [PATCH] Spline div zero and leash limit fix --- libraries/AC_WPNav/AC_WPNav.cpp | 16 +++++++++------- libraries/AC_WPNav/AC_WPNav.h | 1 + 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 6b0103b282..e119ff105a 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -492,14 +492,16 @@ void AC_WPNav::advance_wp_target_along_track(float dt) } // advance the current target if (!reached_leash_limit) { - track_desired_temp += _limited_speed_xy_cms * dt; + _track_desired += _limited_speed_xy_cms * dt; + + if (_track_desired > track_desired_max){ + _track_desired = track_desired_max; + _limited_speed_xy_cms -= 2.0f * _track_accel * dt; + } } // do not let desired point go past the end of the segment - track_desired_temp = constrain_float(track_desired_temp, 0, _track_length); - - // set our new desired position on track - _track_desired = max(_track_desired, track_desired_temp); + _track_desired = constrain_float(_track_desired, 0, _track_length); // recalculate the desired position _pos_control.set_pos_target(_origin + _pos_delta_unit * _track_desired); @@ -807,7 +809,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt) } // scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator - float spline_time_scale = _spline_vel_scaler/target_vel.length(); + float _spline_time_scale = _spline_vel_scaler/target_vel.length(); // update target position _pos_control.set_pos_target(target_pos); @@ -816,7 +818,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt) _yaw = RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x)); // advance spline time to next step - _spline_time += spline_time_scale*dt; + _spline_time += _spline_time_scale*dt; // we will reach the next waypoint in the next step so set reached_destination flag // To-Do: is this one step too early? diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index d3580377c8..2bf61200a4 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -290,6 +290,7 @@ protected: // spline variables float _spline_time; // current spline time between origin and destination + float _spline_time_scale; // current spline time between origin and destination Vector3f _spline_origin_vel; // the target velocity vector at the origin of the spline segment Vector3f _spline_destination_vel;// the target velocity vector at the destination point of the spline segment Vector3f _hermite_spline_solution[4]; // array describing spline path between origin and destination