diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 32c97d734b..6a995a43e0 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -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; + } +} diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 885895309b..010bf1279a 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -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