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

Spline div zero and leash limit fix

Browse Source
This commit is contained in:
lthall 2014-05-31 22:56:37 +09:30 committed by Randy Mackay
parent ea086fa79c
commit 0912bec8f5
2 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -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?

1
libraries/AC_WPNav/AC_WPNav.h
View File

@ -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