AC_WPNav: slow target point's speed near destination

This commit is contained in:
Randy Mackay 2014-04-23 22:00:32 +09:00
parent a5602c5f14
commit 46fba47c8e
2 changed files with 57 additions and 9 deletions

View File

@ -84,9 +84,9 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosContro
_track_accel(0.0),
_track_speed(0.0),
_track_leash_length(0.0),
_slow_down_dist(0.0),
_spline_time(0.0),
_spline_vel_scaler(0.0),
_spline_slow_down_dist(0.0),
_yaw(0.0)
{
AP_Param::setup_object_defaults(this, var_info);
@ -335,6 +335,11 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
_track_desired = 0; // target is at beginning of track
_flags.reached_destination = false;
_flags.fast_waypoint = false; // default waypoint back to slow
// calculate slow down distance (the distance from the destination when the target point should begin to slow down)
_flags.slowing_down = false; // target is not slowing down yet
calc_slow_down_distance(_wp_speed_cms, _wp_accel_cms);
_flags.segment_type = SEGMENT_STRAIGHT;
// initialise the limited speed to current speed along the track
@ -422,6 +427,18 @@ void AC_WPNav::advance_wp_target_along_track(float dt)
// do not allow speed to be below zero or over top speed
_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms, 0.0f, _track_speed);
// check if we should begin slowing down
if (!_flags.fast_waypoint) {
float dist_to_dest = _track_length - _track_desired;
if (!_flags.slowing_down && dist_to_dest <= _slow_down_dist) {
_flags.slowing_down = true;
}
// if target is slowing down, limit the speed
if (_flags.slowing_down) {
_limited_speed_xy_cms = min(_limited_speed_xy_cms, get_slow_down_speed(dist_to_dest, _track_accel));
}
}
// if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity
if (fabsf(speed_along_track) < linear_velocity) {
_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity);
@ -660,10 +677,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
calculate_wp_leash_length();
// calculate slow down distance
// To-Do: this should be used for straight segments as well
// To-Do: should we use a combination of horizontal and vertical speeds?
// To-Do: update this automatically when speed or acceleration is changed
_spline_slow_down_dist = _wp_speed_cms * _wp_speed_cms / (2.0f*_wp_accel_cms);
calc_slow_down_distance(_wp_speed_cms, _wp_accel_cms);
// initialise intermediate point to the origin
_pos_control.set_pos_target(origin);
@ -724,7 +738,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt)
float spline_dist_to_wp = (_destination - target_pos).length();
// if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
if (!_flags.fast_waypoint && spline_dist_to_wp < _spline_slow_down_dist) {
if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) {
_spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cms);
}else if(_spline_vel_scaler < _wp_speed_cms) {
// increase velocity using acceleration
@ -789,3 +803,30 @@ float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destinati
}
return bearing;
}
/// calc_slow_down_distance - calculates distance before waypoint that target point should begin to slow-down assuming it is travelling at full speed
void AC_WPNav::calc_slow_down_distance(float speed_cms, float accel_cmss)
{
// To-Do: should we use a combination of horizontal and vertical speeds?
// To-Do: update this automatically when speed or acceleration is changed
_slow_down_dist = speed_cms * speed_cms / (4.0f*accel_cmss);
}
/// get_slow_down_speed - returns target speed of target point based on distance from the destination (in cm)
float AC_WPNav::get_slow_down_speed(float dist_from_dest_cm, float accel_cmss)
{
// return immediately if distance is zero (or less)
if (dist_from_dest_cm <= 0) {
return WPNAV_WP_TRACK_SPEED_MIN;
}
// calculate desired speed near destination
float target_speed = safe_sqrt(dist_from_dest_cm * 4.0f * accel_cmss);
// ensure desired speed never becomes too low
if (target_speed < WPNAV_WP_TRACK_SPEED_MIN) {
return WPNAV_WP_TRACK_SPEED_MIN;
} else {
return target_speed;
}
}

View File

@ -18,7 +18,8 @@
#define WPNAV_LOITER_ACCEL_MIN 25.0f // minimum acceleration in loiter mode
#define WPNAV_WP_SPEED 500.0f // default horizontal speed betwen waypoints in cm/s
#define WPNAV_WP_SPEED_MIN 100.0f // minimum horitzontal speed between waypoints in cm/s
#define WPNAV_WP_SPEED_MIN 100.0f // minimum horizontal speed between waypoints in cm/s
#define WPNAV_WP_TRACK_SPEED_MIN 50.0f // minimum speed along track of the target point the vehicle is chasing in cm/s (used as target slows down before reaching destination)
#define WPNAV_WP_RADIUS 200.0f // default waypoint radius in cm
#define WPNAV_WP_SPEED_UP 250.0f // default maximum climb velocity
@ -200,6 +201,7 @@ protected:
struct wpnav_flags {
uint8_t reached_destination : 1; // true if we have reached the destination
uint8_t fast_waypoint : 1; // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
uint8_t slowing_down : 1; // true when target point is slowing down before reaching the destination
SegmentType segment_type : 1; // active segment is either straight or spline
} _flags;
@ -210,6 +212,12 @@ protected:
/// get_bearing_cd - return bearing in centi-degrees between two positions
float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const;
/// calc_slow_down_distance - calculates distance before waypoint that target point should begin to slow-down assuming it is traveling at full speed
void calc_slow_down_distance(float speed_cms, float accel_cmss);
/// get_slow_down_speed - returns target speed of target point based on distance from the destination (in cm)
float get_slow_down_speed(float dist_from_dest_cm, float accel_cmss);
/// spline protected functions
/// update_spline_solution - recalculates hermite_spline_solution grid
@ -254,6 +262,7 @@ protected:
float _track_accel; // acceleration along track
float _track_speed; // speed in cm/s along track
float _track_leash_length; // leash length along track
float _slow_down_dist; // vehicle should begin to slow down once it is within this distance from the destination
// spline variables
float _spline_time; // current spline time between origin and destination
@ -261,8 +270,6 @@ protected:
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
float _spline_vel_scaler; //
float _spline_slow_down_dist; // vehicle should begin to slow down once it is within this distance from the destination
// To-Do: this should be used for straight segments as well
float _yaw; // heading according to yaw
};
#endif // AC_WPNAV_H