Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AC_WPNAV - Make speed slowdown requests obey WPNAV_ACCEL #11068

Closed
wants to merge 1 commit into from
Closed

AC_WPNAV - Make speed slowdown requests obey WPNAV_ACCEL #11068

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
36 changes: 32 additions & 4 deletions libraries/AC_WPNav/AC_WPNav.cpp
View file
Open in desktop
Expand Up @@ -146,6 +146,9 @@ void AC_WPNav::wp_and_spline_init()

// initialise feed forward velocity to zero
_pos_control.set_desired_velocity_xy(0.0f, 0.0f);

// initialize desired waypoint speed for mission speed changes
_wp_desired_speed_cms = _wp_speed_cms;

// initialise position controller speed and acceleration
_pos_control.set_max_speed_xy(_wp_speed_cms);
Expand All @@ -162,11 +165,9 @@ void AC_WPNav::wp_and_spline_init()
/// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation
void AC_WPNav::set_speed_xy(float speed_cms)
{
// range check new target speed and update position controller
// range check target speed
if (speed_cms >= WPNAV_WP_SPEED_MIN) {
_pos_control.set_max_speed_xy(speed_cms);
// flag that wp leash must be recalculated
_flags.recalc_wp_leash = true;
_wp_desired_speed_cms = speed_cms;
}
}

Expand Down Expand Up @@ -522,6 +523,9 @@ bool AC_WPNav::update_wpnav()
_pos_control.set_max_accel_xy(_wp_accel_cmss);
_pos_control.set_max_accel_z(_wp_accel_z_cmss);

// wp_speed_update - update _pos_control.set_max_speed_xy if speed change has been requested
wp_speed_update(dt);

// advance the target if necessary
if (!advance_wp_target_along_track(dt)) {
// To-Do: handle inability to advance along track (probably because of missing terrain data)
Expand Down Expand Up @@ -808,6 +812,9 @@ bool AC_WPNav::update_spline()
// get dt from pos controller
float dt = _pos_control.get_dt();

// wp_speed_update - update _pos_control.set_max_speed_xy if speed change has been requested
wp_speed_update(dt);

// advance the target if necessary
if (!advance_spline_target_along_track(dt)) {
// To-Do: handle failure to advance along track (due to missing terrain data)
Expand Down Expand Up @@ -1056,3 +1063,24 @@ float AC_WPNav::get_slow_down_speed(float dist_from_dest_cm, float accel_cmss)
return target_speed;
}
}

/// wp_speed_update - calculates how to handle speed change requests
void AC_WPNav::wp_speed_update(float dt)
{
// calculate speed change for steady-state or speeding up
if (_wp_desired_speed_cms >= _wp_speed_cms) {
_wp_speed_cms = _wp_desired_speed_cms;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there any reason we don't slew the speed up and down?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I tried that and it didn't seem to make any difference. Feed the PSC a new speed and it somehow seems to handle that by itself. I tested with a Ch6 tuning knob set from 5 to 20 m/s range with both my 1.2m TriCopter and helicopter. It accelerates very smoothly and gradual when the knob is turned to max. And I don't understand why. I originally had it coded to use WPNAV_ACCEL on speedup request but I removed it after testing it because it took forever to get it up to the requested speed. Again, I don't understand why. It was only the slowdown that had a problem with sudden request for lower speed.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My theory is that the reason it accelerates smoothly without slewing it is because it is buffered by the re-calculation of the leash length. The leash stretches like a rubber band as speed increases. This provides a natural buffer as the target gets further away from the actual aircraft. The inverse is true on slow down request.

} else {
// slow down is requested so reduce speed within limit set by WPNAV_ACCEL
_wp_speed_cms -= _wp_accel_cmss * dt;
if (_wp_speed_cms < _wp_desired_speed_cms) {
_wp_speed_cms = _wp_desired_speed_cms;
}
}

//update position controller speed
_pos_control.set_max_speed_xy(_wp_speed_cms);

// flag that wp leash must be recalculated
_flags.recalc_wp_leash = true;
}
4 changes: 4 additions & 0 deletions libraries/AC_WPNav/AC_WPNav.h
View file
Open in desktop
Expand Up @@ -249,6 +249,9 @@ class AC_WPNav

/// 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);

/// wp_speed_update - calculates how to change speed when changes are requested
void wp_speed_update(float dt);

/// spline protected functions

Expand Down Expand Up @@ -291,6 +294,7 @@ class AC_WPNav

// waypoint controller internal variables
uint32_t _wp_last_update; // time of last update_wpnav call
float _wp_desired_speed_cms; // desired wp speed in cm/sec
Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin
Vector3f _destination; // target destination in cm from ekf origin
Vector3f _pos_delta_unit; // each axis's percentage of the total track from origin to destination
Expand Down