Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: remove unused set_spline_dest_and_vel

This commit is contained in:
Randy Mackay 2015-05-13 22:39:00 +09:00
parent 7056484ef0
commit 61c851885f
2 changed files with 0 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -832,55 +832,6 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
_flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition
}
void AC_WPNav::set_spline_dest_and_vel(const Vector3f& dest_pos, const Vector3f& dest_vel)
{
// check _wp_accel_cms is reasonable to avoid divide by zero
if (_wp_accel_cms <= 0) {
_wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
}
_spline_time = 0.0f;
_origin = _inav.get_position();
_destination = dest_pos;
_spline_origin_vel = _inav.get_velocity();
_spline_destination_vel = dest_vel;
if(_spline_origin_vel.length() < 100.0f) {
_spline_origin_vel = (_destination - _origin) * 0.1f;
}
_spline_vel_scaler = _spline_origin_vel.length();
_flags.fast_waypoint = _spline_destination_vel.length() > 0.0f;
float vel_len = _spline_origin_vel.length() + _spline_destination_vel.length();
float pos_len = (_destination - _origin).length() * 4.0f;
if (vel_len > pos_len) {
// if total start+stop velocity is more than twice position difference
// use a scaled down start and stop velocity
float vel_scaling = pos_len / vel_len;
// update spline calculator
update_spline_solution(_origin, _destination, _spline_origin_vel * vel_scaling, _spline_destination_vel * vel_scaling);
}else{
// update spline calculator
update_spline_solution(_origin, _destination, _spline_origin_vel, _spline_destination_vel);
}
// initialise yaw heading to current heading
_yaw = _attitude_control.angle_ef_targets().z;
// calculate slow down distance
calc_slow_down_distance(_wp_speed_cms, _wp_accel_cms);
// initialise intermediate point to the origin
_pos_control.set_pos_target(_origin);
_flags.reached_destination = false;
_flags.segment_type = SEGMENT_SPLINE;
_flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition
}
/// update_spline - update spline controller
void AC_WPNav::update_spline()
{

3
libraries/AC_WPNav/AC_WPNav.h
View File

@ -203,9 +203,6 @@ public:
/// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE
void set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination);
// set_spline_dest_and_vel - accepts a destination position and velocity, sets origin to current position and velocity
void set_spline_dest_and_vel(const Vector3f& dest_pos, const Vector3f& dest_vel);
/// reached_spline_destination - true when we have come within RADIUS cm of the waypoint
bool reached_spline_destination() const { return _flags.reached_destination; }